GitBucket
Newer
#!/usr/bin/env python
# -*- coding: utf-8 -*-
################################################################################
# Copyright 2018 ROBOTIS CO., LTD.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
################################################################################
# Authors: Leon Jung, [AuTURBO] Kihoon Kim (https://github.com/auturbo), Gilbert, Ashe Kim
import rospy, roslaunch
import subprocess
import os
import sys
from enum import Enum
from std_msgs.msg import UInt8, Float64
class CoreNodeController():
def __init__(self):
self.ros_package_path = os.path.dirname(os.path.realpath(__file__))
self.ros_package_path = self.ros_package_path.replace('turtlebot3_autorace_core/nodes', '')
# subscribes : status returned
self.sub_mode_control = rospy.Subscriber('/core/decided_mode', UInt8, self.cbReceiveMode, queue_size=1)
self.sub_intersection_stamped = rospy.Subscriber('/detect/intersection_stamped', UInt8, self.cbIntersectionStamped, queue_size=1)
self.sub_construction_stamped = rospy.Subscriber('/detect/construction_stamped', UInt8, self.cbconstructionStamped, queue_size=1)
self.sub_parking_stamped = rospy.Subscriber('/detect/parking_stamped', UInt8, self.cbParkingStamped, queue_size=1)
self.sub_level_crossing_stamped = rospy.Subscriber('/detect/level_crossing_stamped', UInt8, self.cbLevelCrossingStamped, queue_size=1)
self.sub_tunnel_stamped = rospy.Subscriber('/detect/tunnel_stamped', UInt8, self.cbTunnelStamped, queue_size=1)
# 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_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.current_mode = self.CurrentMode.idle.value
self.InvokedObject = Enum('InvokedObject', 'traffic_light traffic_sign')
self.StepOfTrafficLight = Enum('StepOfTrafficLight', 'searching_traffic_light in_traffic_light')
self.StepOfIntersection = Enum('StepOfIntersection', 'detect_direction exit')
self.StepOfConstruction = Enum('StepOfConstruction', 'find_obstacle avoid_obstacle exit')
self.StepOfParking = Enum('StepOfParking', 'parking exit')
self.StepOfTunnel = Enum('StepOfTunnel', 'searching_tunnel_sign go_in_to_tunnel navigation go_out_from_tunnel exit')
self.current_step_traffic_light = self.StepOfTrafficLight.searching_traffic_light.value
self.current_step_intersection = self.StepOfIntersection.detect_direction.value
self.current_step_construction = self.StepOfConstruction.find_obstacle.value
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.uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
self.launch_camera_launched = False
self.launch_detect_sign_intersection_launched = False
self.launch_detect_sign_construction_launched = False
self.launch_detect_sign_parking_launched = False
self.launch_detect_sign_level_crossing_launched = False
self.launch_detect_sign_tunnel_launched = False
self.launch_detect_lane_launched = False
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_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
loop_rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
if self.is_triggered == True:
self.fnControlNode()
loop_rate.sleep()
def cbReceiveMode(self, mode_msg):
rospy.loginfo("node starts the progress with %d", mode_msg.data)
self.current_mode = mode_msg.data
self.is_triggered = True
def cbIntersectionStamped(self, intersection_msg):
rospy.loginfo("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
msg_mode_return = UInt8()
msg_mode_return.data = self.current_mode
self.pub_mode_return.publish(msg_mode_return)
self.is_triggered = True
def cbconstructionStamped(self, construction_msg):
rospy.loginfo("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
msg_mode_return = UInt8()
msg_mode_return.data = self.current_mode
self.pub_mode_return.publish(msg_mode_return)
self.is_triggered = True
# Which step is in Tunnel
def cbTunnelStamped(self, tunnel_msg):
rospy.loginfo("Tunnel Step changed from %d", self.current_step_tunnel)
self.current_step_tunnel = tunnel_msg.data
rospy.loginfo("into %d", self.current_step_tunnel)
if self.current_step_tunnel == self.StepOfTunnel.searching_tunnel_sign.value:
self.current_mode = self.CurrentMode.tunnel.value
msg_mode_return = UInt8
msg_mode_return.data = self.current_mode
self.pub_mode_return.publish(msg_mode_return)
self.is_triggered = True
def cbParkingStamped(self, parking_msg):
rospy.loginfo("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
msg_mode_return = UInt8()
msg_mode_return.data = self.current_mode
self.pub_mode_return.publish(msg_mode_return)
self.is_triggered = True
def cbLevelCrossingStamped(self, level_crossing_msg):
rospy.loginfo("LevelCrossing Step changed from %d", self.current_step_level_crossing)
self.current_step_level_crossing = level_crossing_msg.data
if self.current_step_level_crossing == self.StepOfLevelCrossing.pass_level.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)
self.is_triggered = True
def fnControlNode(self):
# lane_following
rospy.loginfo("node all_core_node_controller current_mode=%d",self.current_mode)
rospy.loginfo("node Current step traffic_light=%d",self.current_step_traffic_light)
rospy.loginfo("node Current step construction=%d",self.current_step_construction)
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:
rospy.loginfo("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_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_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")
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)
msg_pub_traffic_light_order.data = self.StepOfTrafficLight.in_traffic_light.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, 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_intersection.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("---------------------------------------------------")
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
self.pub_mode_return.publish(msg_mode_return)
rospy.sleep(2)
self.pub_traffic_light_order.publish(msg_pub_traffic_light_order)
# intersection
elif self.current_mode == self.CurrentMode.intersection.value:
rospy.loginfo("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")
msg_pub_intersection_order.data = self.StepOfIntersection.detect_direction.value
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_intersection.value, True)
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")
msg_pub_intersection_order.data = self.StepOfIntersection.exit.value
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_intersection.value, False)
self.fnLaunch(self.Launcher.launch_control_lane.value, True)
self.fnLaunch(self.Launcher.launch_control_moving.value, False)
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")
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")
msg_pub_construction_order.data = self.StepOfConstruction.find_obstacle.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_construction.value, False)
self.fnLaunch(self.Launcher.launch_detect_construction.value, True)
self.fnLaunch(self.Launcher.launch_control_lane.value, True)
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")
msg_pub_construction_order.data = self.StepOfConstruction.avoid_obstacle.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_construction.value, False)
self.fnLaunch(self.Launcher.launch_detect_construction.value, True)
self.fnLaunch(self.Launcher.launch_control_lane.value, False)
self.fnLaunch(self.Launcher.launch_control_moving.value, True)
elif self.current_step_construction == self.StepOfConstruction.exit.value:
rospy.loginfo("Current step : exit")
msg_pub_construction_order.data = self.StepOfConstruction.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_construction.value, True)
self.fnLaunch(self.Launcher.launch_detect_construction.value, False)
self.fnLaunch(self.Launcher.launch_control_lane.value, True)
self.fnLaunch(self.Launcher.launch_control_moving.value, False)
rospy.sleep(2)
self.pub_construction_order.publish(msg_pub_construction_order)
# parking
elif self.current_mode == self.CurrentMode.parking.value:
rospy.loginfo("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")
msg_pub_parking_order.data = self.StepOfParking.parking.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_parking.value, False)
self.fnLaunch(self.Launcher.launch_detect_parking.value, True)
self.fnLaunch(self.Launcher.launch_control_lane.value, False)
self.fnLaunch(self.Launcher.launch_control_moving.value, True)
elif self.current_step_parking == self.StepOfParking.exit.value:
rospy.loginfo("Current step : finish parking")
msg_pub_parking_order.data = self.StepOfParking.exit.value
self.fnLaunch(self.Launcher.launch_camera_ex_calib.value, True)
self.fnLaunch(self.Launcher.launch_detect_sign_parking.value, True)
self.fnLaunch(self.Launcher.launch_detect_lane.value, True)
self.fnLaunch(self.Launcher.launch_detect_parking.value, False)
self.fnLaunch(self.Launcher.launch_control_lane.value, True)
self.fnLaunch(self.Launcher.launch_control_moving.value, False)
rospy.sleep(2)
self.pub_parking_order.publish(msg_pub_parking_order)
# level_crossing
elif self.current_mode == self.CurrentMode.level_crossing.value:
rospy.loginfo("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")
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_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")
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_control_lane.value, True)
self.fnLaunch(self.Launcher.launch_control_moving.value, False)
self.pub_level_crossing_order.publish(msg_pub_level_crossing_order)
rospy.sleep(2)
# tunnel
elif self.current_mode == self.CurrentMode.tunnel.value:
rospy.loginfo("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")
msg_pub_tunnel_order.data = self.StepOfTunnel.go_in_to_tunnel.value
self.fnLaunch(self.Launcher.launch_camera_ex_calib.value, False)
self.fnLaunch(self.Launcher.launch_detect_lane.value, False)
self.fnLaunch(self.Launcher.launch_detect_tunnel.value, True)
self.fnLaunch(self.Launcher.launch_detect_sign_tunnel.value, False)
self.fnLaunch(self.Launcher.launch_control_lane.value, False)
self.fnLaunch(self.Launcher.launch_control_tunnel.value, False)
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")
msg_pub_tunnel_order.data = self.StepOfTunnel.navigation.value
self.fnLaunch(self.Launcher.launch_camera_ex_calib.value, False)
self.fnLaunch(self.Launcher.launch_detect_lane.value, False)
self.fnLaunch(self.Launcher.launch_detect_tunnel.value, True)
self.fnLaunch(self.Launcher.launch_detect_sign_tunnel.value, False)
self.fnLaunch(self.Launcher.launch_control_lane.value, False)
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")
msg_pub_tunnel_order.data = self.StepOfTunnel.go_out_from_tunnel.value
self.fnLaunch(self.Launcher.launch_camera_ex_calib.value, True)
self.fnLaunch(self.Launcher.launch_detect_sign_tunnel.value, False)
self.fnLaunch(self.Launcher.launch_detect_lane.value, False)
self.fnLaunch(self.Launcher.launch_detect_tunnel.value, True)
self.fnLaunch(self.Launcher.launch_control_lane.value, False)
self.fnLaunch(self.Launcher.launch_control_tunnel.value, False)
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")
msg_pub_tunnel_order.data = self.StepOfTunnel.exit.value
self.fnLaunch(self.Launcher.launch_camera_ex_calib.value, True)
self.fnLaunch(self.Launcher.launch_detect_tunnel.value, False)
self.fnLaunch(self.Launcher.launch_detect_lane.value, True)
self.fnLaunch(self.Launcher.launch_detect_sign_tunnel.value, True)
self.fnLaunch(self.Launcher.launch_control_lane.value, True)
self.fnLaunch(self.Launcher.launch_control_tunnel.value, False)
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")
msg_pub_tunnel_order.data = self.StepOfTunnel.searching_tunneROSLaunchParentvalue
self.fnLaunch(self.Launcher.launch_camera_ex_calib.value, True)
self.fnLaunch(self.Launcher.launch_detect_tunnel.value, False)
self.fnLaunch(self.Launcher.launch_detect_lane.value, True)
self.fnLaunch(self.Launcher.launch_detect_sign_tunnel.value, True)
self.fnLaunch(self.Launcher.launch_control_lane.value, True)
self.fnLaunch(self.Launcher.launch_control_tunnel.value, False)
self.fnLaunch(self.Launcher.launch_control_moving.value, False)
rospy.sleep(2)
self.pub_tunnel_order.publish(msg_pub_tunnel_order)
self.is_triggered = False
def fnLaunch(self, launch_num, is_start):
if launch_num == self.Launcher.launch_camera_ex_calib.value:
if is_start == True:
if self.launch_camera_launched == False:
self.launch_camera = roslaunch.scriptapi.ROSLaunch()
self.launch_camera = roslaunch.parent.ROSLaunchParent(self.uuid, [self.ros_package_path + "turtlebot3_autorace_camera/launch/extrinsic_camera_calibration.launch"])
self.launch_camera_launched = True
self.launch_camera.start()
else:
pass
else:
if self.launch_camera_launched == True:
self.launch_camera_launched = False
self.launch_camera.shutdown()
else:
pass
elif launch_num == self.Launcher.launch_detect_lane.value:
if is_start == True:
if self.launch_detect_lane_launched == False:
self.launch_detect_lane = roslaunch.scriptapi.ROSLaunch()
self.launch_detect_lane = roslaunch.parent.ROSLaunchParent(self.uuid, [self.ros_package_path + "turtlebot3_autorace_detect/launch/detect_lane.launch"])
self.launch_detect_lane_launched = True
self.launch_detect_lane.start()
else:
pass
else:
if self.launch_detect_lane_launched == True:
self.launch_detect_lane_launched = False
self.launch_detect_lane.shutdown()
else:
pass
elif launch_num == self.Launcher.launch_detect_sign_intersection.value:
if is_start == True:
if self.launch_detect_sign_intersection_launched == False:
self.launch_detect_sign_intersection = roslaunch.scriptapi.ROSLaunch()
self.launch_detect_sign_intersection = roslaunch.parent.ROSLaunchParent(self.uuid, [self.ros_package_path + "turtlebot3_autorace_detect/launch/detect_sign_intersection.launch"])
self.launch_detect_sign_intersection_launched = True
self.launch_detect_sign_intersection.start()
else:
pass
else:
if self.launch_detect_sign_intersection_launched == True:
self.launch_detect_sign_intersection_launched = False
self.launch_detect_sign_intersection.shutdown()
else:
pass
elif launch_num == self.Launcher.launch_detect_sign_construction.value:
if is_start == True:
if self.launch_detect_sign_construction_launched == False:
self.launch_detect_sign_construction = roslaunch.scriptapi.ROSLaunch()
self.launch_detect_sign_construction = roslaunch.parent.ROSLaunchParent(self.uuid, [self.ros_package_path + "turtlebot3_autorace_detect/launch/detect_sign_construction.launch"])
self.launch_detect_sign_construction_launched = True
self.launch_detect_sign_construction.start()
else:
pass
else:
if self.launch_detect_sign_construction_launched == True:
self.launch_detect_sign_construction_launched = False
self.launch_detect_sign_construction.shutdown()
else:
pass
elif launch_num == self.Launcher.launch_detect_sign_parking.value:
if is_start == True:
if self.launch_detect_sign_parking_launched == False:
self.launch_detect_sign_parking = roslaunch.scriptapi.ROSLaunch()
self.launch_detect_sign_parking = roslaunch.parent.ROSLaunchParent(self.uuid, [self.ros_package_path + "turtlebot3_autorace_detect/launch/detect_sign_parking.launch"])
self.launch_detect_sign_parking_launched = True
self.launch_detect_sign_parking.start()
else:
pass
else:
if self.launch_detect_sign_parking_launched == True:
self.launch_detect_sign_parking_launched = False
self.launch_detect_sign_parking.shutdown()
else:
pass
elif launch_num == self.Launcher.launch_detect_sign_level_crossing.value:
if is_start == True:
if self.launch_detect_sign_level_crossing_launched == False:
self.launch_detect_sign_level_crossing = roslaunch.scriptapi.ROSLaunch()
self.launch_detect_sign_level_crossing = roslaunch.parent.ROSLaunchParent(self.uuid, [self.ros_package_path + "turtlebot3_autorace_detect/launch/detect_sign_level_crossing.launch"])
self.launch_detect_sign_level_crossing_launched = True
self.launch_detect_sign_level_crossing.start()
else:
pass
else:
if self.launch_detect_sign_level_crossing_launched == True:
self.launch_detect_sign_level_crossing_launched = False
self.launch_detect_sign_level_crossing.shutdown()
else:
pass
elif launch_num == self.Launcher.launch_detect_sign_tunnel.value:
if is_start == True:
if self.launch_detect_sign_tunnel_launched == False:
self.launch_detect_sign_tunnel = roslaunch.scriptapi.ROSLaunch()
self.launch_detect_sign_tunnel = roslaunch.parent.ROSLaunchParent(self.uuid, [self.ros_package_path + "turtlebot3_autorace_detect/launch/detect_sign_tunnel.launch"])
self.launch_detect_sign_tunnel_launched = True
self.launch_detect_sign_tunnel.start()
else:
pass
else:
if self.launch_detect_sign_tunnel_launched == True:
self.launch_detect_sign_tunnel_launched = False
self.launch_detect_sign_tunnel.shutdown()
else:
pass
elif launch_num == self.Launcher.launch_detect_traffic_light.value:
if is_start == True:
if self.launch_detect_traffic_light_launched == False:
self.launch_detect_traffic_light = roslaunch.scriptapi.ROSLaunch()
self.launch_detect_traffic_light = roslaunch.parent.ROSLaunchParent(self.uuid, [self.ros_package_path + "turtlebot3_autorace_detect/launch/detect_traffic_light.launch"])
self.launch_detect_traffic_light_launched = True
self.launch_detect_traffic_light.start()
else:
pass
else:
if self.launch_detect_traffic_light_launched == True:
self.launch_detect_traffic_light_launched = False
self.launch_detect_traffic_light.shutdown()
else:
pass
elif launch_num == self.Launcher.launch_detect_intersection.value:
if is_start == True:
if self.launch_detect_intersection_launched == False:
self.launch_detect_intersection = roslaunch.scriptapi.ROSLaunch()
self.launch_detect_intersection = roslaunch.parent.ROSLaunchParent(self.uuid, [self.ros_package_path + "turtlebot3_autorace_detect/launch/detect_intersection.launch"])
self.launch_detect_intersection_launched = True
self.launch_detect_intersection.start()
else:
pass
else:
if self.launch_detect_intersection_launched == True:
self.launch_detect_intersection_launched = False
self.launch_detect_intersection.shutdown()
pass
elif launch_num == self.Launcher.launch_detect_construction.value:
if is_start == True:
if self.launch_detect_construction_launched == False:
self.launch_detect_construction = roslaunch.scriptapi.ROSLaunch()
self.launch_detect_construction = roslaunch.parent.ROSLaunchParent(self.uuid, [self.ros_package_path + "turtlebot3_autorace_detect/launch/detect_construction.launch"])
self.launch_detect_construction_launched = True
self.launch_detect_construction.start()
else:
pass
else:
if self.launch_detect_construction_launched == True:
self.launch_detect_construction_launched = False
self.launch_detect_construction.shutdown()
pass
elif launch_num == self.Launcher.launch_detect_level.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()
else:
pass
else:
if self.launch_detect_level_launched == True:
self.launch_detect_level_launched = False
self.launch_detect_level.shutdown()
else:
pass
elif launch_num == self.Launcher.launch_control_lane.value:
if is_start == True:
if self.launch_control_lane_launched == False:
self.launch_control_lane = roslaunch.scriptapi.ROSLaunch()
self.launch_control_lane = roslaunch.parent.ROSLaunchParent(self.uuid, [self.ros_package_path + "turtlebot3_autorace_driving/launch/turtlebot3_autorace_control_lane.launch"])
self.launch_control_lane_launched = True
self.launch_control_lane.start()
else:
pass
else:
if self.launch_control_lane_launched == True:
self.launch_control_lane_launched = False
self.launch_control_lane.shutdown()
else:
pass
elif launch_num == self.Launcher.launch_detect_parking.value:
if is_start == True:
if self.launch_detect_parking_launched == False:
self.launch_detect_parking = roslaunch.scriptapi.ROSLaunch()
self.launch_detect_parking = roslaunch.parent.ROSLaunchParent(self.uuid, [self.ros_package_path + "turtlebot3_autorace_detect/launch/detect_parking.launch"])
self.launch_detect_parking_launched = True
self.launch_detect_parking.start()
else:
pass
else:
if self.launch_detect_parking_launched == True:
self.launch_detect_parking_launched = False
self.launch_detect_parking.shutdown()
else:
pass
elif launch_num == self.Launcher.launch_detect_tunnel.value:
if is_start == True:
if self.launch_detect_tunnel_launched == False:
self.launch_detect_tunnel = roslaunch.scriptapi.ROSLaunch()
self.launch_detect_tunnel = roslaunch.parent.ROSLaunchParent(self.uuid, [self.ros_package_path + "turtlebot3_autorace_detect/launch/detect_tunnel.launch"])
self.launch_detect_tunnel_launched = True
self.launch_detect_tunnel.start()
else:
pass
else:
if self.launch_detect_tunnel_launched == True:
self.launch_detect_tunnel_launched = False
self.launch_detect_tunnel.shutdown()
else:
pass
elif launch_num == self.Launcher.launch_control_moving.value:
if is_start == True:
if self.launch_control_moving_launched == False:
self.launch_control_moving = roslaunch.scriptapi.ROSLaunch()
self.launch_control_moving = roslaunch.parent.ROSLaunchParent(self.uuid, [self.ros_package_path + "turtlebot3_autorace_driving/launch/turtlebot3_autorace_control_moving.launch"])
self.launch_control_moving_launched = True
self.launch_control_moving.start()
else:
pass
else:
if self.launch_control_moving_launched == True:
self.launch_control_moving_launched = False
self.launch_control_moving.shutdown()
pass
elif launch_num == self.Launcher.launch_control_tunnel.value:
if is_start == True:
if self.launch_control_tunnel_launched == False:
self.launch_control_tunnel = roslaunch.scriptapi.ROSLaunch()
self.launch_control_tunnel = roslaunch.parent.ROSLaunchParent(self.uuid, [self.ros_package_path + "turtlebot3_autorace_driving/launch/turtlebot3_autorace_control_tunnel.launch"])
self.launch_control_tunnel_launched = True
self.launch_control_tunnel.start()
else:
pass
else:
if self.launch_control_tunnel_launched == True:
self.launch_control_tunnel_launched = False
self.launch_control_tunnel.shutdown()
else:
pass
def main(self):
rospy.spin()
if __name__ == '__main__':
rospy.init_node('core_node_controller')
node = CoreNodeController()
node.main()