class RosVehicleControl(BasicControl):
def __init__(self, actor, args=None):
super(RosVehicleControl, self).__init__(actor)
ros_init(args=None)
self._carla_actor = actor
self._role_name = actor.attributes["role_name"]
if not self._role_name:
if ROS_VERSION == 1:
rospy.logerr("Invalid role_name!")
elif ROS_VERSION == 2:
logging.get_logger("pre_logger").error("Invalid role_name!")
self._path_topic_name = "waypoints"
if "path_topic_name" in args:
self._path_topic_name = args["path_topic_name"]
self.node = CompatibleNode('ros_agent_{}'.format(self._role_name))
self._current_target_speed = None
self._current_path = None
self.controller_launch = None
self._target_speed_publisher = self.node.new_publisher(
Float64, "/carla/{}/target_speed".format(self._role_name),
QoSProfile(depth=10, durability=latch_on))
self.node.loginfo(
"Publishing target_speed on /carla/{}/target_speed".format(
self._role_name))
self._path_publisher = self.node.new_publisher(
Path, "/carla/{}/{}".format(self._role_name,
self._path_topic_name),
QoSProfile(depth=10, durability=latch_on))
self.node.loginfo("Publishing path on /carla/{}/{}".format(
self._role_name, self._path_topic_name))
if "launch" in args and "launch-package" in args:
launch_file = args["launch"]
launch_package = args["launch-package"]
if ROS_VERSION == 1:
cli_args = [launch_package, launch_file]
elif ROS_VERSION == 2:
cli_args = [launch_package, launch_file + '.py']
cli_args.append('role_name:={}'.format(self._role_name))
# add additional launch parameters
launch_parameters = []
for key, value in args.items():
if not key == "launch" and not key == "launch-package" and not key == "path_topic_name":
launch_parameters.append('{}:={}'.format(key, value))
cli_args.append('{}:={}'.format(key, value))
self.node.loginfo(
"{}: Launching {} from package {} (parameters: {})...".format(
self._role_name, launch_file, launch_package,
launch_parameters))
if ROS_VERSION == 1:
uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
roslaunch.configure_logging(uuid)
roslaunch_file = roslaunch.rlutil.resolve_launch_arguments(
cli_args)
roslaunch_args = cli_args[2:]
launch_files = [(roslaunch_file[0], roslaunch_args)]
parent = roslaunch.parent.ROSLaunchParent(uuid, launch_files)
parent.start()
elif ROS_VERSION == 2:
cmdline = ['ros2 launch'] + cli_args
self.controller_launch = ApplicationRunner(
self.controller_runner_status_updated,
self.controller_runner_log,
'RosVehicleControl: launching controller node')
self.controller_launch.execute(
cmdline,
env=os.environ,
)
self.node.loginfo(
"{}: Successfully started ros vehicle control".format(
self._role_name))
else:
self.node.logwarn(
"{}: Missing value for 'launch' and/or 'launch-package'.".
format(self._role_name))
def controller_runner_log(self, log): # pylint: disable=no-self-use
"""
Callback for application logs
"""
self.node.logwarn("[Controller]{}".format(log))
def controller_runner_status_updated(self, status):
"""
Executed from application runner whenever the status changed
"""
self.node.loginfo("Controller status is: {}".format(status))
def update_target_speed(self, speed):
super(RosVehicleControl, self).update_target_speed(speed)
self.node.loginfo("{}: Target speed changed to {}".format(
self._role_name, speed))
self._target_speed_publisher.publish(Float64(data=speed))
def update_waypoints(self, waypoints, start_time=None):
super(RosVehicleControl, self).update_waypoints(waypoints, start_time)
self.node.loginfo("{}: Waypoints changed.".format(self._role_name))
path = Path()
path.header.stamp = ros_timestamp(sec=self.node.get_time(),
from_sec=True)
path.header.frame_id = "map"
for wpt in waypoints:
print(wpt)
path.poses.append(
PoseStamped(pose=trans.carla_transform_to_ros_pose(wpt)))
self._path_publisher.publish(path)
def reset(self):
# set target speed to zero before closing as the controller can take time to shutdown
if ROS_VERSION == 2:
self.update_target_speed(0.)
if self.controller_launch and self.controller_launch.is_running():
self.controller_launch.shutdown()
if self._carla_actor and self._carla_actor.is_alive:
self._carla_actor = None
if self._target_speed_publisher:
self.node.destroy_subscription(self._target_speed_publisher)
self._target_speed_publisher = None
if self._path_publisher:
self.node.destroy_subscription(self._path_publisher)
self._path_publisher = None
def run_step(self):
pass
class CarlaControlPlugin(Plugin):
"""
RQT Plugin to control CARLA
"""
def __init__(self, context):
"""
Constructor
"""
super(CarlaControlPlugin, self).__init__(context)
self.setObjectName('CARLA Control')
self._widget = QWidget()
self._node = CompatibleNode('rqt_carla_control_node', rospy_init=False)
if ROS_VERSION == 1:
package_share_dir = rospkg.RosPack().get_path('rqt_carla_control')
self.callback_group = None
elif ROS_VERSION == 2:
package_share_dir = get_package_share_directory(
'rqt_carla_control')
self.callback_group = ReentrantCallbackGroup()
ui_file = os.path.join(package_share_dir, 'resource',
'CarlaControl.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('CarlaControl')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
self.pause_icon = QIcon(
QPixmap(os.path.join(package_share_dir, 'resource', 'pause.png')))
self.play_icon = QIcon(
QPixmap(os.path.join(package_share_dir, 'resource', 'play.png')))
self._widget.pushButtonStepOnce.setIcon(
QIcon(
QPixmap(
os.path.join(package_share_dir, 'resource',
'step_once.png'))))
self.carla_status = None
self.carla_status_subscriber = self._node.create_subscriber(
CarlaStatus,
"/carla/status",
self.carla_status_changed,
callback_group=self.callback_group)
self.carla_control_publisher = self._node.new_publisher(
CarlaControl, "/carla/control",
QoSProfile(depth=10, durability=False))
self._widget.pushButtonPlayPause.setDisabled(True)
self._widget.pushButtonStepOnce.setDisabled(True)
self._widget.pushButtonPlayPause.setIcon(self.play_icon)
self._widget.pushButtonPlayPause.clicked.connect(
self.toggle_play_pause)
self._widget.pushButtonStepOnce.clicked.connect(self.step_once)
context.add_widget(self._widget)
if ROS_VERSION == 2:
spin_thread = threading.Thread(target=self._node.spin, daemon=True)
spin_thread.start()
def toggle_play_pause(self):
"""
toggle play/pause
"""
if self.carla_status.synchronous_mode:
if self.carla_status.synchronous_mode_running:
self.carla_control_publisher.publish(
CarlaControl(command=CarlaControl.PAUSE))
else:
self.carla_control_publisher.publish(
CarlaControl(command=CarlaControl.PLAY))
def step_once(self):
"""
execute one step
"""
self.carla_control_publisher.publish(
CarlaControl(command=CarlaControl.STEP_ONCE))
def carla_status_changed(self, status):
"""
callback whenever carla status changes
"""
self.carla_status = status
if status.synchronous_mode:
self._widget.pushButtonPlayPause.setDisabled(False)
self._widget.pushButtonStepOnce.setDisabled(False)
if status.synchronous_mode_running:
self._widget.pushButtonPlayPause.setIcon(self.pause_icon)
else:
self._widget.pushButtonPlayPause.setIcon(self.play_icon)
else:
self._widget.pushButtonPlayPause.setDisabled(True)
self._widget.pushButtonStepOnce.setDisabled(True)
def shutdown_plugin(self):
"""
shutdown plugin
"""
self._node.destroy_subscription(self.carla_control_publisher)
