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 cad3db6..88cd8e9 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 @@ -46,8 +46,18 @@ rospy.loginfo("mode Invoke Traffic sign detected") self.fnDecideMode(self.InvokedObject.traffic_sign.value, traffic_sign_msg) def cbReturnedMode(self, mode): - rospy.loginfo("mode Init Mode") - self.fnInitMode() + rospy.loginfo("mode =%d construction=%d",mode.data,self.CurrentMode.construction.value) + if mode.data == self.CurrentMode.parking.value: + self.current_mode = self.CurrentMode.parking.value + rospy.loginfo("set parking") + elif mode.data == self.CurrentMode.level_crossing: + self.current_mode = self.Current.level_crossing + rospy.loginfo("set level_crossing") + else: + self.current_mode = self.CurrentMode.lane_following.value + rospy.loginfo("mode Init Mode") + self.fnPublishMode() + def fnInitMode(self): # starts only when the program is started initially or any mission is completed self.current_mode = self.CurrentMode.lane_following.value self.fnPublishMode() @@ -65,8 +75,8 @@ 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 %d",self.countintersection) self.countintersection = self.countintersection - 1 + rospy.loginfo("mode detect sign : intersection left %d",self.countintersection) self.current_mode = self.CurrentMode.intersection.value elif msg_data.data == self.TrafficSign.right.value: self.countintersection = self.countintersection + 1 @@ -78,14 +88,14 @@ 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: + elif msg_data.data == self.TrafficSign.stop.value: 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("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.loginfo("mode jfidsjfisodafjidsiosdafjiodsaf") + rospy.loginfo("mode intersection and count intersection") self.current_mode = self.CurrentMode.construction.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 016724c..1ba3d6b 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 @@ -103,6 +103,7 @@ self.current_step_intersection = intersection_msg.data if self.current_step_intersection == self.StepOfIntersection.exit.value: + rospy.loginfo("mode intersection exit") self.current_mode = self.CurrentMode.construction.value msg_mode_return = UInt8() msg_mode_return.data = self.current_mode @@ -115,6 +116,7 @@ self.current_step_construction = construction_msg.data if self.current_step_construction == self.StepOfConstruction.exit.value: + rospy.loginfo("node construction exit") self.current_mode = self.CurrentMode.parking.value msg_mode_return = UInt8() msg_mode_return.data = self.current_mode @@ -176,7 +178,7 @@ 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_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) @@ -262,7 +264,7 @@ 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_detect_construction.value, Falses) 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) @@ -270,6 +272,8 @@ self.fnLaunch(self.Launcher.launch_control_lane.value, True) self.fnLaunch(self.Launcher.launch_control_tunnel.value, False) + self.current_mode = self.CurrentMode.construction.value + rospy.sleep(3) self.pub_intersection_order.publish(msg_pub_intersection_order) @@ -375,8 +379,8 @@ self.fnLaunch(self.Launcher.launch_detect_parking.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, True) - self.fnLaunch(self.Launcher.launch_control_lane.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: 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 399d11a..670896a 100755 --- a/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_construction +++ b/src/turtlebot3_autorace_2020/turtlebot3_autorace_detect/nodes/detect_construction @@ -132,7 +132,7 @@ rospy.loginfo("DetectConstruction go straight") msg_moving.moving_type= 4 msg_moving.moving_value_angular=0.0 - msg_moving.moving_value_linear=0.4 + msg_moving.moving_value_linear=0.25 #0.4 self.pub_moving.publish(msg_moving) while True: if self.is_moving_complete == True: @@ -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.4 + msg_moving.moving_value_linear=0.6 ################0.4 self.pub_moving.publish(msg_moving) while True: if self.is_moving_complete == True: @@ -167,7 +167,7 @@ rospy.loginfo("DetectConstruction go left") msg_moving.moving_type=2 - msg_moving.moving_value_angular=80 + msg_moving.moving_value_angular=90 msg_moving.moving_value_linear=0.0 self.pub_moving.publish(msg_moving) while 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 2954a51..6102f93 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_detection = False + self.start_obstacle_detection2 = False self.is_obstacle_detected_R = False self.is_obstacle_detected_L = False self.is_moving_complete = False @@ -62,7 +62,7 @@ msg_moving = MovingParam() msg_moving.moving_type= 4 msg_moving.moving_value_angular=0.0 - msg_moving.moving_value_linear=0.45 + msg_moving.moving_value_linear=0.7 #0.45 self.pub_moving.publish(msg_moving) while True: if self.is_moving_complete == True: @@ -97,7 +97,7 @@ rospy.sleep(1) - self.start_obstacle_detection = True + self.start_obstacle_detection2 = True while True: if self.is_obstacle_detected_L == True or self.is_obstacle_detected_R == True: break @@ -107,7 +107,7 @@ rospy.loginfo("DetectParking go right") msg_moving = MovingParam() msg_moving.moving_type=3 - msg_moving.moving_value_angular=90 + msg_moving.moving_value_angular=90 msg_moving.moving_value_linear=0.0 self.pub_moving.publish(msg_moving) while True: @@ -298,16 +298,12 @@ def cbScanObstacle(self, scan): angle_scan = 30 - scan_start_left = 90 - angle_scan scan_end_left = 90 + angle_scan - scan_start_right = 270 - angle_scan scan_end_right = 270 + angle_scan - threshold_distance = 0.5 - - if self.start_obstacle_detection == True: + if self.start_obstacle_detection2 == 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 @@ -318,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_detection = False + self.start_obstacle_detection2 = False def main(self): rospy.spin()