From 357c535ecd8faa9939804f311c0f3703f405223d Mon Sep 17 00:00:00 2001 From: Bernd Gassmann Date: Tue, 30 Jul 2024 16:01:24 +0200 Subject: [PATCH] Split CarlaVehicleStatus into Speed, Odometry, CarlaVehicleControlStatus This provides the pseudo.speedometer and pseudo.odom sensors for better backward compatibility and usability. --- carla_ad_agent/src/carla_ad_agent/ad_agent.py | 14 ++--- .../src/carla_ad_agent/local_planner.py | 31 +++++----- .../carla_manual_control.py | 62 +++++++++++-------- carla_msgs | 2 +- docs/images/ad_demo.dot | 30 ++++----- docs/ros_msgs.md | 13 ++-- docs/run_ros.md | 2 +- docs/rviz_plugin.md | 14 ++--- .../src/carla_control_panel_ROS2.cpp | 20 ++++-- .../src/carla_control_panel_ROS2.h | 27 ++++---- 10 files changed, 119 insertions(+), 96 deletions(-) diff --git a/carla_ad_agent/src/carla_ad_agent/ad_agent.py b/carla_ad_agent/src/carla_ad_agent/ad_agent.py index f954ad70..827bb5aa 100755 --- a/carla_ad_agent/src/carla_ad_agent/ad_agent.py +++ b/carla_ad_agent/src/carla_ad_agent/ad_agent.py @@ -22,11 +22,11 @@ from carla_msgs.msg import ( CarlaVehicleInfo, - CarlaVehicleStatus, CarlaActorList, CarlaTrafficLightStatusList, CarlaTrafficLightInfoList) from derived_object_msgs.msg import ObjectArray +from nav_msgs.msg import Odometry from std_msgs.msg import Float64 # pylint: disable=import-error @@ -57,10 +57,10 @@ def __init__(self): Float64, "/carla/vehicles/{}/speed_command".format(role_name), QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL)) - self._vehicle_status_subscriber = self.new_subscription( - CarlaVehicleStatus, - "/carla/vehicles/{}/vehicle_status".format(role_name), - self.vehicle_status_cb, + self._odometry_subscriber = self.new_subscription( + Odometry, + "/carla/{}/odometry".format(role_name), + self.odometry_cb, qos_profile=10 ) @@ -104,9 +104,9 @@ def vehicle_info_cb(self, vehicle_info_msg): self._ego_vehicle_id = vehicle_info_msg.id self._objects.pop(self._ego_vehicle_id) - def vehicle_status_cb(self, vehicle_status_msg): + def odometry_cb(self, odometry_msg): with self.data_lock: - self._ego_vehicle_pose = vehicle_status_msg.pose + self._ego_vehicle_pose = odometry_msg.pose.pose def target_speed_cb(self, target_speed_msg): with self.data_lock: diff --git a/carla_ad_agent/src/carla_ad_agent/local_planner.py b/carla_ad_agent/src/carla_ad_agent/local_planner.py index e69c5f85..fa60b737 100755 --- a/carla_ad_agent/src/carla_ad_agent/local_planner.py +++ b/carla_ad_agent/src/carla_ad_agent/local_planner.py @@ -21,8 +21,8 @@ from carla_ad_agent.vehicle_pid_controller import VehiclePIDController from carla_ad_agent.misc import distance_vehicle -from carla_msgs.msg import CarlaVehicleControl, CarlaVehicleStatus # pylint: disable=import-error -from nav_msgs.msg import Path +from carla_msgs.msg import CarlaVehicleControl # pylint: disable=import-error +from nav_msgs.msg import Odometry, Path from std_msgs.msg import Float64 from visualization_msgs.msg import Marker @@ -69,10 +69,10 @@ def __init__(self): self._waypoint_buffer = collections.deque(maxlen=self._buffer_size) # subscribers - self._vehicle_status_subscriber = self.new_subscription( - CarlaVehicleStatus, - "/carla/vehicles/{}/vehicle_status".format(role_name), - self.vehicle_status_cb, + self._odometry_subscriber = self.new_subscription( + Odometry, + "/carla/vehicles/{}/odometry".format(role_name), + self.odometry_cb, QoSProfileSubscriber(depth=10)) self._path_subscriber = self.new_subscription( Path, @@ -99,24 +99,27 @@ def __init__(self): self._vehicle_controller = VehiclePIDController( self, args_lateral=args_lateral_dict, args_longitudinal=args_longitudinal_dict) - def vehicle_status_cb(self, vehicle_status_msg): + def odometry_cb(self, odometry_msg): with self.data_lock: - self._current_header = vehicle_status_msg.header - self._current_header.frame_id = vehicle_status_msg.child_frame_id - self._current_pose = vehicle_status_msg.pose - self._current_speed = math.sqrt(vehicle_status_msg.twist.linear.x ** 2 + - vehicle_status_msg.twist.linear.y ** 2 + - vehicle_status_msg.twist.linear.z ** 2) * 3.6 + self._current_header = odometry_msg.header + self._current_header.frame_id = odometry_msg.child_frame_id + self._current_pose = odometry_msg.pose.pose + self._current_speed = math.sqrt(odometry_msg.twist.twist.linear.x ** 2 + + odometry_msg.twist.twist.linear.y ** 2 + + odometry_msg.twist.twist.linear.z ** 2) * 3.6 + self.loginfo("Receiving odometry: speed={}".format(self._current_speed)) def target_speed_cb(self, target_speed_msg): with self.data_lock: self._target_speed = target_speed_msg.data + self.loginfo("Receiving target_speed: target_speed={}".format(self._target_speed)) def path_cb(self, path_msg): with self.data_lock: self._waypoint_buffer.clear() self._waypoints_queue.clear() self._waypoints_queue.extend([pose.pose for pose in path_msg.poses]) + self.loginfo("Receiving route: resulting queue {}".format(self._waypoints_queue)) def pose_to_marker_msg(self, pose): marker_msg = Marker() @@ -142,7 +145,7 @@ def run_step(self): return if (self._current_pose is None) or (self._current_speed is None) or (self._current_header is None): - self.loginfo("Waiting for first vehicle_status message...") + self.loginfo("Waiting for first odometry message...") self.emergency_stop() return diff --git a/carla_manual_control/src/carla_manual_control/carla_manual_control.py b/carla_manual_control/src/carla_manual_control/carla_manual_control.py index f4d6c7f9..58e3443c 100755 --- a/carla_manual_control/src/carla_manual_control/carla_manual_control.py +++ b/carla_manual_control/src/carla_manual_control/carla_manual_control.py @@ -62,11 +62,11 @@ import ros_compatibility as roscomp from ros_compatibility.node import CompatibleNode -from ros_compatibility.qos import QoSProfile, DurabilityPolicy +from ros_compatibility.qos import QoSProfile, DurabilityPolicy, QoSProfileSubscriber from carla_msgs.msg import CarlaStatus from carla_msgs.msg import CarlaVehicleInfo -from carla_msgs.msg import CarlaVehicleStatus +from carla_msgs.msg import CarlaVehicleControlStatus from carla_msgs.msg import CarlaVehicleControl from carla_msgs.msg import CarlaLaneInvasionEvent from carla_msgs.msg import CarlaCollisionEvent @@ -314,11 +314,16 @@ def __init__(self, role_name, width, height, node): self.help = HelpText(pygame.font.Font(mono, 24), width, height) self._show_info = True self._info_text = [] - self.vehicle_status = CarlaVehicleStatus() + self.vehicle_control_status = CarlaVehicleControlStatus() - self.vehicle_status_subscriber = node.new_subscription( - CarlaVehicleStatus, "/carla/vehicles/{}/vehicle_status".format(self.role_name), - self.vehicle_status_updated, qos_profile=10) + self.vehicle_odometry_subscriber = self.new_subscription( + Odometry, + "/carla/vehicles/{}/odometry".format(self.role_name), + self.vehicle_odometry_updated, + QoSProfileSubscriber(depth=10)) + self.vehicle_control_status_subscriber = node.new_subscription( + CarlaVehicleControlStatus, "/carla/vehicles/{}/vehicle_control_status".format(self.role_name), + self.vehicle_control_status_updated, qos_profile=10) self.vehicle_info = CarlaVehicleInfo() self.vehicle_info_subscriber = node.new_subscription( @@ -339,10 +344,10 @@ def __init__(self, role_name, width, height, node): self.gnss_updated, qos_profile=10) - self.vehicle_status_subscriber = node.new_subscription( + self.vehicle_control_status_subscriber = node.new_subscription( Odometry, - "/carla/vehicles/{}/vehicle_status".format(self.role_name), - self.vehicle_status_updated, + "/carla/vehicles/{}/vehicle_control_status".format(self.role_name), + self.vehicle_control_status_updated, qos_profile=10 ) @@ -379,11 +384,11 @@ def manual_control_override_updated(self, data): self.manual_control = data.data self.update_info_text() - def vehicle_status_updated(self, vehicle_status): + def vehicle_control_status_updated(self, vehicle_control_status): """ Callback on vehicle status updates """ - self.vehicle_status = vehicle_status + self.vehicle_control_status = vehicle_control_status self.update_info_text() def vehicle_info_updated(self, vehicle_info): @@ -401,16 +406,19 @@ def gnss_updated(self, data): self.longitude = data.longitude self.update_info_text() - def vehicle_status_updated(self, data): - self.x = data.pose.pose.position.x - self.y = data.pose.pose.position.y - self.z = data.pose.pose.position.z + def vehicle_odometry_updated(self, odometry_msg): + self.x = odometry_msg.pose.pose.position.x + self.y = odometry_msg.pose.pose.position.y + self.z = odometry_msg.pose.pose.position.z _, _, yaw = quat2euler( - [data.pose.pose.orientation.w, - data.pose.pose.orientation.x, - data.pose.pose.orientation.y, - data.pose.pose.orientation.z]) + [odometry_msg.pose.pose.orientation.w, + odometry_msg.pose.pose.orientation.x, + odometry_msg.pose.pose.orientation.y, + odometry_msg.pose.pose.orientation.z]) self.yaw = math.degrees(yaw) + self.velocity = math.sqrt(odometry_msg.twist.twist.linear.x ** 2 + + odometry_msg.twist.twist.linear.y ** 2 + + odometry_msg.twist.twist.linear.z ** 2) * 3.6 self.update_info_text() def update_info_text(self): @@ -438,23 +446,23 @@ def update_info_text(self): 'Simulation time: % 12s' % time, 'FPS: % 24.1f' % fps, '', 'Vehicle: % 20s' % ' '.join(self.vehicle_info.type.title().split('.')[1:]), - 'Speed: % 15.0f km/h' % (3.6 * self.vehicle_status.velocity), + 'Speed: % 15.0f km/h' % (self.velocity), u'Heading:% 16.0f\N{DEGREE SIGN} % 2s' % (yaw, heading), 'Location:% 20s' % ('(% 5.1f, % 5.1f)' % (x, y)), 'GNSS:% 24s' % ('(% 2.6f, % 3.6f)' % (self.latitude, self.longitude)), 'Height: % 18.0f m' % z, '' ] self._info_text += [ - ('Throttle:', self.vehicle_status.control.throttle, 0.0, 1.0), - ('Steer:', self.vehicle_status.control.steer, -1.0, 1.0), - ('Brake:', self.vehicle_status.control.brake, 0.0, 1.0), - ('Reverse:', self.vehicle_status.control.reverse), - ('Hand brake:', self.vehicle_status.control.hand_brake), - ('Manual:', self.vehicle_status.control.manual_gear_shift), + ('Throttle:', self.vehicle_control_status.last_applied_vehicle_control.throttle, 0.0, 1.0), + ('Steer:', self.vehicle_control_status.last_applied_vehicle_control.steer, -1.0, 1.0), + ('Brake:', self.vehicle_control_status.last_applied_vehicle_control.brake, 0.0, 1.0), + ('Reverse:', self.vehicle_control_status.last_applied_vehicle_control.reverse), + ('Hand brake:', self.vehicle_control_status.last_applied_vehicle_control.hand_brake), + ('Manual:', self.vehicle_control_status.last_applied_vehicle_control.manual_gear_shift), 'Gear: %s' % { -1: 'R', 0: 'N' - }.get(self.vehicle_status.control.gear, self.vehicle_status.control.gear), '' + }.get(self.vehicle_control_status.last_applied_vehicle_control.gear, self.vehicle_control_status.last_applied_vehicle_control.gear), '' ] self._info_text += [('Manual ctrl:', self.manual_control)] if self.carla_status.synchronous_mode: diff --git a/carla_msgs b/carla_msgs index 9029ff69..165f3061 160000 --- a/carla_msgs +++ b/carla_msgs @@ -1 +1 @@ -Subproject commit 9029ff6956efe6c780ac920bf81b730fd889c51e +Subproject commit 165f3061aca6a5ec943d78daa69cb3df8da201ce diff --git a/docs/images/ad_demo.dot b/docs/images/ad_demo.dot index cd97b1ac..12b363c7 100644 --- a/docs/images/ad_demo.dot +++ b/docs/images/ad_demo.dot @@ -39,7 +39,7 @@ digraph graphname { tooltip="/carla_waypoint_publisher", width=2.9067]; n___rviz -> n___carla_waypoint_publisher [URL=topic_3A__carla__ego_vehicle__goal, - label="/carla/ego_vehicle/goal", + label="/carla/vehicles/ego_vehicle/goal", lp="188.49,609.5", penwidth=1, pos="e,322.49,631.48 92.426,541.3 145.96,562.28 250.36,603.21 313.16,627.82"]; @@ -58,7 +58,7 @@ digraph graphname { tooltip="/rviz", width=0.75]; n___rviz -> n___carla_ego_vehicle_ego_vehicle [URL=topic_3A__initialpose, - label="/carla/ego_vehicle/initialpose", + label="/carla/vehicles/ego_vehicle/initialpose", lp="1169.4,526.5", penwidth=1, pos="e,1545.9,484.96 686.93,420.12 692.63,424.89 699.37,429.75 706.27,433 842.05,496.96 888.71,477.92 1036.9,502 1129.3,517.02 1380.6,503.05 1454.1,561 1461,566.42 1454.8,574.23 1462.1,579 1476.3,588.2 1487.6,590.27 1500.1,579 1512.3,568.11 1497.2,518.1 1508.1,506 1515.8,497.57 1525.6,491.81 1536.3,487.92"]; @@ -70,7 +70,7 @@ digraph graphname { tooltip="/carla_twist_to_control", width=2.5637]; n___rviz -> n___carla_twist_to_control [URL=topic_3A__carla__ego_vehicle__twist, - label="/carla/ego_vehicle/twist", + label="/carla/vehicles/ego_vehicle/twist", lp="771.27,421.5", penwidth=1, pos="e,847.79,414.98 698.66,406.55 731.36,408.4 787.87,411.59 837.61,414.41"]; @@ -101,17 +101,17 @@ digraph graphname { fontcolor=blue, pos="e,1536.6,304.89 675.79,387.15 680.68,371.53 690.05,349.45 706.27,337 756.06,298.78 782.37,317.4 844.27,307 1045.1,273.26 1098.5,274.03 1301.9,284 1377.7,287.72 1463.4,296.44 1526.3,303.69"]; n___rviz -> n___carla_spectator_camera [URL=topic_3A__carla__ego_vehicle__spectator_pose, - label="/carla/ego_vehicle/spectator_pose", + label="/carla/vehicles/ego_vehicle/spectator_pose", lp="1169.4,291.5", penwidth=1, pos="e,1536.6,304.89 675.79,387.15 680.68,371.53 690.05,349.45 706.27,337 756.06,298.78 782.37,317.4 844.27,307 1045.1,273.26 1098.5,274.03 1301.9,284 1377.7,287.72 1463.4,296.44 1526.3,303.69"]; n___carla_twist_to_control -> n___carla_ros_bridge [URL=topic_3A__carla__ego_vehicle__vehicle_control_cmd_manual, - label="/carla/ego_vehicle/vehicle_control_cmd_manual", + label="/carla/vehicles/ego_vehicle/vehicle_control_cmd_manual", lp="1169.4,457.5", penwidth=1, pos="e,1309.8,450.27 1005.5,432.06 1016,433.59 1026.7,434.97 1036.9,436 1126.4,445.06 1229.6,448.65 1299.7,450.08"]; n___carla_waypoint_publisher -> n___rviz [URL=topic_3A__carla__ego_vehicle__waypoints, - label="/carla/ego_vehicle/waypoints", + label="/carla/vehicles/ego_vehicle/waypoints", lp="556.77,575.5", penwidth=1, pos="e,662.37,422.27 391.18,630.38 455.5,587.57 615.9,480.06 636.27,459 644.28,450.73 651.51,440.36 657.29,430.93"]; @@ -123,7 +123,7 @@ digraph graphname { tooltip="/carla_ad_agent_ego_vehicle", width=3.1414]; n___carla_waypoint_publisher -> n___carla_ad_agent_ego_vehicle [URL=topic_3A__carla__ego_vehicle__waypoints, - label="/carla/ego_vehicle/waypoints", + label="/carla/vehicles/ego_vehicle/waypoints", lp="1169.4,676.5", penwidth=1, pos="e,2017.5,594.46 446.66,659.26 456.95,660.36 467.34,661.32 477.27,662 640.01,673.19 681.15,665.89 844.27,667 1104.4,668.77 1620,671.24 1726.1,666 1840.4,660.36 1895.2,715.05 1982.4,641 1992.9,632.07 1980.2,620.31 1990.4,611 1995.8,606.05 2001.9,601.96 2008.4,598.6"]; @@ -164,17 +164,17 @@ digraph graphname { penwidth=1, pos="e,676.56,422.87 1537.5,626.95 1396.9,605.25 1095.9,556.81 844.27,503 782.5,489.79 756.33,507.53 706.27,469 694.28,459.77 685.85,445.2 680.28,432.3"]; n___carla_ros_scenario_runner -> n___carla_waypoint_publisher [URL=topic_3A__carla__ego_vehicle__goal, - label="/carla/ego_vehicle/goal", + label="/carla/vehicles/ego_vehicle/goal", lp="936.56,655.5", penwidth=1, pos="e,469.3,648 1515.6,644.58 1474.2,646.42 1425.8,648 1382,648 671.27,648 671.27,648 671.27,648 607.87,648 537.09,648 479.52,648"]; n___carla_ros_scenario_runner -> n___carla_ad_agent_ego_vehicle [URL=topic_3A__carla__ego_vehicle__target_speed, - label="/carla/ego_vehicle/target_speed", + label="/carla/vehicles/ego_vehicle/target_speed", lp="1858.3,629.5", penwidth=1, pos="e,2022.7,596.15 1696.4,626.86 1709,625.11 1721.9,623.42 1734.1,622 1786.2,615.96 1917,604.85 2012.7,596.97"]; - n___carla_ros_bridge -> n___rviz [URL=topic_3A__carla__ego_vehicle__vehicle_status, - label="/carla/ego_vehicle/vehicle_status", + n___carla_ros_bridge -> n___rviz [URL=topic_3A__carla__ego_vehicle__vehicle_control_status, + label="/carla/vehicles/ego_vehicle/vehicle_control_status", lp="936.56,385.5", penwidth=1, pos="e,697.21,399.67 1361.4,433.48 1346.1,421.51 1324,406.49 1301.9,399 1109,333.77 1047.2,362.26 844.27,378 796.62,381.7 742.24,391.04 707.37,397.7"]; @@ -184,12 +184,12 @@ digraph graphname { penwidth=1, pos="e,2016.2,593.77 1402.9,468.38 1418.9,481.63 1441.7,500.22 1462.1,516 1478.7,528.83 1485.7,528.82 1500.1,544 1504.6,548.64 1502.6,552.83 1508.1,556 1590.2,602.71 1851.7,600.22 2006.2,594.17"]; n___carla_ros_bridge -> n___carla_ad_agent_ego_vehicle [URL=topic_3A__carla__odometry, - label="/carla/ego_vehicle/odometry", + label="/carla/vehicles/ego_vehicle/odometry", lp="1617.1,601.5", penwidth=1, pos="e,2016.2,593.77 1402.9,468.38 1418.9,481.63 1441.7,500.22 1462.1,516 1478.7,528.83 1485.7,528.82 1500.1,544 1504.6,548.64 1502.6,552.83 1508.1,556 1590.2,602.71 1851.7,600.22 2006.2,594.17"]; n___carla_ros_bridge -> n___carla_ad_agent_ego_vehicle [URL=topic_3A__carla__objects, - label="/carla/ego_vehicle/objects", + label="/carla/vehicles/ego_vehicle/objects", lp="1617.1,601.5", penwidth=1, pos="e,2016.2,593.77 1402.9,468.38 1418.9,481.63 1441.7,500.22 1462.1,516 1478.7,528.83 1485.7,528.82 1500.1,544 1504.6,548.64 1502.6,552.83 1508.1,556 1590.2,602.71 1851.7,600.22 2006.2,594.17"]; @@ -213,12 +213,12 @@ digraph graphname { fontcolor=blue, pos="e,1410.5,467.54 2035.6,576.6 1955.1,566.19 1832.6,550.42 1726.1,537 1629.3,524.79 1602,537.01 1508.1,510 1477,501.04 1444,485.32 1419.5,472.35"]; n___carla_ad_agent_ego_vehicle -> n___carla_ros_bridge [URL=topic_3A__carla__ego_vehicle__vehicle_control_cmd, - label="/carla/ego_vehicle/vehicle_control_cmd", + label="/carla/vehicles/ego_vehicle/vehicle_control_cmd", lp="1617.1,544.5", penwidth=1, pos="e,1410.5,467.54 2035.6,576.6 1955.1,566.19 1832.6,550.42 1726.1,537 1629.3,524.79 1602,537.01 1508.1,510 1477,501.04 1444,485.32 1419.5,472.35"]; n___carla_ros_bridge -> n___carla_ad_agent_ego_vehicle [URL=topic_3A__carla__ros_bridge_ad_agent_ego_vehicle, - label="/carla/ego_vehicle/vehicle_info", + label="/carla/vehicles/ego_vehicle/vehicle_info", lp="1617.1,544.5", penwidth=1, pos="e,1410.5,467.54 2035.6,576.6 1955.1,566.19 1832.6,550.42 1726.1,537 1629.3,524.79 1602,537.01 1508.1,510 1477,501.04 1444,485.32 1419.5,472.35"]; diff --git a/docs/ros_msgs.md b/docs/ros_msgs.md index 8cdc60bf..7fdaa82d 100644 --- a/docs/ros_msgs.md +++ b/docs/ros_msgs.md @@ -122,17 +122,16 @@ Static information regarding a wheel that will be part of a [CarlaVehicleInfo.ms --- -## CarlaVehicleStatus.msg +## CarlaVehicleControlStatus.msg -Current status of the vehicle as an object in the world. +Current status of the vehicle control settings. | Field | Type | Description | | ------------------------------------------------------------------------- | ------------------------------------------------------------------------- | ------------------------------------------------------------------------- | -| `header` | [Header](https://docs.ros.org/en/melodic/api/std_msgs/html/msg/Header.html) | Time stamp and frame ID when the message is published. | -| `velocity` | float32 | Current speed of the vehicle. | -| `acceleration` | geometry\_msgs/Accel | Current acceleration of the vehicle. | -| `orientation` | geometry\_msgs/Quaternion | Current orientation of the vehicle. | -| `control` | [CarlaVehicleControl](<#carlavehiclecontrolmsg>) | Current control values as reported by CARLA. | +| `active_control_type` | {VEHICLE_CONTROL, ACKERMANN_CONTROL} | Current active control type as reported by CARLA. | +| `last_applied_vehicle_control` | [CarlaVehicleControl](<#carlavehiclecontrolmsg>) | Current control values as reported by CARLA. | +| `last_applied_ackermann_control` | AckermannDriveStamped | Current ackermann control values as reported by CARLA. | + --- diff --git a/docs/run_ros.md b/docs/run_ros.md index 3a2822fd..777e79a5 100644 --- a/docs/run_ros.md +++ b/docs/run_ros.md @@ -120,7 +120,7 @@ __2.__ In another terminal, publish to the topic `/carla/vehicles//co ``` -The current status of the vehicle can be received via topic `/carla/vehicles//vehicle_status`. Static information about the vehicle can be received via `/carla/vehicles//vehicle_info`. +The current control status of the vehicle can be received via topic `/carla/vehicles//vehicle_constrol_status`. Static information about the vehicle can be received via `/carla/vehicles//vehicle_info`. It is possible to use [AckermannDrive](https://docs.ros.org/en/api/ackermann_msgs/html/msg/AckermannDrive.html) messages to control the ego vehicles. This can be achieved by publishing to the topic ```sh diff --git a/docs/rviz_plugin.md b/docs/rviz_plugin.md index f811c7fe..ba53759e 100644 --- a/docs/rviz_plugin.md +++ b/docs/rviz_plugin.md @@ -76,9 +76,9 @@ ros2 launch carla_manual_control carla_manual_control.launch.py | Topic | Type | Description | |-------|------|-------------| -| `/carla/status` | [carla_msgs/CarlaStatus](ros_msgs.md#carlastatusmsg) | Read the current status of CARLA | -| `/carla/ego_vehicle/vehicle_status` | [carla_msgs/CarlaVehicleStatus](ros_msgs.md#carlavehiclestatusmsg) | Display the current state of the ego vehicle | -| `/carla/ego_vehicle/odometry` | [nav_msgs/Odometry](https://docs.ros.org/en/api/nav_msgs/html/msg/Odometry.html) | Display the current pose of the ego vehicle | +| `/carla/world/status` | [carla_msgs/CarlaStatus](ros_msgs.md#carlastatusmsg) | Read the current status of CARLA | +| `/carla/vehicles/ego_vehicle/vehicle_control_status` | [carla_msgs/CarlaVehicleControlStatus](ros_msgs.md#carlavehiclecontrolstatusmsg) | Display the current state of the ego vehicle | +| `/carla/vehicles/ego_vehicle/odometry` | [nav_msgs/Odometry](https://docs.ros.org/en/api/nav_msgs/html/msg/Odometry.html) | Display the current pose of the ego vehicle | | `/scenario_runner/status` | [carla_ros_scenario_runner_types/CarlaScenarioRunnerStatus](ros_msgs.md#carlascenariorunnerstatusmsg) | Visualize the scenario runner status | | `/carla/available_scenarios` | [carla_ros_scenario_runner_types/CarlaScenarioList](ros_msgs.md#carlascenariolistmsg) | Provides a list of scenarios to execute (disabled in combo box)| @@ -88,10 +88,10 @@ ros2 launch carla_manual_control carla_manual_control.launch.py | Topic | Type | Description | |-------|------|-------------| -| `/carla/control` | [carla_msgs/CarlaControl](ros_msgs.md#carlacontrolmsg) | Play/pause/step CARLA | -| `/carla/ego_vehicle/spectator_pose` | [geometry_msgs/PoseStamped](https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html) | Publish the current pose of the RVIZ camera view | -| `/carla/ego_vehicle/vehicle_control_manual_override` | [std_msgs/Bool](https://docs.ros.org/en/api/std_msgs/html/msg/Bool.html) | Enable/disable vehicle control override | -| `/carla/ego_vehicle/twist` | [geometry_msgs/Twist](https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html) | The twist command, created via mouse | +| `/carla/world/control` | [carla_msgs/CarlaControl](ros_msgs.md#carlacontrolmsg) | Play/pause/step CARLA | +| `/carla/vehicles/ego_vehicle/spectator_pose` | [geometry_msgs/PoseStamped](https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html) | Publish the current pose of the RVIZ camera view | +| `/carla/vehicles/ego_vehicle/vehicle_control_manual_override` | [std_msgs/Bool](https://docs.ros.org/en/api/std_msgs/html/msg/Bool.html) | Enable/disable vehicle control override | +| `/carla/vehicles/ego_vehicle/twist` | [geometry_msgs/Twist](https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html) | The twist command, created via mouse |
diff --git a/rviz_carla_plugin/src/carla_control_panel_ROS2.cpp b/rviz_carla_plugin/src/carla_control_panel_ROS2.cpp index 84b70c01..a1cfc715 100644 --- a/rviz_carla_plugin/src/carla_control_panel_ROS2.cpp +++ b/rviz_carla_plugin/src/carla_control_panel_ROS2.cpp @@ -177,8 +177,10 @@ void CarlaControlPanel::onInitialize() // set up ros subscriber and publishers mCarlaStatusSubscriber = _node->create_subscription("/carla/world/status", rclcpp::SensorDataQoS().keep_last(10).transient_local(), std::bind(&CarlaControlPanel::carlaStatusChanged, this, _1)); mCarlaControlPublisher = _node->create_publisher("/carla/world/control", 10); - mVehicleStatusSubscriber - = _node->create_subscription("/carla/vehicles/hero/vehicle_status", 1000, std::bind(&CarlaControlPanel::vehicleStatusChanged, this, _1)); + mVehicleControlStatusSubscriber + = _node->create_subscription("/carla/vehicles/hero/vehicle_control_status", 1000, std::bind(&CarlaControlPanel::vehicleControlStatusChanged, this, _1)); + mVehicleOdometrySubscriber = _node->create_subscription("/carla/vehicles/hero/odometry", 1000, std::bind(&CarlaControlPanel::vehicleOdometryChanged, this, _1)); + mVehicleSpeedSubscriber = _node->create_subscription("/carla/vehicles/hero/speed", 1000, std::bind(&CarlaControlPanel::vehicleSpeedChanged, this, _1)); auto qos_latch_10 = rclcpp::QoS( rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, 10)); qos_latch_10.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); @@ -301,23 +303,29 @@ void CarlaControlPanel::carlaScenariosChanged(const carla_ros_scenario_runner_ty setScenarioRunnerStatus(mScenarioSelection->count() > 0); } -void CarlaControlPanel::vehicleStatusChanged(const carla_msgs::msg::CarlaVehicleStatus::SharedPtr msg) +void CarlaControlPanel::vehicleControlStatusChanged(const carla_msgs::msg::CarlaVehicleControlStatus::SharedPtr msg) { mOverrideVehicleControl->setEnabled(true); mSteerBar->setValue(msg->last_applied_vehicle_control.steer * 100); mThrottleBar->setValue(msg->last_applied_vehicle_control.throttle * 100); mBrakeBar->setValue(msg->last_applied_vehicle_control.brake * 100); +} +void CarlaControlPanel::vehicleSpeedChanged(const std_msgs::msg::Float32::SharedPtr msg) +{ std::stringstream speedStream; - speedStream << std::fixed << std::setprecision(2) << msg->velocity * 3.6; + speedStream << std::fixed << std::setprecision(2) << msg->data * 3.6; mSpeedLabel->setText(speedStream.str().c_str()); +} +void CarlaControlPanel::vehicleOdometryChanged(const nav_msgs::msg::Odometry::SharedPtr msg) +{ std::stringstream posStream; - posStream << std::fixed << std::setprecision(2) << msg->pose.position.x << ", " << msg->pose.position.y; + posStream << std::fixed << std::setprecision(2) << msg->pose.pose.position.x << ", " << msg->pose.pose.position.y; mPosLabel->setText(posStream.str().c_str()); std::stringstream headingStream; - headingStream << std::fixed << std::setprecision(2) << tf2::getYaw(msg->pose.orientation); + headingStream << std::fixed << std::setprecision(2) << tf2::getYaw(msg->pose.pose.orientation); mHeadingLabel->setText(headingStream.str().c_str()); } diff --git a/rviz_carla_plugin/src/carla_control_panel_ROS2.h b/rviz_carla_plugin/src/carla_control_panel_ROS2.h index 46a99834..2c2fa4df 100644 --- a/rviz_carla_plugin/src/carla_control_panel_ROS2.h +++ b/rviz_carla_plugin/src/carla_control_panel_ROS2.h @@ -6,20 +6,21 @@ */ #pragma once -#include -#include -#include #include +#include +#include #include #include -#include "rclcpp/rclcpp.hpp" -#include -#include #include -#include -#include +#include +#include +#include +#include +#include + +#include "rviz_common/panel.hpp" #include "rviz_common/ros_integration/ros_node_abstraction_iface.hpp" -#include +#include "rviz_common/frame_position_tracking_view_controller.hpp" class QLineEdit; class QPushButton; @@ -66,7 +67,9 @@ protected Q_SLOTS: void scenarioRunnerStatusChanged(const carla_ros_scenario_runner_types::msg::CarlaScenarioRunnerStatus::SharedPtr msg); void carlaStatusChanged(const carla_msgs::msg::CarlaStatus::SharedPtr msg); - void vehicleStatusChanged(const carla_msgs::msg::CarlaVehicleStatus::SharedPtr msg); + void vehicleControlStatusChanged(const carla_msgs::msg::CarlaVehicleControlStatus::SharedPtr msg); + void vehicleSpeedChanged(const std_msgs::msg::Float32::SharedPtr msg); + void vehicleOdometryChanged(const nav_msgs::msg::Odometry::SharedPtr msg); void carlaScenariosChanged(const carla_ros_scenario_runner_types::msg::CarlaScenarioList::SharedPtr msg); carla_msgs::msg::CarlaStatus::SharedPtr mCarlaStatus{nullptr}; @@ -89,7 +92,9 @@ protected Q_SLOTS: rclcpp::Publisher::SharedPtr mCarlaControlPublisher; rclcpp::Publisher::SharedPtr mVehicleControlManualOverridePublisher; rclcpp::Subscription::SharedPtr mCarlaStatusSubscriber; - rclcpp::Subscription::SharedPtr mVehicleStatusSubscriber; + rclcpp::Subscription::SharedPtr mVehicleControlStatusSubscriber; + rclcpp::Subscription::SharedPtr mVehicleOdometrySubscriber; + rclcpp::Subscription::SharedPtr mVehicleSpeedSubscriber; rclcpp::Client::SharedPtr mExecuteScenarioClient; rclcpp::Subscription::SharedPtr mScenarioSubscriber; rclcpp::Subscription::SharedPtr mScenarioRunnerStatusSubscriber;