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 88cd8e9..2e9e704 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,7 +35,8 @@ 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 level_crossing tunnel') +# self.CurrentMode = Enum('CurrentMode', 'idle lane_following traffic_light intersection construction parking level_crossing tunnel') + self.CurrentMode = Enum('CurrentMode', 'idle lane_following traffic_light intersection construction parking level_crossing tunnel lane1 lane2 lane3 lane4 lane5') self.fnInitMode() self.countintersection = 0 # Invoke if traffic light is detected @@ -68,12 +69,12 @@ 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 + self.current_mode = self.CurrentMode.lane1.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 + self.current_mode = self.CurrentMode.lane2.value elif msg_data.data == self.TrafficSign.left.value: self.countintersection = self.countintersection - 1 rospy.loginfo("mode detect sign : intersection left %d",self.countintersection) @@ -95,6 +96,7 @@ rospy.loginfo("mode detect sign : tunnel") self.current_mode = self.CurrentMode.tunnel.value if self.current_mode == self.CurrentMode.intersection.value and abs(self.countintersection) > 2: + rospy.sleep(1) rospy.loginfo("mode intersection and count intersection") self.current_mode = self.CurrentMode.construction.value else: 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 1ba3d6b..f09104e 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 @@ -48,7 +48,7 @@ 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 level_crossing tunnel') + self.CurrentMode = Enum('CurrentMode', 'idle lane_following traffic_light intersection construction parking level_crossing tunnel lane1 lane2 lane3 lane4 lane5') self.current_mode = self.CurrentMode.idle.value self.InvokedObject = Enum('InvokedObject', 'traffic_light traffic_sign') @@ -173,17 +173,39 @@ rospy.loginfo("node Current step parking=%d",self.current_step_parking) rospy.loginfo("node Current step tunnel=%d",self.current_step_tunnel) - if self.current_mode == self.CurrentMode.lane_following.value: + if self.current_mode == self.CurrentMode.lane_following.value or self.current_mode == self.CurrentMode.lane1.value \ + or self.current_mode == self.CurrentMode.lane2.value or self.current_mode == self.CurrentMode.lane3.value \ + or self.current_mode == self.CurrentMode.lane4.value or self.current_mode == self.CurrentMode.lane5.value: 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, 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) + if self.current_mode == self.CurrentMode.lane1.value: + self.fnLaunch(self.Launcher.launch_detect_sign_intersection.value, True) + else: + self.fnLaunch(self.Launcher.launch_detect_sign_intersection.value, False) + + if self.current_mode == self.CurrentMode.lane2.value: + self.fnLaunch(self.Launcher.launch_detect_sign_construction.value, True) + else: + self.fnLaunch(self.Launcher.launch_detect_sign_construction.value, False) + + if self.current_mode == self.CurrentMode.lane3.value: + self.fnLaunch(self.Launcher.launch_detect_sign_parking.value, True) + else: + self.fnLaunch(self.Launcher.launch_detect_sign_parking.value, False) + + if self.current_mode == self.CurrentMode.lane4.value: + self.fnLaunch(self.Launcher.launch_detect_sign_level_crossing.value, True) + else: + self.fnLaunch(self.Launcher.launch_detect_sign_level_crossing.value, False) + + if self.current_mode == self.CurrentMode.lane5.value: + self.fnLaunch(self.Launcher.launch_detect_sign_tunnel.value, True) + else: + 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) 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 670896a..e9aae45 100755 --- a/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_construction +++ b/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_construction @@ -156,7 +156,7 @@ rospy.loginfo("DetectConstructioni go straight") msg_moving.moving_type=4 msg_moving.moving_value_angular=0.0 - msg_moving.moving_value_linear=0.6 ################0.4 + msg_moving.moving_value_linear=0.4 self.pub_moving.publish(msg_moving) while True: if self.is_moving_complete == True: 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 6102f93..7c57e3e 100755 --- a/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_parking +++ b/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_parking @@ -39,7 +39,7 @@ self.pub_max_vel = rospy.Publisher('/control/max_vel', Float64, queue_size = 1) self.StepOfParking = Enum('StepOfParking', 'parking exit') - self.start_obstacle_detection2 = False + self.start_obstacle_detection = False self.is_obstacle_detected_R = False self.is_obstacle_detected_L = False self.is_moving_complete = False @@ -97,7 +97,7 @@ rospy.sleep(1) - self.start_obstacle_detection2 = True + self.start_obstacle_detection = True while True: if self.is_obstacle_detected_L == True or self.is_obstacle_detected_R == True: break @@ -253,7 +253,7 @@ msg_moving = MovingParam() msg_moving.moving_type= 4 msg_moving.moving_value_angular=0.0 - msg_moving.moving_value_linear=1.0 + msg_moving.moving_value_linear=0.5 self.pub_moving.publish(msg_moving) while True: if self.is_moving_complete == True: @@ -303,7 +303,7 @@ scan_start_right = 270 - angle_scan scan_end_right = 270 + angle_scan threshold_distance = 0.5 - if self.start_obstacle_detection2 == True: + if self.start_obstacle_detection == True: 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 @@ -314,7 +314,7 @@ if scan.ranges[i] < threshold_distance and scan.ranges[i] > 0.01: self.is_obstacle_detected_R = True rospy.loginfo("DetectParking right detected") - self.start_obstacle_detection2 = False + self.start_obstacle_detection = False def main(self): rospy.spin() 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 2234819..06b2fa5 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 @@ -93,8 +93,8 @@ self.pub_traffic_light_return = rospy.Publisher('/detect/traffic_light_stamped', UInt8, queue_size=1) 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 level_crossing tunnel') + self.CurrentMode = Enum('CurrentMode', 'idle lane_following traffic_light intersection construction parking level_crossing tunnel lane1 lane2 lane3 lane4 lane5') +# 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 @@ -219,7 +219,8 @@ rospy.loginfo("GREEN") 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 + msg_sign.data = self.CurrentMode.lane1.value + #msg_sign.data = self.CurrentMode.intersection.value rospy.loginfo("detect traffic_light Publish intersection from detect") self.pub_traffic_light.publish(msg_sign) self.is_traffic_light_finished = True