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 new file mode 100644 index 0000000..e6ca166 --- /dev/null +++ b/src/turtlebot3_autorace_2020/turtlebot3_autorace_core/nodes/.all_core_node_controller.swp 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 6f16472..0d9598c 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 @@ -56,44 +56,30 @@ rospy.loginfo("mode invoked_object=%d",invoked_object) rospy.loginfo("mode msg_data=%d",msg_data.data) rospy.loginfo("mode-------------------------------") - if self.current_mode == self.CurrentMode.lane_following.value: - rospy.loginfo("mode current_mode %d",self.current_mode) - if invoked_object == self.InvokedObject.traffic_light.value: # Traffic Light detected - rospy.loginfo("mode invoked object %d",invoked_object) - if msg_data.data == self.CurrentMode.intersection.value: - rospy.loginfo("mode detect traffic_light and chenge mode") - self.current_mode = self.CurrentMode.intersection.value - elif invoked_object == self.InvokedObject.traffic_sign.value: # Any Sign detected - rospy.loginfo("mode Any Sign detected") - if msg_data.data == self.TrafficSign.intersection.value: - rospy.loginfo("detect sign : intersection") - self.current_mode = self.CurrentMode.intersection.value - elif msg_data.data == self.TrafficSign.left.value: - rospy.loginfo("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") - self.current_mode = self.CurrentMode.intersection.value - elif msg_data.data == self.TrafficSign.construction.value: - rospy.loginfo("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.level_crossing.value: - rospy.loginfo("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") - self.current_mode = self.CurrentMode.tunnel.value - else: - pass - elif self.current_mode == self.CurrentMode.traffic_light.value: - if invoked_object == self.InvokedObject.traffic_light.value: # Traffic Light detected - rospy.loginfo("traffic_light to lane_following") - self.current_mode = self.CurrentMode.lane_follwoing.value - else: - pass + 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: + rospy.loginfo("detect sign : intersection") + self.current_mode = self.CurrentMode.intersection.value + elif msg_data.data == self.TrafficSign.left.value: + rospy.loginfo("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") + self.current_mode = self.CurrentMode.intersection.value + elif msg_data.data == self.TrafficSign.construction.value: + rospy.loginfo("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.level_crossing.value: + rospy.loginfo("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") + self.current_mode = self.CurrentMode.tunnel.value else: pass self.fnPublishMode() 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 8ea7868..94588f9 100755 --- a/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_intersection +++ b/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_intersection @@ -38,7 +38,7 @@ self.pub_moving = rospy.Publisher('/control/moving/state', MovingParam, queue_size= 1) self.StepOfIntersection = Enum('StepOfIntersection', 'detect_direction exit') - self.TrafficSign = Enum('TrafficSign', 'intersection left right') + self.TrafficSign = Enum('TrafficSign', 'intersection left right construction stop parking tunnel') self.is_moving_complete = False self.is_intersection_detected = False 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 2c5c70c..3084627 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 @@ -215,7 +215,6 @@ self.red_count = 0 self.stop_count = 0 - # rospy.loginfo("RGB = %d %d %d stop = %d status =%d",self.red_count,self.yellow_count,self.green_count,self.stop_count,status1) if self.green_count >= 3: rospy.loginfo("GREEN") cv2.putText(self.cv_image,"GREEN", (self.point_col, self.point_low), cv2.FONT_HERSHEY_DUPLEX, 0.5, (80, 255, 0))