def test_marker(self):
"""
Tests marker
"""
try:
node = None
ros_init()
node = CompatibleNode('test_node')
msg = node.wait_for_one_message("/carla/markers",
MarkerArray,
timeout=TIMEOUT)
self.assertEqual(len(msg.markers), 1) # only ego vehicle exists
ego_marker = msg.markers[0]
self.assertEqual(ego_marker.header.frame_id, "map")
self.assertNotEqual(ego_marker.id, 0)
self.assertEqual(ego_marker.type, 1)
self.assertNotEqual(ego_marker.pose, Pose())
self.assertNotEqual(ego_marker.scale, Vector3())
self.assertEqual(ego_marker.color.r, 0.0)
self.assertEqual(ego_marker.color.g, 255.0)
self.assertEqual(ego_marker.color.b, 0.0)
finally:
if node is not None:
node.destroy_node()
ros_shutdown()
def test_vehicle_info(self):
"""
Tests vehicle_info
"""
try:
node = None
ros_init()
node = CompatibleNode('test_node')
msg = node.wait_for_one_message("/carla/ego_vehicle/vehicle_info",
CarlaEgoVehicleInfo,
timeout=TIMEOUT,
qos_profile=QoSProfile(
depth=1, durability=latch_on))
self.assertNotEqual(msg.id, 0)
self.assertEqual(msg.type, "vehicle.tesla.model3")
self.assertEqual(msg.rolename, "ego_vehicle")
self.assertEqual(len(msg.wheels), 4)
self.assertNotEqual(msg.max_rpm, 0.0)
self.assertNotEqual(msg.moi, 0.0)
self.assertNotEqual(msg.damping_rate_full_throttle, 0.0)
self.assertNotEqual(msg.damping_rate_zero_throttle_clutch_engaged,
0.0)
self.assertNotEqual(
msg.damping_rate_zero_throttle_clutch_disengaged, 0.0)
self.assertTrue(msg.use_gear_autobox)
self.assertNotEqual(msg.gear_switch_time, 0.0)
self.assertNotEqual(msg.mass, 0.0)
self.assertNotEqual(msg.clutch_strength, 0.0)
self.assertNotEqual(msg.drag_coefficient, 0.0)
self.assertNotEqual(msg.center_of_mass, Vector3())
finally:
if node is not None:
node.destroy_node()
ros_shutdown()
def test_clock(self):
"""
Tests clock
"""
try:
node = None
ros_init()
node = CompatibleNode('test_node')
msg = node.wait_for_one_message("/clock", Clock, timeout=TIMEOUT)
self.assertNotEqual(Clock(), msg)
finally:
if node is not None:
node.destroy_node()
ros_shutdown()
def test_actor_list(self):
"""
Tests actor_list
"""
try:
node = None
ros_init()
node = CompatibleNode('test_node')
msg = node.wait_for_one_message("/carla/actor_list",
CarlaActorList,
timeout=TIMEOUT)
self.assertNotEqual(len(msg.actors), 0)
finally:
if node is not None:
node.destroy_node()
ros_shutdown()
def test_lidar(self):
"""
Tests Lidar sensor node
"""
try:
node = None
ros_init()
node = CompatibleNode('test_node')
msg = node.wait_for_one_message("/carla/ego_vehicle/lidar",
PointCloud2,
timeout=TIMEOUT)
self.assertEqual(msg.header.frame_id, "ego_vehicle/lidar")
finally:
if node is not None:
node.destroy_node()
ros_shutdown()
def test_dvs_camera_events(self):
"""
Tests dvs camera events
"""
try:
node = None
ros_init()
node = CompatibleNode('test_node')
msg = node.wait_for_message("/carla/ego_vehicle/dvs_front/events",
PointCloud2,
timeout=TIMEOUT)
self.assertEqual(msg.header.frame_id, "ego_vehicle/dvs_front")
finally:
if node is not None:
node.destroy_node()
ros_shutdown()
def test_ego_vehicle_objects(self):
"""
Tests objects node for ego_vehicle
"""
try:
node = None
ros_init()
node = CompatibleNode('test_node')
msg = node.wait_for_one_message("/carla/ego_vehicle/objects",
ObjectArray,
timeout=15)
self.assertEqual(msg.header.frame_id, "map")
self.assertEqual(len(msg.objects), 0)
finally:
if node is not None:
node.destroy_node()
ros_shutdown()
def test_objects(self):
"""
Tests carla objects
"""
try:
node = None
ros_init()
node = CompatibleNode('test_node')
msg = node.wait_for_one_message("/carla/objects",
ObjectArray,
timeout=TIMEOUT)
self.assertEqual(msg.header.frame_id, "map")
self.assertEqual(len(msg.objects), 1) # only ego vehicle exists
finally:
if node is not None:
node.destroy_node()
ros_shutdown()
def test_vehicle_status(self, proc_output):
"""
Tests vehicle_status
"""
try:
node = None
ros_init()
node = CompatibleNode('test_node')
msg = node.wait_for_one_message(
"/carla/ego_vehicle/vehicle_status", CarlaEgoVehicleStatus)
self.assertNotEqual(msg.header, Header())
self.assertEqual(msg.header.frame_id, 'map')
self.assertNotEqual(msg.orientation, Quaternion())
finally:
if node is not None:
node.destroy_node()
ros_shutdown()
def test_odometry(self):
"""
Tests Odometry
"""
try:
node = None
ros_init()
node = CompatibleNode('test_node')
msg = node.wait_for_one_message("/carla/ego_vehicle/odometry",
Odometry,
timeout=TIMEOUT)
self.assertEqual(msg.header.frame_id, "map")
self.assertEqual(msg.child_frame_id, "ego_vehicle")
self.assertNotEqual(msg.pose, Pose())
finally:
if node is not None:
node.destroy_node()
ros_shutdown()
def test_dvs_camera_image(self):
"""
Tests dvs camera images
"""
try:
node = None
ros_init()
node = CompatibleNode('test_node')
msg = node.wait_for_one_message(
"/carla/ego_vehicle/dvs_front/image", Image, timeout=TIMEOUT)
self.assertEqual(msg.header.frame_id, "ego_vehicle/dvs_front")
self.assertEqual(msg.height, 70)
self.assertEqual(msg.width, 400)
self.assertEqual(msg.encoding, "bgr8")
finally:
if node is not None:
node.destroy_node()
ros_shutdown()
def test_traffic_lights_info(self):
"""
Tests traffic_lights
"""
try:
node = None
ros_init()
node = CompatibleNode('test_node')
msg = node.wait_for_one_message("/carla/traffic_lights/info",
CarlaTrafficLightInfoList,
timeout=TIMEOUT,
qos_profile=QoSProfile(
depth=10, durability=latch_on))
self.assertNotEqual(len(msg.traffic_lights), 0)
finally:
if node is not None:
node.destroy_node()
ros_shutdown()
def test_map(self):
"""
Tests map
"""
try:
node = None
ros_init()
node = CompatibleNode('test_node')
msg = node.wait_for_one_message("/carla/map",
String,
timeout=TIMEOUT,
qos_profile=QoSProfile(
depth=10, durability=latch_on))
self.assertNotEqual(len(msg.data), 0)
finally:
if node is not None:
node.destroy_node()
ros_shutdown()
def test_world_info(self):
"""
Tests world_info
"""
try:
node = None
ros_init()
node = CompatibleNode('test_node')
msg = node.wait_for_one_message("/carla/world_info",
CarlaWorldInfo,
timeout=TIMEOUT,
qos_profile=QoSProfile(
depth=10, durability=latch_on))
self.assertNotEqual(len(msg.map_name), 0)
self.assertNotEqual(len(msg.opendrive), 0)
finally:
if node is not None:
node.destroy_node()
ros_shutdown()
def test_gnss(self):
"""
Tests Gnss
"""
try:
node = None
ros_init()
node = CompatibleNode('test_node')
msg = node.wait_for_one_message("/carla/ego_vehicle/gnss",
NavSatFix,
timeout=TIMEOUT)
self.assertEqual(msg.header.frame_id, "ego_vehicle/gnss")
self.assertNotEqual(msg.latitude, 0.0)
self.assertNotEqual(msg.longitude, 0.0)
self.assertNotEqual(msg.altitude, 0.0)
finally:
if node is not None:
node.destroy_node()
ros_shutdown()
def test_imu(self):
"""
Tests IMU sensor node
"""
try:
node = None
ros_init()
node = CompatibleNode('test_node')
msg = node.wait_for_one_message("/carla/ego_vehicle/imu",
Imu,
timeout=TIMEOUT)
self.assertEqual(msg.header.frame_id, "ego_vehicle/imu")
self.assertNotEqual(msg.linear_acceleration, 0.0)
self.assertNotEqual(msg.angular_velocity, 0.0)
self.assertNotEqual(msg.orientation, 0.0)
finally:
if node is not None:
node.destroy_node()
ros_shutdown()
def test_camera_info(self):
"""
Tests camera_info
"""
try:
node = None
ros_init()
node = CompatibleNode('test_node')
msg = node.wait_for_one_message(
"/carla/ego_vehicle/rgb_front/camera_info",
CameraInfo,
timeout=TIMEOUT)
self.assertEqual(msg.header.frame_id, "ego_vehicle/rgb_front")
self.assertEqual(msg.height, 600)
self.assertEqual(msg.width, 800)
finally:
if node is not None:
node.destroy_node()
ros_shutdown()
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))
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
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()
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)
