diff --git a/src/turtlebot3_autorace_2020/turtlebot3_autorace_core/nodes/.all_core_node_controller.swp b/src/turtlebot3_autorace_2020/turtlebot3_autorace_core/nodes/.all_core_node_controller.swp deleted file mode 100644 index e6ca166..0000000 --- a/src/turtlebot3_autorace_2020/turtlebot3_autorace_core/nodes/.all_core_node_controller.swp +++ /dev/null Binary files differ 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 0d9598c..ab957fe 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 @@ -35,17 +35,17 @@ self.pub_decided_mode = rospy.Publisher('/core/decided_mode', UInt8, queue_size=1) self.InvokedObject = Enum('InvokedObject', 'traffic_light traffic_sign') self.TrafficSign = Enum('TrafficSign', 'intersection left right construction stop parking tunnel') - self.CurrentMode = Enum('CurrentMode', 'idle lane_following traffic_light intersection construction parking_lot level_crossing tunnel') + self.CurrentMode = Enum('CurrentMode', 'idle lane_following traffic_light intersection construction parking level_crossing tunnel') self.fnInitMode() # Invoke if traffic light is detected def cbInvokedByTrafficLight(self, invoke_type_msg): - rospy.loginfo("Invoke Traffic light detected") + rospy.loginfo("mode Invoke Traffic light detected") self.fnDecideMode(self.InvokedObject.traffic_light.value, invoke_type_msg) def cbInvokedByTrafficSign(self, traffic_sign_msg): - rospy.loginfo("Invoke Traffic sign detected") + rospy.loginfo("mode Invoke Traffic sign detected") self.fnDecideMode(self.InvokedObject.traffic_sign.value, traffic_sign_msg) def cbReturnedMode(self, mode): - rospy.loginfo("Init Mode") + rospy.loginfo("mode Init Mode") self.fnInitMode() def fnInitMode(self): # starts only when the program is started initially or any mission is completed self.current_mode = self.CurrentMode.lane_following.value @@ -60,25 +60,25 @@ self.current_mode = self.CurrentMode.intersection.value elif invoked_object == self.InvokedObject.traffic_sign.value: if msg_data.data == self.TrafficSign.intersection.value: - rospy.loginfo("detect sign : intersection") + rospy.loginfo("mode detect sign : intersection") self.current_mode = self.CurrentMode.intersection.value elif msg_data.data == self.TrafficSign.left.value: - rospy.loginfo("detect sign : intersection left") + rospy.loginfo("mode detect sign : intersection left") self.current_mode = self.CurrentMode.intersection.value elif msg_data.data == self.TrafficSign.right.value: - rospy.loginfo("detect sign : inteersection right") + rospy.loginfo("mode detect sign : inteersection right") self.current_mode = self.CurrentMode.intersection.value elif msg_data.data == self.TrafficSign.construction.value: - rospy.loginfo("detect sign : construction") + rospy.loginfo("mode detect sign : construction") self.current_mode = self.CurrentMode.construction.value - elif msg_data.data == self.TrafficSign.parking_lot.value: - rospy.loginfo("detect sign : parking_log") - self.current_mode = self.CurrentMode.parking_lot.value + elif msg_data.data == self.TrafficSign.parking.value: + rospy.loginfo("mode detect sign : parking_log") + self.current_mode = self.CurrentMode.parking.value elif msg_data.data == self.TrafficSign.level_crossing.value: - rospy.loginfo("detect sign : level_crossing") + rospy.loginfo("mode detect sign : level_crossing") self.current_mode = self.CurrentMode.level_crossing.value elif msg_data.data == self.TrafficSign.tunnel.value: - rospy.loginfo("detect sign : tunnel") + rospy.loginfo("mode detect sign : tunnel") self.current_mode = self.CurrentMode.tunnel.value else: pass 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 ead94f1..3a5bb19 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 @@ -41,13 +41,14 @@ # publishes orders self.pub_traffic_light_order = rospy.Publisher('/detect/traffic_light_order', UInt8, queue_size=1) self.pub_intersection_order = rospy.Publisher('/detect/intersection_order', UInt8, queue_size=1) + self.pub_construction_order = rospy.Publisher('/detect/construction_order', UInt8, queue_size=1) self.pub_parking_order = rospy.Publisher('/detect/parking_order', UInt8, queue_size=1) self.pub_level_crossing_order = rospy.Publisher('/detect/level_crossing_order', UInt8, queue_size=1) self.pub_tunnel_order = rospy.Publisher('/detect/tunnel_order', UInt8, queue_size=1) self.pub_mode_return = rospy.Publisher('/core/returned_mode', UInt8, queue_size=1) - self.CurrentMode = Enum('CurrentMode', 'idle lane_following traffic_light intersection construction parking_lot level_crossing tunnel') + self.CurrentMode = Enum('CurrentMode', 'idle lane_following traffic_light intersection construction parking level_crossing tunnel') self.current_mode = self.CurrentMode.idle.value self.InvokedObject = Enum('InvokedObject', 'traffic_light traffic_sign') @@ -64,7 +65,7 @@ self.current_step_parking = self.StepOfParking.parking.value self.current_step_tunnel = self.StepOfTunnel.searching_tunnel_sign.value - self.Launcher = Enum('Launcher', 'launch_camera_ex_calib launch_detect_lane launch_detect_traffic_light launch_control_lane launch_detect_sign_intersection launch_detect_sign_construction launch_detect_sign_parking launch_detect_sign_level_crossing launch_detect_sign_tunnel launch_detect_intersection launch_control_moving launch_detect_parking launch_detect_construction launch_detect_level launch_detect_tunnel launch_control_tunnel') + self.Launcher = Enum('Launcher', 'launch_camera_ex_calib launch_detect_lane launch_detect_traffic_light launch_control_lane launch_detect_sign_intersection launch_detect_sign_construction launch_detect_sign_parking launch_detect_sign_level_crossing launch_detect_sign_tunnel launch_detect_intersection launch_control_moving launch_detect_parking launch_detect_construction launch_detect_level_crossing launch_detect_tunnel launch_control_tunnel') self.uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) self.launch_camera_launched = False @@ -77,13 +78,12 @@ self.launch_detect_traffic_light_launched = False self.launch_detect_intersection_launched = False self.launch_detect_construction_launched = False - self.launch_detect_parking = False - self.launch_detect_level_launched = False + self.launch_detect_parking_launched = False + self.launch_detect_level_crossing_launched = False self.launch_control_moving_launched = False self.launch_control_lane_launched = False self.launch_detect_tunnel_launched = False self.launch_control_tunnel_launched = False - self.is_triggered = False self.is_go = False @@ -100,11 +100,11 @@ self.is_triggered = True def cbIntersectionStamped(self, intersection_msg): - rospy.loginfo("intersection Step changed from %d", self.current_step_intersection) + rospy.loginfo("node intersection Step changed from %d", self.current_step_intersection) self.current_step_intersection = intersection_msg.data if self.current_step_intersection == self.StepOfIntersection.exit.value: - self.current_mode = self.CurrentMode.lane_following.value + self.current_mode = self.CurrentMode.construction.value msg_mode_return = UInt8() msg_mode_return.data = self.current_mode self.pub_mode_return.publish(msg_mode_return) @@ -112,11 +112,11 @@ self.is_triggered = True def cbconstructionStamped(self, construction_msg): - rospy.loginfo("construction Step changed from %d", self.current_step_construction) + rospy.loginfo("node construction Step changed from %d", self.current_step_construction) self.current_step_construction = construction_msg.data if self.current_step_construction == self.StepOfConstruction.exit.value: - self.current_mode = self.CurrentMode.lane_following.value + self.current_mode = self.CurrentMode.parking.value msg_mode_return = UInt8() msg_mode_return.data = self.current_mode self.pub_mode_return.publish(msg_mode_return) @@ -125,11 +125,11 @@ # Which step is in Tunnel def cbTunnelStamped(self, tunnel_msg): - rospy.loginfo("Tunnel Step changed from %d", self.current_step_tunnel) + rospy.loginfo("node Tunnel Step changed from %d", self.current_step_tunnel) self.current_step_tunnel = tunnel_msg.data - rospy.loginfo("into %d", self.current_step_tunnel) + 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 @@ -140,11 +140,11 @@ self.is_triggered = True def cbParkingStamped(self, parking_msg): - rospy.loginfo("Parking Step changed from %d", self.current_step_parking) + rospy.loginfo("node Parking Step changed from %d", self.current_step_parking) self.current_step_parking = parking_msg.data if self.current_step_parking == self.StepOfParking.exit.value: - self.current_mode = self.CurrentMode.lane_following.value + self.current_mode = self.CurrentMode.level_crossing.value msg_mode_return = UInt8() msg_mode_return.data = self.current_mode self.pub_mode_return.publish(msg_mode_return) @@ -152,7 +152,7 @@ self.is_triggered = True def cbLevelCrossingStamped(self, level_crossing_msg): - rospy.loginfo("LevelCrossing Step changed from %d", self.current_step_level_crossing) + rospy.loginfo("node LevelCrossing Step changed from %d", self.current_step_level_crossing) self.current_step_level_crossing = level_crossing_msg.data @@ -173,27 +173,31 @@ rospy.loginfo("node Current step tunnel=%d",self.current_step_tunnel) if self.current_mode == self.CurrentMode.lane_following.value: - rospy.loginfo("New trigger for lane_following") + rospy.loginfo("node New trigger for lane_following") 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, True) 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_traffic_light.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) # traffic_light elif self.current_mode == self.CurrentMode.traffic_light.value: - rospy.loginfo("New trigger for traffic_light") + rospy.loginfo("node New trigger for traffic_light") msg_pub_traffic_light_order = UInt8() if self.current_step_traffic_light == self.StepOfTrafficLight.searching_traffic_light.value: - rospy.loginfo("Current step : searching_traffic_light") - rospy.loginfo("Go to next step : in_traffic_light %d",self.is_go) + rospy.loginfo("node Current step : searching_traffic_light") + rospy.loginfo("node Go to next step : in_traffic_light %d",self.is_go) msg_pub_traffic_light_order.data = self.StepOfTrafficLight.in_traffic_light.value self.fnLaunch(self.Launcher.launch_camera_ex_calib.value, True) @@ -205,19 +209,19 @@ 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, False) self.fnLaunch(self.Launcher.launch_control_moving.value, False) if self.is_go == False: self.fnLaunch(self.Launcher.launch_control_lane.value, False) else: - rospy.loginfo("---------------------------------------------------") + rospy.loginfo("node ---------------------------------------------------") self.fnLaunch(self.Launcher.launch_control_lane.value, True) rospy.sleep(2) - #self.current_mode = self.CurrentMode.lane_following.value - self.current_mode = self.CurrentMode.intersection.value - #self.current_step_traffic_light = self.StepOfTrafficLight.in_traffic_light.value self.current_step_intersection = self.StepOfIntersection.detect_direction.value - rospy.sleep(2) self.current_mode = self.CurrentMode.intersection.value msg_mode_return = UInt8() msg_mode_return.data = self.current_mode @@ -228,45 +232,63 @@ # intersection elif self.current_mode == self.CurrentMode.intersection.value: - rospy.loginfo("New trigger for intersection") + rospy.loginfo("node New trigger for intersection") msg_pub_intersection_order = UInt8() if self.current_step_intersection == self.StepOfIntersection.detect_direction.value: - rospy.loginfo("Current step : searching_intersection_sign") - rospy.loginfo("Go to next step : exit") - + rospy.loginfo("node Current step : searching_intersection_sign") + rospy.loginfo("node Go to next step : exit") + msg_pub_intersection_order.data = self.StepOfIntersection.detect_direction.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, True) + 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, True) - + 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_moving.value, True) elif self.current_step_intersection == self.StepOfIntersection.exit.value: - rospy.loginfo("Current step : exit") + rospy.loginfo("node Current step : exit") msg_pub_intersection_order.data = self.StepOfIntersection.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_sign_intersection.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, 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_control_lane.value, True) + self.fnLaunch(self.Launcher.launch_detect_construction.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) rospy.sleep(3) self.pub_intersection_order.publish(msg_pub_intersection_order) # construction elif self.current_mode == self.CurrentMode.construction.value: - rospy.loginfo("New trigger for construction") + rospy.loginfo("node New trigger for construction") msg_pub_construction_order = UInt8() if self.current_step_construction == self.StepOfConstruction.find_obstacle.value: - rospy.loginfo("Current step : find_obstacle") - rospy.loginfo("Go to next setp : avoid_obstacle") + rospy.loginfo("node Current step : find_obstacle") + rospy.loginfo("node Go to next setp : avoid_obstacle") msg_pub_construction_order.data = self.StepOfConstruction.find_obstacle.value @@ -280,8 +302,8 @@ self.fnLaunch(self.Launcher.launch_control_moving.value, False) elif self.current_step_construction == self.StepOfConstruction.avoid_obstacle.value: - rospy.loginfo("Current step : avoid_obstacle") - rospy.loginfo("Go to next step : exit") + rospy.loginfo("node Current step : avoid_obstacle") + rospy.loginfo("node Go to next step : exit") msg_pub_construction_order.data = self.StepOfConstruction.avoid_obstacle.value @@ -295,7 +317,7 @@ self.fnLaunch(self.Launcher.launch_control_moving.value, True) elif self.current_step_construction == self.StepOfConstruction.exit.value: - rospy.loginfo("Current step : exit") + rospy.loginfo("node Current step : exit") msg_pub_construction_order.data = self.StepOfConstruction.exit.value @@ -314,12 +336,12 @@ # parking elif self.current_mode == self.CurrentMode.parking.value: - rospy.loginfo("New trigger for parking") + rospy.loginfo("noode New trigger for parking") msg_pub_parking_order = UInt8() if self.current_step_parking == self.StepOfParking.parking.value: - rospy.loginfo("Current step : parking") - rospy.loginfo("Go to next step : finish parking") + rospy.loginfo("node Current step : parking") + rospy.loginfo("node Go to next step : finish parking") msg_pub_parking_order.data = self.StepOfParking.parking.value @@ -333,7 +355,7 @@ self.fnLaunch(self.Launcher.launch_control_moving.value, True) elif self.current_step_parking == self.StepOfParking.exit.value: - rospy.loginfo("Current step : finish parking") + rospy.loginfo("node Current step : finish parking") msg_pub_parking_order.data = self.StepOfParking.exit.value @@ -351,29 +373,29 @@ # level_crossing elif self.current_mode == self.CurrentMode.level_crossing.value: - rospy.loginfo("New trigger for level_crossing") + rospy.loginfo("node New trigger for level_crossing") msg_pub_level_crossing_order = UInt8() if self.current_step_level_crossing == self.StepOfLevelCrossing.pass_level.value: - rospy.loginfo("Current step : pass_level") - rospy.loginfo("Go to next step : exit") + rospy.loginfo("node Current step : pass_level") + 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.value, True) + self.fnLaunch(self.Launcher.launch_detect_level_crossing.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_control_moving.value, True) elif self.current_step_level_crossing == self.StepOfLevelCrossing.exit.value: - rospy.loginfo("Current step : exit") + 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_detect_lane.value, True) - self.fnLaunch(self.Launcher.launch_detect_level.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_control_moving.value, False) @@ -383,12 +405,12 @@ # tunnel elif self.current_mode == self.CurrentMode.tunnel.value: - rospy.loginfo("New trigger for tunnel") + rospy.loginfo("node New trigger for tunnel") msg_pub_tunnel_order = UInt8() if self.current_step_tunnel == self.StepOfTunnel.searching_tunnel_sign.value: - rospy.loginfo("Current step : searching_tunnel_sign") - rospy.loginfo("Go to next step : go_in_to_tunnel") + rospy.loginfo("node Current step : searching_tunnel_sign") + rospy.loginfo("node Go to next step : go_in_to_tunnel") msg_pub_tunnel_order.data = self.StepOfTunnel.go_in_to_tunnel.value @@ -403,8 +425,8 @@ self.fnLaunch(self.Launcher.launch_control_moving.value, True) elif self.current_step_tunnel == self.StepOfTunnel.go_in_to_tunnel.value: - rospy.loginfo("Current step : go_in_to_tunnel") - rospy.loginfo("Go to next step : navigation") + rospy.loginfo("node Current step : go_in_to_tunnel") + rospy.loginfo("node Go to next step : navigation") msg_pub_tunnel_order.data = self.StepOfTunnel.navigation.value @@ -418,8 +440,8 @@ 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("Current step : navigation") - rospy.loginfo("Go to next step : go_out_from_tunnel") + rospy.loginfo("node Current step : navigation") + rospy.loginfo("node Go to next step : go_out_from_tunnel") msg_pub_tunnel_order.data = self.StepOfTunnel.go_out_from_tunnel.value @@ -434,8 +456,8 @@ self.fnLaunch(self.Launcher.launch_control_moving.value, True) elif self.current_step_tunnel == self.StepOfTunnel.go_out_from_tunnel.value: - rospy.loginfo("Current step : go_out_from_tunnel") - rospy.loginfo("Go to next step : exit") + rospy.loginfo("node Current step : go_out_from_tunnel") + rospy.loginfo("node Go to next step : exit") msg_pub_tunnel_order.data = self.StepOfTunnel.exit.value @@ -450,8 +472,8 @@ self.fnLaunch(self.Launcher.launch_control_moving.value, False) elif self.current_step_tunnel == self.StepOfTunnel.exit.value: - rospy.loginfo("Current step : exit") - rospy.loginfo("Go to next step : searching_tunnel_sign") + rospy.loginfo("node Current step : exit") + rospy.loginfo("node Go to next step : searching_tunnel_sign") msg_pub_tunnel_order.data = self.StepOfTunnel.searching_tunneROSLaunchParentvalue @@ -621,19 +643,19 @@ self.launch_detect_construction_launched = False self.launch_detect_construction.shutdown() pass - elif launch_num == self.Launcher.launch_detect_level.value: + elif launch_num == self.Launcher.launch_detect_level_crossing.value: if is_start == True: - if self.launch_detect_level_launched == False: - self.launch_detect_level = roslaunch.scriptapi.ROSLaunch() - self.launch_detect_level = roslaunch.parent.ROSLaunchParent(self.uuid, [self.ros_package_path + "turtlebot3_autorace_detect/launch/detect_level_crossing.launch"]) - self.launch_detect_level_launched = True - self.launch_detect_level.start() + if self.launch_detect_level_crossing_launched == False: + self.launch_detect_level_crossing = roslaunch.scriptapi.ROSLaunch() + self.launch_detect_level_crossing = roslaunch.parent.ROSLaunchParent(self.uuid, [self.ros_package_path + "turtlebot3_autorace_detect/launch/detect_level_crossing.launch"]) + self.launch_detect_level_crossing_launched = True + self.launch_detect_level_crossing.start() else: pass else: - if self.launch_detect_level_launched == True: - self.launch_detect_level_launched = False - self.launch_detect_level.shutdown() + if self.launch_detect_level_crossing_launched == True: + self.launch_detect_level_crossing_launched = False + self.launch_detect_level_crossing.shutdown() else: pass elif launch_num == self.Launcher.launch_control_lane.value: diff --git a/src/turtlebot3_autorace_2020/turtlebot3_autorace_core/nodes/construction_core_node_controller b/src/turtlebot3_autorace_2020/turtlebot3_autorace_core/nodes/construction_core_node_controller index 001f695..a485bfa 100755 --- a/src/turtlebot3_autorace_2020/turtlebot3_autorace_core/nodes/construction_core_node_controller +++ b/src/turtlebot3_autorace_2020/turtlebot3_autorace_core/nodes/construction_core_node_controller @@ -65,7 +65,7 @@ loop_rate.sleep() def cbReceiveMode(self, mode_msg): - rospy.loginfo("starts the progress with %d", mode_msg.data) + rospy.loginfo("nodes starts the progress with %d", mode_msg.data) self.current_mode = mode_msg.data self.is_triggered = True 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 40eaffc..543f96e 100755 --- a/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_construction +++ b/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_construction @@ -49,11 +49,11 @@ msg_pub_construction_return = UInt8() if order.data == self.StepOfConstruction.find_obstacle.value: - rospy.loginfo("find obstacle") + rospy.loginfo("DetectConstruction find obstacle") while True: if self.is_obstacle_detected == True: - rospy.loginfo("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("avoid obstacle") - rospy.loginfo("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("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("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 @@ -105,7 +105,7 @@ rospy.sleep(1) - rospy.loginfo("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.5 @@ -117,7 +117,7 @@ rospy.sleep(1) - rospy.loginfo("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 @@ -129,7 +129,7 @@ rospy.sleep(1) - rospy.loginfo("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.4 @@ -141,7 +141,7 @@ rospy.sleep(1) - rospy.loginfo("go left") + rospy.loginfo("DetectConstruction go left") msg_moving.moving_type=2 msg_moving.moving_value_angular=90 msg_moving.moving_value_linear=0.0 @@ -153,7 +153,7 @@ rospy.sleep(1) - rospy.loginfo("go straight") + rospy.loginfo("DetectConstructioni go straight") msg_moving.moving_type=4 msg_moving.moving_value_angular=0.0 msg_moving.moving_value_linear=0.4 @@ -165,7 +165,7 @@ rospy.sleep(1) - rospy.loginfo("go left") + rospy.loginfo("DetectConstruction go left") msg_moving.moving_type=2 msg_moving.moving_value_angular=80 msg_moving.moving_value_linear=0.0 @@ -177,7 +177,7 @@ rospy.sleep(1) - rospy.loginfo("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.4 @@ -187,11 +187,11 @@ break self.is_moving_complete = False - rospy.loginfo("construction finished") + rospy.loginfo("DetectConstruction construction finished") msg_pub_construction_return.data = self.StepOfConstruction.exit.value elif order.data == self.StepOfConstruction.exit.value: - rospy.loginfo("construction finished") + rospy.loginfo("DetectConstruction construction finished") msg_pub_construction_return.data = self.StepOfConstruction.exit.value self.pub_construction_return.publish(msg_pub_construction_return) diff --git a/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_construction_sign b/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_construction_sign index 3539d4b..c2962a0 100755 --- a/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_construction_sign +++ b/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_construction_sign @@ -124,11 +124,11 @@ self.pub_traffic_sign.publish(msg_sign) - rospy.loginfo("construction") + rospy.loginfo("DetectConstructionSign construction") image_out_num = 2 else: matches_construction = None - rospy.loginfo("not found") + rospy.loginfo("DetectConstructionSign not found") if image_out_num == 1: if self.pub_image_type == "compressed": diff --git a/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_intersection b/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_intersection index 94588f9..e6fafe8 100755 --- a/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_intersection +++ b/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_intersection @@ -71,7 +71,7 @@ if self.is_right_detected == True: - rospy.loginfo("go straight") + rospy.loginfo("DetectIntersection go straight") msg_moving = MovingParam() msg_moving.moving_type=4 msg_moving.moving_value_angular=0 @@ -84,7 +84,7 @@ rospy.sleep(1) - rospy.loginfo("go right") + rospy.loginfo("DetectIntersection go right") msg_moving = MovingParam() msg_moving.moving_type=3 msg_moving.moving_value_angular=45 @@ -97,7 +97,7 @@ rospy.sleep(1) - rospy.loginfo("go straight") + rospy.loginfo("DetectIntersection go straight") msg_moving = MovingParam() msg_moving.moving_type=4 msg_moving.moving_value_angular=0 @@ -112,7 +112,7 @@ elif self.is_left_detected == True: - rospy.loginfo("go straight") + rospy.loginfo("DetectIntersection go straight") msg_moving = MovingParam() msg_moving.moving_type=4 msg_moving.moving_value_angular=0 @@ -125,7 +125,7 @@ rospy.sleep(1) - rospy.loginfo("go left") + rospy.loginfo("DetectIntersection go left") msg_moving = MovingParam() msg_moving.moving_type=2 msg_moving.moving_value_angular=85 @@ -138,7 +138,7 @@ rospy.sleep(1) - rospy.loginfo("go straight") + rospy.loginfo("DetectIntersection go straight") msg_moving = MovingParam() msg_moving.moving_type=4 msg_moving.moving_value_angular=0 @@ -151,11 +151,11 @@ rospy.sleep(1) - rospy.loginfo("moving finished") + rospy.loginfo("DetectIntersection moving finished") msg_pub_intersection_return.data = self.StepOfIntersection.exit.value elif order.data == self.StepOfIntersection.exit.value: - rospy.loginfo("Now finished") + rospy.loginfo("DetectIntersection Now finished") rospy.sleep(2) msg_pub_intersection_return.data = self.StepOfIntersection.exit.value 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 73eb019..18256b4 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 @@ -130,7 +130,7 @@ msg_sign.data = self.TrafficSign.intersection.value self.pub_traffic_sign.publish(msg_sign) - rospy.loginfo("detect intersection sign") + rospy.loginfo("DetectInstersectionSign detect intersection sign") image_out_num = 2 good_left = [] @@ -150,12 +150,12 @@ msg_sign.data = self.TrafficSign.left.value self.pub_traffic_sign.publish(msg_sign) - rospy.loginfo("detect left sign") + rospy.loginfo("DetectIntersectionSign detect left sign") image_out_num = 3 else: matches_left = None - # rospy.loginfo("nothing detected") + rospy.loginfo("DetectIntersectionSign nothing detected") good_right = [] for m,n in matches_right: @@ -174,12 +174,12 @@ msg_sign.data = self.TrafficSign.right.value self.pub_traffic_sign.publish(msg_sign) - rospy.loginfo("detect right sign") + rospy.loginfo("DetectIntersectionSign detect right sign") image_out_num = 4 else: matches_right = None - # rospy.loginfo("nothing detected") + # rospy.loginfo("DetectIntersectionSign nothing detected") if image_out_num == 1: if self.pub_image_type == "compressed": diff --git a/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_level_crossing b/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_level_crossing index 58b5d7d..16ba178 100755 --- a/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_level_crossing +++ b/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_level_crossing @@ -186,7 +186,7 @@ while True: is_level_detected, _, _ = self.fnFindLevel() if is_level_detected == True: - rospy.loginfo("Level Detect") + rospy.loginfo("DetectLevelCrossing Level Detect") msg_pub_max_vel = Float64() msg_pub_max_vel.data = 0.03 self.pub_max_vel.publish(msg_pub_max_vel) @@ -197,7 +197,7 @@ while True: _, is_level_close, _ = self.fnFindLevel() if is_level_close == True: - rospy.loginfo("STOP") + rospy.loginfo("DetectLevelCrossing STOP") msg_pub_max_vel = Float64() msg_pub_max_vel.data = 0.0 self.pub_max_vel.publish(msg_pub_max_vel) @@ -208,7 +208,7 @@ while True: _, _, is_level_opened = self.fnFindLevel() if is_level_opened == True: - rospy.loginfo("GO") + rospy.loginfo("DetectLevelCrossing GO") msg_pub_max_vel = Float64() msg_pub_max_vel.data = 0.05 self.pub_max_vel.publish(msg_pub_max_vel) @@ -219,7 +219,7 @@ pub_level_crossing_return.data = self.StepOfLevelCrossing.exit.value elif order.data == self.StepOfLevelCrossing.exit.value: - rospy.loginfo("Now pass_level") + rospy.loginfo("DetectLevelCrossing Now pass_level") pub_level_crossing_return.data = self.StepOfLevelCrossing.exit.value self.pub_level_crossing_return.publish(pub_level_crossing_return) diff --git a/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_level_crossing_sign b/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_level_crossing_sign index 082661e..c046846 100755 --- a/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_level_crossing_sign +++ b/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_level_crossing_sign @@ -125,12 +125,12 @@ self.pub_traffic_sign.publish(msg_sign) - rospy.loginfo("stop") + rospy.loginfo("DetectLevelCrossingSign stop") image_out_num = 2 else: matches_stop = None - rospy.loginfo("nothing") + rospy.loginfo("DetectLevelCrossingSign nothing") if image_out_num == 1: if self.pub_image_type == "compressed": diff --git a/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_parking b/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_parking index 347e749..2954a51 100755 --- a/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_parking +++ b/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_parking @@ -51,14 +51,14 @@ msg_pub_parking_return = UInt8() if order.data == self.StepOfParking.parking.value: - rospy.loginfo("Now motion") + rospy.loginfo("DetectParking Now motion") msg_pub_max_vel = Float64() msg_pub_max_vel.data = 0.00 self.pub_max_vel.publish(msg_pub_max_vel) rospy.sleep(1) - rospy.loginfo("go straight") + rospy.loginfo("DetectParking go straight") msg_moving = MovingParam() msg_moving.moving_type= 4 msg_moving.moving_value_angular=0.0 @@ -71,7 +71,7 @@ rospy.sleep(1) - rospy.loginfo("go left") + rospy.loginfo("DetectParking go left") msg_moving = MovingParam() msg_moving.moving_type=2 msg_moving.moving_value_angular=90 @@ -84,7 +84,7 @@ rospy.sleep(1) - rospy.loginfo("go straight") + rospy.loginfo("DetectParking go straight") msg_moving = MovingParam() msg_moving.moving_type= 4 msg_moving.moving_value_angular=0.0 @@ -103,8 +103,8 @@ break if self.is_obstacle_detected_R == False: - rospy.loginfo("right parking is clear") - rospy.loginfo("go right") + rospy.loginfo("DetectParking right parking is clear") + rospy.loginfo("DetectParking go right") msg_moving = MovingParam() msg_moving.moving_type=3 msg_moving.moving_value_angular=90 @@ -117,7 +117,7 @@ rospy.sleep(1) - rospy.loginfo("go straight") + rospy.loginfo("DetectParking go straight") msg_moving = MovingParam() msg_moving.moving_type= 4 msg_moving.moving_value_angular=0.0 @@ -130,7 +130,7 @@ rospy.sleep(1) - rospy.loginfo("go back straight") + rospy.loginfo("DetectParking go back straight") msg_moving = MovingParam() msg_moving.moving_type= 5 msg_moving.moving_value_angular=0.0 @@ -143,7 +143,7 @@ rospy.sleep(1) - rospy.loginfo("go right") + rospy.loginfo("DetectParking go right") msg_moving = MovingParam() msg_moving.moving_type= 3 msg_moving.moving_value_angular=90 @@ -156,7 +156,7 @@ rospy.sleep(1) - rospy.loginfo("go straight") + rospy.loginfo("DetectParking go straight") msg_moving = MovingParam() msg_moving.moving_type= 4 msg_moving.moving_value_angular=0.0 @@ -169,7 +169,7 @@ rospy.sleep(1) - rospy.loginfo("go left") + rospy.loginfo("DetectParking go left") msg_moving = MovingParam() msg_moving.moving_type=2 msg_moving.moving_value_angular=90 @@ -182,7 +182,7 @@ rospy.sleep(1) - rospy.loginfo("go straight") + rospy.loginfo("DetectParking go straight") msg_moving = MovingParam() msg_moving.moving_type=4 msg_moving.moving_value_angular=0.0 @@ -196,8 +196,8 @@ rospy.sleep(1) elif self.is_obstacle_detected_L == False: - rospy.loginfo("left parking is clear") - rospy.loginfo("go left") + rospy.loginfo("DetectParking left parking is clear") + rospy.loginfo("DetectParking go left") msg_moving = MovingParam() msg_moving.moving_type=2 msg_moving.moving_value_angular=90 @@ -210,7 +210,7 @@ rospy.sleep(1) - rospy.loginfo("go straight") + rospy.loginfo("DetectParking go straight") msg_moving = MovingParam() msg_moving.moving_type= 4 msg_moving.moving_value_angular=0.0 @@ -223,7 +223,7 @@ rospy.sleep(1) - rospy.loginfo("go back straight") + rospy.loginfo("DetectParking go back straight") msg_moving = MovingParam() msg_moving.moving_type= 5 msg_moving.moving_value_angular=0.0 @@ -236,7 +236,7 @@ rospy.sleep(1) - rospy.loginfo("go left") + rospy.loginfo("DetectParking go left") msg_moving = MovingParam() msg_moving.moving_type= 2 msg_moving.moving_value_angular=90 @@ -249,7 +249,7 @@ rospy.sleep(1) - rospy.loginfo("go straight") + rospy.loginfo("DetectParking go straight") msg_moving = MovingParam() msg_moving.moving_type= 4 msg_moving.moving_value_angular=0.0 @@ -262,7 +262,7 @@ rospy.sleep(1) - rospy.loginfo("go left") + rospy.loginfo("DetectParking go left") msg_moving = MovingParam() msg_moving.moving_type=2 msg_moving.moving_value_angular=90 @@ -275,7 +275,7 @@ rospy.sleep(1) - rospy.loginfo("go straight") + rospy.loginfo("DetectParking go straight") msg_moving = MovingParam() msg_moving.moving_type=4 msg_moving.moving_value_angular=0.0 @@ -286,11 +286,11 @@ break self.is_moving_complete = False - rospy.loginfo("parking finished") + rospy.loginfo("DetectParking parking finished") msg_pub_parking_return.data = self.StepOfParking.exit.value elif order.data == self.StepOfParking.exit.value: - rospy.loginfo("parking finished") + rospy.loginfo("DetectParking parking finished") msg_pub_parking_return.data = self.StepOfParking.exit.value self.pub_parking_return.publish(msg_pub_parking_return) @@ -311,13 +311,13 @@ for i in range(scan_start_left, scan_end_left): if scan.ranges[i] < threshold_distance and scan.ranges[i] > 0.01: self.is_obstacle_detected_L = True - rospy.loginfo("left detected") + rospy.loginfo("DetectParking left detected") for i in range(scan_start_right, scan_end_right): if scan.ranges[i] < threshold_distance and scan.ranges[i] > 0.01: self.is_obstacle_detected_R = True - rospy.loginfo("right detected") + rospy.loginfo("DetectParking right detected") self.start_obstacle_detection = False def main(self): diff --git a/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_parking_sign b/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_parking_sign index ab9681a..a2d5239 100755 --- a/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_parking_sign +++ b/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_parking_sign @@ -124,12 +124,12 @@ self.pub_traffic_sign.publish(msg_sign) - rospy.loginfo("parking") + rospy.loginfo("DetectParkingSign parking") image_out_num = 2 else: matchesMask_parking = None - rospy.loginfo("nothing") + rospy.loginfo("DetectParkingSign nothing") if image_out_num == 1: diff --git a/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_traffic_light b/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_traffic_light index 3084627..2234819 100755 --- a/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_traffic_light +++ b/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_traffic_light @@ -94,7 +94,7 @@ self.pub_traffic_start = rospy.Publisher('/control/traffic_light_start', UInt8, queue_size = 1) self.pub_traffic_light = rospy.Publisher('/detect/traffic_light', UInt8, queue_size=1) - self.CurrentMode = Enum('CurrentMode', 'idle lane_following traffic_light intersection construction parking_lot level_crossing tunnel') + self.CurrentMode = Enum('CurrentMode', 'idle lane_following traffic_light intersection construction parking level_crossing tunnel') self.InvokedObject = Enum('InvokedObject', 'traffic_light traffic_sign') self.cvBridge = CvBridge() self.cv_image = None @@ -220,7 +220,7 @@ cv2.putText(self.cv_image,"GREEN", (self.point_col, self.point_low), cv2.FONT_HERSHEY_DUPLEX, 0.5, (80, 255, 0)) msg_sign = UInt8() msg_sign.data = self.CurrentMode.intersection.value - rospy.loginfo("Publish intersection from detect") + rospy.loginfo("detect traffic_light Publish intersection from detect") self.pub_traffic_light.publish(msg_sign) self.is_traffic_light_finished = True diff --git a/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_tunnel b/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_tunnel index 5b6b2ca..2f744d7 100755 --- a/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_tunnel +++ b/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_tunnel @@ -50,7 +50,7 @@ def cbGetNavigationResult(self, msg_nav_result): if msg_nav_result.status.status == 3: - rospy.loginfo("Reached") + rospy.loginfo("DetectTunnel Reached") self.is_navigation_finished = True def cbMovingComplete(self, data): @@ -60,16 +60,16 @@ pub_tunnel_return = UInt8() if order.data == self.StepOfTunnel.searching_tunnel_sign.value: - rospy.loginfo("Now lane_following") + rospy.loginfo("DetectTunnel Now lane_following") pub_tunnel_return.data = self.StepOfTunnel.searching_tunnel_sign.value elif order.data == self.StepOfTunnel.go_in_to_tunnel.value: - rospy.loginfo("Now go_in_to_tunnel") + rospy.loginfo("DetectTunnel Now go_in_to_tunnel") rospy.sleep(1) - rospy.loginfo("go straight") + rospy.loginfo("DetectTunnel go straight") msg_moving = MovingParam() msg_moving.moving_type= 4 msg_moving.moving_value_angular=0.0 @@ -82,7 +82,7 @@ rospy.sleep(1) - rospy.loginfo("go left") + rospy.loginfo("DetectTunnel go left") msg_moving = MovingParam() msg_moving.moving_type=2 msg_moving.moving_value_angular=45 @@ -95,7 +95,7 @@ rospy.sleep(1) - rospy.loginfo("go straight") + rospy.loginfo("DetectTunnel go straight") msg_moving = MovingParam() msg_moving.moving_type= 4 msg_moving.moving_value_angular=0.0 @@ -108,12 +108,12 @@ rospy.sleep(1) - rospy.loginfo("go_in_to_tunnel finished") + rospy.loginfo("DetectTunnel go_in_to_tunnel finished") pub_tunnel_return.data = self.StepOfTunnel.go_in_to_tunnel.value elif order.data == self.StepOfTunnel.navigation.value: - rospy.loginfo("Now navigation") + rospy.loginfo("DetectTunnel Now navigation") initialPose = PoseWithCovarianceStamped() initialPose.header.frame_id = "map" initialPose.header.stamp = rospy.Time.now() @@ -128,13 +128,13 @@ pass pub_tunnel_return.data = self.StepOfTunnel.navigation.value - rospy.loginfo("navigation!") + rospy.loginfo("DetectTunnel navigation!") elif order.data == self.StepOfTunnel.go_out_from_tunnel.value: - rospy.loginfo("Now go_out_from_tunnel") + rospy.loginfo("DetectTunnel Now go_out_from_tunnel") - rospy.loginfo("go straight") + rospy.loginfo("DetectTunnel go straight") msg_moving = MovingParam() msg_moving.moving_type= 4 msg_moving.moving_value_angular=0.0 @@ -150,7 +150,7 @@ pub_tunnel_return.data = self.StepOfTunnel.go_out_from_tunnel.value elif order.data == self.StepOfTunnel.exit.value: - rospy.loginfo("Now exit") + rospy.loginfo("DetectTunnel Now exit") pub_tunnel_return.data = self.StepOfTunnel.exit.value diff --git a/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_tunnel_sign b/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_tunnel_sign index d55a6d4..eacb4f7 100755 --- a/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_tunnel_sign +++ b/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_tunnel_sign @@ -123,12 +123,12 @@ self.pub_traffic_sign.publish(msg_sign) - rospy.loginfo("tunnel") + rospy.loginfo("DetectTunnelSign tunnel") image_out_num = 4 else: matchesMask_tunnel = None - rospy.loginfo("nothing") + rospy.loginfo("DetectTunnelSign nothing") if image_out_num == 1: