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 fd6ba3b..016724c 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 @@ -85,7 +85,6 @@ self.launch_detect_tunnel_launched = False self.launch_control_tunnel_launched = False self.is_triggered = False - self.is_go = False loop_rate = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): @@ -198,7 +197,7 @@ msg_pub_traffic_light_order = UInt8() if self.current_step_traffic_light == self.StepOfTrafficLight.searching_traffic_light.value: rospy.loginfo("node Current step : searching_traffic_light") - rospy.loginfo("node Go to next step : in_traffic_light %d",self.is_go) + rospy.loginfo("node Go to next step : in_traffic_light ") msg_pub_traffic_light_order.data = self.StepOfTrafficLight.in_traffic_light.value self.fnLaunch(self.Launcher.launch_camera_ex_calib.value, True) @@ -216,18 +215,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_tunnel.value, False) - - if self.is_go == False: - self.fnLaunch(self.Launcher.launch_control_lane.value, False) - else: - rospy.loginfo("node -----------------------------------") - self.fnLaunch(self.Launcher.launch_control_lane.value, True) - rospy.sleep(2) - self.current_step_intersection = self.StepOfIntersection.detect_direction.value - self.current_mode = self.CurrentMode.intersection.value - msg_mode_return = UInt8() - msg_mode_return.data = self.current_mode - self.pub_mode_return.publish(msg_mode_return) + self.fnLaunch(self.Launcher.launch_control_lane.value, False) rospy.sleep(2) self.pub_traffic_light_order.publish(msg_pub_traffic_light_order)