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')
package_share_dir = roscomp.get_package_share_directory(
'rqt_carla_control')
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.new_subscription(
CarlaStatus,
"/carla/status",
self.carla_status_changed,
qos_profile=10)
self.carla_control_publisher = self._node.new_publisher(
CarlaControl, "/carla/control", qos_profile=10)
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 roscomp.get_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)
class RosVehicleControl(BasicControl):
def __init__(self, actor, args=None):
super(RosVehicleControl, self).__init__(actor)
self._carla_actor = actor
self._role_name = actor.attributes["role_name"]
if not self._role_name:
roscomp.logerr("Invalid role_name")
self._path_topic_name = "waypoints"
if "path_topic_name" in args:
self._path_topic_name = args["path_topic_name"]
roscomp.init("ros_agent_{}".format(self._role_name), args=None)
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=DurabilityPolicy.TRANSIENT_LOCAL))
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=DurabilityPolicy.TRANSIENT_LOCAL))
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:
executable = "roslaunch"
cli_args = [launch_package, launch_file]
elif ROS_VERSION == 2:
executable = "ros2 launch"
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))
cmdline = [executable] + 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 = roscomp.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
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