diff --git a/src/turtlebot3_autorace_2020/turtlebot3_autorace_core/nodes/all_core_mode_decider b/src/turtlebot3_autorace_2020/turtlebot3_autorace_core/nodes/all_core_mode_decider index ab957fe..cad3db6 100755 --- a/src/turtlebot3_autorace_2020/turtlebot3_autorace_core/nodes/all_core_mode_decider +++ b/src/turtlebot3_autorace_2020/turtlebot3_autorace_core/nodes/all_core_mode_decider @@ -37,6 +37,7 @@ self.TrafficSign = Enum('TrafficSign', 'intersection left right construction stop parking tunnel') self.CurrentMode = Enum('CurrentMode', 'idle lane_following traffic_light intersection construction parking level_crossing tunnel') self.fnInitMode() + self.countintersection = 0 # Invoke if traffic light is detected def cbInvokedByTrafficLight(self, invoke_type_msg): rospy.loginfo("mode Invoke Traffic light detected") @@ -51,22 +52,25 @@ self.current_mode = self.CurrentMode.lane_following.value self.fnPublishMode() def fnDecideMode(self, invoked_object, msg_data): # starts only when the traffic sign / traffic light is detected & current_mode is lane_following - rospy.loginfo("mode --all_core_mode_decider-----------------------------") + rospy.loginfo("mode ------------------------------") rospy.loginfo("mode current_mode=%d",self.current_mode) rospy.loginfo("mode invoked_object=%d",invoked_object) - rospy.loginfo("mode msg_data=%d",msg_data.data) + rospy.loginfo("mode msg_data=%d",msg_data.data) # TrafficSign rospy.loginfo("mode-------------------------------") if invoked_object == self.InvokedObject.traffic_light.value: self.current_mode = self.CurrentMode.intersection.value elif invoked_object == self.InvokedObject.traffic_sign.value: if msg_data.data == self.TrafficSign.intersection.value: + self.countintersection = 0 rospy.loginfo("mode detect sign : intersection") self.current_mode = self.CurrentMode.intersection.value elif msg_data.data == self.TrafficSign.left.value: - rospy.loginfo("mode detect sign : intersection left") + rospy.loginfo("mode detect sign : intersection left %d",self.countintersection) + self.countintersection = self.countintersection - 1 self.current_mode = self.CurrentMode.intersection.value elif msg_data.data == self.TrafficSign.right.value: - rospy.loginfo("mode detect sign : inteersection right") + self.countintersection = self.countintersection + 1 + rospy.loginfo("mode detect sign : inteersection right %d",self.countintersection) self.current_mode = self.CurrentMode.intersection.value elif msg_data.data == self.TrafficSign.construction.value: rospy.loginfo("mode detect sign : construction") @@ -79,7 +83,10 @@ self.current_mode = self.CurrentMode.level_crossing.value elif msg_data.data == self.TrafficSign.tunnel.value: rospy.loginfo("mode detect sign : tunnel") - self.current_mode = self.CurrentMode.tunnel.value + self.current_mode = self.CurrentMode.tunnel.value + if self.current_mode == self.CurrentMode.intersection.value and abs(self.countintersection) > 2: + rospy.loginfo("mode jfidsjfisodafjidsiosdafjiodsaf") + self.current_mode = self.CurrentMode.construction.value else: pass self.fnPublishMode() diff --git a/src/turtlebot3_autorace_2020/turtlebot3_autorace_core/nodes/all_core_node_controller b/src/turtlebot3_autorace_2020/turtlebot3_autorace_core/nodes/all_core_node_controller index 3a5bb19..fd6ba3b 100755 --- a/src/turtlebot3_autorace_2020/turtlebot3_autorace_core/nodes/all_core_node_controller +++ b/src/turtlebot3_autorace_2020/turtlebot3_autorace_core/nodes/all_core_node_controller @@ -33,7 +33,7 @@ # subscribes : status returned self.sub_mode_control = rospy.Subscriber('/core/decided_mode', UInt8, self.cbReceiveMode, queue_size=1) self.sub_intersection_stamped = rospy.Subscriber('/detect/intersection_stamped', UInt8, self.cbIntersectionStamped, queue_size=1) - self.sub_construction_stamped = rospy.Subscriber('/detect/construction_stamped', UInt8, self.cbconstructionStamped, queue_size=1) + self.sub_construction_stamped = rospy.Subscriber('/detect/construction_stamped', UInt8, self.cbConstructionStamped, queue_size=1) self.sub_parking_stamped = rospy.Subscriber('/detect/parking_stamped', UInt8, self.cbParkingStamped, queue_size=1) self.sub_level_crossing_stamped = rospy.Subscriber('/detect/level_crossing_stamped', UInt8, self.cbLevelCrossingStamped, queue_size=1) self.sub_tunnel_stamped = rospy.Subscriber('/detect/tunnel_stamped', UInt8, self.cbTunnelStamped, queue_size=1) @@ -111,7 +111,7 @@ self.is_triggered = True - def cbconstructionStamped(self, construction_msg): + def cbConstructionStamped(self, construction_msg): rospy.loginfo("node construction Step changed from %d", self.current_step_construction) self.current_step_construction = construction_msg.data @@ -123,22 +123,6 @@ self.is_triggered = True - # Which step is in Tunnel - def cbTunnelStamped(self, tunnel_msg): - rospy.loginfo("node Tunnel Step changed from %d", self.current_step_tunnel) - - self.current_step_tunnel = tunnel_msg.data - - rospy.loginfo("node into %d", self.current_step_tunnel) - - if self.current_step_tunnel == self.StepOfTunnel.searching_tunnel_sign.value: - self.current_mode = self.CurrentMode.tunnel.value - msg_mode_return = UInt8 - msg_mode_return.data = self.current_mode - self.pub_mode_return.publish(msg_mode_return) - - self.is_triggered = True - def cbParkingStamped(self, parking_msg): rospy.loginfo("node Parking Step changed from %d", self.current_step_parking) self.current_step_parking = parking_msg.data @@ -164,6 +148,22 @@ self.is_triggered = True + # Which step is in Tunnel + def cbTunnelStamped(self, tunnel_msg): + rospy.loginfo("node Tunnel Step changed from %d", self.current_step_tunnel) + + self.current_step_tunnel = tunnel_msg.data + + rospy.loginfo("node into %d", self.current_step_tunnel) + + if self.current_step_tunnel == self.StepOfTunnel.searching_tunnel_sign.value: + self.current_mode = self.CurrentMode.tunnel.value + msg_mode_return = UInt8 + msg_mode_return.data = self.current_mode + self.pub_mode_return.publish(msg_mode_return) + + self.is_triggered = True + def fnControlNode(self): # lane_following rospy.loginfo("node all_core_node_controller current_mode=%d",self.current_mode) @@ -190,6 +190,7 @@ self.fnLaunch(self.Launcher.launch_detect_tunnel.value, False) self.fnLaunch(self.Launcher.launch_control_moving.value, False) self.fnLaunch(self.Launcher.launch_control_lane.value, True) + self.fnLaunch(self.Launcher.launch_control_tunnel.value, False) # traffic_light elif self.current_mode == self.CurrentMode.traffic_light.value: @@ -214,11 +215,12 @@ self.fnLaunch(self.Launcher.launch_detect_level_crossing.value, False) self.fnLaunch(self.Launcher.launch_detect_tunnel.value, False) self.fnLaunch(self.Launcher.launch_control_moving.value, False) + self.fnLaunch(self.Launcher.launch_control_tunnel.value, False) if self.is_go == False: self.fnLaunch(self.Launcher.launch_control_lane.value, False) else: - rospy.loginfo("node ---------------------------------------------------") + rospy.loginfo("node -----------------------------------") self.fnLaunch(self.Launcher.launch_control_lane.value, True) rospy.sleep(2) self.current_step_intersection = self.StepOfIntersection.detect_direction.value @@ -256,6 +258,7 @@ self.fnLaunch(self.Launcher.launch_detect_tunnel.value, False) self.fnLaunch(self.Launcher.launch_control_moving.value, False) self.fnLaunch(self.Launcher.launch_control_lane.value, True) + self.fnLaunch(self.Launcher.launch_control_tunnel.value, False) elif self.current_step_intersection == self.StepOfIntersection.exit.value: rospy.loginfo("node Current step : exit") @@ -277,6 +280,7 @@ self.fnLaunch(self.Launcher.launch_detect_tunnel.value, False) self.fnLaunch(self.Launcher.launch_control_moving.value, False) self.fnLaunch(self.Launcher.launch_control_lane.value, True) + self.fnLaunch(self.Launcher.launch_control_tunnel.value, False) rospy.sleep(3) self.pub_intersection_order.publish(msg_pub_intersection_order) @@ -292,14 +296,23 @@ msg_pub_construction_order.data = self.StepOfConstruction.find_obstacle.value + self.fnLaunch(self.Launcher.launch_camera_ex_calib.value, True) - self.fnLaunch(self.Launcher.launch_detect_lane.value, True) + self.fnLaunch(self.Launcher.launch_detect_traffic_light.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_intersection.value, False) self.fnLaunch(self.Launcher.launch_detect_sign_construction.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_parking.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_level_crossing.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_tunnel.value, False) + self.fnLaunch(self.Launcher.launch_detect_intersection.value, False) self.fnLaunch(self.Launcher.launch_detect_construction.value, True) - - self.fnLaunch(self.Launcher.launch_control_lane.value, True) + self.fnLaunch(self.Launcher.launch_detect_parking.value, False) + self.fnLaunch(self.Launcher.launch_detect_level_crossing.value, False) + self.fnLaunch(self.Launcher.launch_detect_tunnel.value, False) self.fnLaunch(self.Launcher.launch_control_moving.value, False) + self.fnLaunch(self.Launcher.launch_control_lane.value, True) + self.fnLaunch(self.Launcher.launch_control_tunnel.value, False) elif self.current_step_construction == self.StepOfConstruction.avoid_obstacle.value: rospy.loginfo("node Current step : avoid_obstacle") @@ -307,14 +320,23 @@ msg_pub_construction_order.data = self.StepOfConstruction.avoid_obstacle.value + self.fnLaunch(self.Launcher.launch_camera_ex_calib.value, True) - self.fnLaunch(self.Launcher.launch_detect_lane.value, True) + self.fnLaunch(self.Launcher.launch_detect_traffic_light.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_intersection.value, False) self.fnLaunch(self.Launcher.launch_detect_sign_construction.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_parking.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_level_crossing.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_tunnel.value, False) + self.fnLaunch(self.Launcher.launch_detect_intersection.value, False) self.fnLaunch(self.Launcher.launch_detect_construction.value, True) - - self.fnLaunch(self.Launcher.launch_control_lane.value, False) + self.fnLaunch(self.Launcher.launch_detect_parking.value, False) + self.fnLaunch(self.Launcher.launch_detect_level_crossing.value, False) + self.fnLaunch(self.Launcher.launch_detect_tunnel.value, False) self.fnLaunch(self.Launcher.launch_control_moving.value, True) + self.fnLaunch(self.Launcher.launch_control_lane.value, False) + self.fnLaunch(self.Launcher.launch_control_tunnel.value, False) elif self.current_step_construction == self.StepOfConstruction.exit.value: rospy.loginfo("node Current step : exit") @@ -322,14 +344,21 @@ msg_pub_construction_order.data = self.StepOfConstruction.exit.value self.fnLaunch(self.Launcher.launch_camera_ex_calib.value, True) - - self.fnLaunch(self.Launcher.launch_detect_lane.value, True) + self.fnLaunch(self.Launcher.launch_detect_traffic_light.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_intersection.value, False) self.fnLaunch(self.Launcher.launch_detect_sign_construction.value, True) + self.fnLaunch(self.Launcher.launch_detect_sign_parking.value, True) + self.fnLaunch(self.Launcher.launch_detect_sign_level_crossing.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_tunnel.value, False) + self.fnLaunch(self.Launcher.launch_detect_intersection.value, False) self.fnLaunch(self.Launcher.launch_detect_construction.value, False) - - self.fnLaunch(self.Launcher.launch_control_lane.value, True) + self.fnLaunch(self.Launcher.launch_detect_parking.value, False) + self.fnLaunch(self.Launcher.launch_detect_level_crossing.value, False) + self.fnLaunch(self.Launcher.launch_detect_tunnel.value, False) self.fnLaunch(self.Launcher.launch_control_moving.value, False) + self.fnLaunch(self.Launcher.launch_control_lane.value, True) + self.fnLaunch(self.Launcher.launch_control_tunnel.value, False) rospy.sleep(2) self.pub_construction_order.publish(msg_pub_construction_order) @@ -346,13 +375,21 @@ msg_pub_parking_order.data = self.StepOfParking.parking.value self.fnLaunch(self.Launcher.launch_camera_ex_calib.value, True) - self.fnLaunch(self.Launcher.launch_detect_lane.value, True) + self.fnLaunch(self.Launcher.launch_detect_traffic_light.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_intersection.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_construction.value, False) self.fnLaunch(self.Launcher.launch_detect_sign_parking.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_level_crossing.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_tunnel.value, False) + self.fnLaunch(self.Launcher.launch_detect_intersection.value, False) + self.fnLaunch(self.Launcher.launch_detect_construction.value, False) self.fnLaunch(self.Launcher.launch_detect_parking.value, True) - - self.fnLaunch(self.Launcher.launch_control_lane.value, False) + self.fnLaunch(self.Launcher.launch_detect_level_crossing.value, False) + self.fnLaunch(self.Launcher.launch_detect_tunnel.value, False) self.fnLaunch(self.Launcher.launch_control_moving.value, True) + self.fnLaunch(self.Launcher.launch_control_lane.value, False) + self.fnLaunch(self.Launcher.launch_control_tunnel.value, False) elif self.current_step_parking == self.StepOfParking.exit.value: rospy.loginfo("node Current step : finish parking") @@ -360,13 +397,21 @@ msg_pub_parking_order.data = self.StepOfParking.exit.value self.fnLaunch(self.Launcher.launch_camera_ex_calib.value, True) - - self.fnLaunch(self.Launcher.launch_detect_sign_parking.value, True) self.fnLaunch(self.Launcher.launch_detect_lane.value, True) + self.fnLaunch(self.Launcher.launch_detect_traffic_light.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_intersection.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_construction.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_parking.value, True) + self.fnLaunch(self.Launcher.launch_detect_sign_level_crossing.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_tunnel.value, False) + self.fnLaunch(self.Launcher.launch_detect_intersection.value, False) + self.fnLaunch(self.Launcher.launch_detect_construction.value, False) self.fnLaunch(self.Launcher.launch_detect_parking.value, False) - - self.fnLaunch(self.Launcher.launch_control_lane.value, True) + self.fnLaunch(self.Launcher.launch_detect_level_crossing.value, False) + self.fnLaunch(self.Launcher.launch_detect_tunnel.value, False) self.fnLaunch(self.Launcher.launch_control_moving.value, False) + self.fnLaunch(self.Launcher.launch_control_lane.value, True) + self.fnLaunch(self.Launcher.launch_control_tunnel.value, False) rospy.sleep(2) self.pub_parking_order.publish(msg_pub_parking_order) @@ -381,24 +426,44 @@ rospy.loginfo("node Go to next step : exit") msg_pub_level_crossing_order.data = self.StepOfLevelCrossing.pass_level.value - self.fnLaunch(self.Launcher.launch_detect_sign_level_crossing.value, False) - self.fnLaunch(self.Launcher.launch_detect_level_crossing.value, True) + self.fnLaunch(self.Launcher.launch_camera_ex_calib.value, True) self.fnLaunch(self.Launcher.launch_detect_lane.value, True) - - self.fnLaunch(self.Launcher.launch_control_lane.value, True) + self.fnLaunch(self.Launcher.launch_detect_traffic_light.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_intersection.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_construction.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_parking.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_level_crossing.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_tunnel.value, False) + self.fnLaunch(self.Launcher.launch_detect_intersection.value, False) + self.fnLaunch(self.Launcher.launch_detect_construction.value, False) + self.fnLaunch(self.Launcher.launch_detect_parking.value, False) + self.fnLaunch(self.Launcher.launch_detect_level_crossing.value, True) + self.fnLaunch(self.Launcher.launch_detect_tunnel.value, False) self.fnLaunch(self.Launcher.launch_control_moving.value, True) + self.fnLaunch(self.Launcher.launch_control_lane.value, True) + self.fnLaunch(self.Launcher.launch_control_tunnel.value, False) elif self.current_step_level_crossing == self.StepOfLevelCrossing.exit.value: rospy.loginfo("node Current step : exit") msg_pub_level_crossing_order.data = self.StepOfLevelCrossing.exit.value - self.fnLaunch(self.Launcher.launch_detect_sign_level_crossing.value, True) + self.fnLaunch(self.Launcher.launch_camera_ex_calib.value, True) self.fnLaunch(self.Launcher.launch_detect_lane.value, True) + self.fnLaunch(self.Launcher.launch_detect_traffic_light.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_intersection.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_construction.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_parking.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_level_crossing.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_tunnel.value, True) + self.fnLaunch(self.Launcher.launch_detect_intersection.value, False) + self.fnLaunch(self.Launcher.launch_detect_construction.value, False) + self.fnLaunch(self.Launcher.launch_detect_parking.value, False) self.fnLaunch(self.Launcher.launch_detect_level_crossing.value, False) - - self.fnLaunch(self.Launcher.launch_control_lane.value, True) + self.fnLaunch(self.Launcher.launch_detect_tunnel.value, False) self.fnLaunch(self.Launcher.launch_control_moving.value, False) + self.fnLaunch(self.Launcher.launch_control_lane.value, True) + self.fnLaunch(self.Launcher.launch_control_tunnel.value, False) self.pub_level_crossing_order.publish(msg_pub_level_crossing_order) rospy.sleep(2) @@ -415,14 +480,21 @@ msg_pub_tunnel_order.data = self.StepOfTunnel.go_in_to_tunnel.value self.fnLaunch(self.Launcher.launch_camera_ex_calib.value, False) - self.fnLaunch(self.Launcher.launch_detect_lane.value, False) - self.fnLaunch(self.Launcher.launch_detect_tunnel.value, True) + self.fnLaunch(self.Launcher.launch_detect_traffic_light.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_intersection.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_construction.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_parking.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_level_crossing.value, False) self.fnLaunch(self.Launcher.launch_detect_sign_tunnel.value, False) - + self.fnLaunch(self.Launcher.launch_detect_intersection.value, False) + self.fnLaunch(self.Launcher.launch_detect_construction.value, False) + self.fnLaunch(self.Launcher.launch_detect_parking.value, False) + self.fnLaunch(self.Launcher.launch_detect_level_crossing.value, False) + self.fnLaunch(self.Launcher.launch_detect_tunnel.value, True) + self.fnLaunch(self.Launcher.launch_control_moving.value, True) self.fnLaunch(self.Launcher.launch_control_lane.value, False) self.fnLaunch(self.Launcher.launch_control_tunnel.value, False) - self.fnLaunch(self.Launcher.launch_control_moving.value, True) elif self.current_step_tunnel == self.StepOfTunnel.go_in_to_tunnel.value: rospy.loginfo("node Current step : go_in_to_tunnel") @@ -431,14 +503,22 @@ msg_pub_tunnel_order.data = self.StepOfTunnel.navigation.value self.fnLaunch(self.Launcher.launch_camera_ex_calib.value, False) - self.fnLaunch(self.Launcher.launch_detect_lane.value, False) - self.fnLaunch(self.Launcher.launch_detect_tunnel.value, True) + self.fnLaunch(self.Launcher.launch_detect_traffic_light.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_intersection.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_construction.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_parking.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_level_crossing.value, False) self.fnLaunch(self.Launcher.launch_detect_sign_tunnel.value, False) - + self.fnLaunch(self.Launcher.launch_detect_intersection.value, False) + self.fnLaunch(self.Launcher.launch_detect_construction.value, False) + self.fnLaunch(self.Launcher.launch_detect_parking.value, False) + self.fnLaunch(self.Launcher.launch_detect_level_crossing.value, False) + self.fnLaunch(self.Launcher.launch_detect_tunnel.value, True) + self.fnLaunch(self.Launcher.launch_control_moving.value, False) self.fnLaunch(self.Launcher.launch_control_lane.value, False) self.fnLaunch(self.Launcher.launch_control_tunnel.value, True) - self.fnLaunch(self.Launcher.launch_control_moving.value, False) + elif self.current_step_tunnel == self.StepOfTunnel.navigation.value: rospy.loginfo("node Current step : navigation") rospy.loginfo("node Go to next step : go_out_from_tunnel") @@ -446,14 +526,21 @@ msg_pub_tunnel_order.data = self.StepOfTunnel.go_out_from_tunnel.value self.fnLaunch(self.Launcher.launch_camera_ex_calib.value, True) - - self.fnLaunch(self.Launcher.launch_detect_sign_tunnel.value, False) self.fnLaunch(self.Launcher.launch_detect_lane.value, False) + self.fnLaunch(self.Launcher.launch_detect_traffic_light.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_intersection.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_construction.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_parking.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_level_crossing.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_tunnel.value, False) + self.fnLaunch(self.Launcher.launch_detect_intersection.value, False) + self.fnLaunch(self.Launcher.launch_detect_construction.value, False) + self.fnLaunch(self.Launcher.launch_detect_parking.value, False) + self.fnLaunch(self.Launcher.launch_detect_level_crossing.value, False) self.fnLaunch(self.Launcher.launch_detect_tunnel.value, True) - + self.fnLaunch(self.Launcher.launch_control_moving.value, True) self.fnLaunch(self.Launcher.launch_control_lane.value, False) self.fnLaunch(self.Launcher.launch_control_tunnel.value, False) - self.fnLaunch(self.Launcher.launch_control_moving.value, True) elif self.current_step_tunnel == self.StepOfTunnel.go_out_from_tunnel.value: rospy.loginfo("node Current step : go_out_from_tunnel") @@ -462,14 +549,21 @@ msg_pub_tunnel_order.data = self.StepOfTunnel.exit.value self.fnLaunch(self.Launcher.launch_camera_ex_calib.value, True) - - self.fnLaunch(self.Launcher.launch_detect_tunnel.value, False) self.fnLaunch(self.Launcher.launch_detect_lane.value, True) + self.fnLaunch(self.Launcher.launch_detect_traffic_light.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_intersection.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_construction.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_parking.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_level_crossing.value, False) self.fnLaunch(self.Launcher.launch_detect_sign_tunnel.value, True) - + self.fnLaunch(self.Launcher.launch_detect_intersection.value, False) + self.fnLaunch(self.Launcher.launch_detect_construction.value, False) + self.fnLaunch(self.Launcher.launch_detect_parking.value, False) + self.fnLaunch(self.Launcher.launch_detect_level_crossing.value, False) + self.fnLaunch(self.Launcher.launch_detect_tunnel.value, False) + self.fnLaunch(self.Launcher.launch_control_moving.value, False) self.fnLaunch(self.Launcher.launch_control_lane.value, True) self.fnLaunch(self.Launcher.launch_control_tunnel.value, False) - self.fnLaunch(self.Launcher.launch_control_moving.value, False) elif self.current_step_tunnel == self.StepOfTunnel.exit.value: rospy.loginfo("node Current step : exit") @@ -478,11 +572,19 @@ msg_pub_tunnel_order.data = self.StepOfTunnel.searching_tunneROSLaunchParentvalue self.fnLaunch(self.Launcher.launch_camera_ex_calib.value, True) - - self.fnLaunch(self.Launcher.launch_detect_tunnel.value, False) self.fnLaunch(self.Launcher.launch_detect_lane.value, True) + self.fnLaunch(self.Launcher.launch_detect_traffic_light.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_intersection.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_construction.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_parking.value, False) + self.fnLaunch(self.Launcher.launch_detect_sign_level_crossing.value, False) self.fnLaunch(self.Launcher.launch_detect_sign_tunnel.value, True) - + self.fnLaunch(self.Launcher.launch_detect_intersection.value, False) + self.fnLaunch(self.Launcher.launch_detect_construction.value, False) + self.fnLaunch(self.Launcher.launch_detect_parking.value, False) + self.fnLaunch(self.Launcher.launch_detect_level_crossing.value, False) + self.fnLaunch(self.Launcher.launch_detect_tunnel.value, False) + self.fnLaunch(self.Launcher.launch_control_moving.value, False) self.fnLaunch(self.Launcher.launch_control_lane.value, True) self.fnLaunch(self.Launcher.launch_control_tunnel.value, False) self.fnLaunch(self.Launcher.launch_control_moving.value, False) diff --git a/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_construction b/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_construction index 543f96e..399d11a 100755 --- a/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_construction +++ b/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_construction @@ -53,7 +53,7 @@ while True: if self.is_obstacle_detected == True: - rospy.loginfo("detectconstruction Encounter obstacle") + rospy.loginfo("Detectconstruction Encounter obstacle") break else: pass @@ -67,8 +67,8 @@ msg_pub_construction_return.data = self.StepOfConstruction.avoid_obstacle.value elif order.data == self.StepOfConstruction.avoid_obstacle.value: - rospy.loginfo("detectconstruction avoid obstacle") - rospy.loginfo("detectconstruction go left") + rospy.loginfo("Detectconstruction avoid obstacle") + rospy.loginfo("Detectconstruction go left") msg_moving = MovingParam() msg_moving.moving_type=2 msg_moving.moving_value_angular=90 @@ -81,7 +81,7 @@ rospy.sleep(1) - rospy.loginfo("detectconstruction go straight") + rospy.loginfo("Detectconstruction go straight") msg_moving.moving_type= 4 msg_moving.moving_value_angular=0.0 msg_moving.moving_value_linear=0.25 @@ -93,7 +93,7 @@ rospy.sleep(1) - rospy.loginfo("detectconstruction go right") + rospy.loginfo("Detectconstruction go right") msg_moving.moving_type=3 msg_moving.moving_value_angular=90 msg_moving.moving_value_linear=0.0 diff --git a/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_intersection_sign b/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_intersection_sign index 18256b4..32d6e08 100755 --- a/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_intersection_sign +++ b/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_intersection_sign @@ -155,7 +155,7 @@ else: matches_left = None - rospy.loginfo("DetectIntersectionSign nothing detected") + #rospy.loginfo("DetectIntersectionSign nothing detected") good_right = [] for m,n in matches_right: @@ -179,7 +179,7 @@ else: matches_right = None - # rospy.loginfo("DetectIntersectionSign nothing detected") + #rospy.loginfo("DetectIntersectionSign nothing detected") if image_out_num == 1: if self.pub_image_type == "compressed":