Skip to content

Commit

Permalink
Split CarlaVehicleStatus into Speed, Odometry, CarlaVehicleControlStatus
Browse files Browse the repository at this point in the history 
This provides the pseudo.speedometer and pseudo.odom sensors for better
backward compatibility and usability.
  • Loading branch information
@berndgassmann
berndgassmann committed Jul 30, 2024
1 parent 5420ae6 commit 357c535
Show file tree
Hide file tree
Showing 10 changed files with 119 additions and 96 deletions.
14 changes: 7 additions & 7 deletions carla_ad_agent/src/carla_ad_agent/ad_agent.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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


Expand Down Expand Up @@ -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
)

Expand Down Expand Up @@ -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:
Expand Down
31 changes: 17 additions & 14 deletions carla_ad_agent/src/carla_ad_agent/local_planner.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down Expand Up @@ -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,
Expand All @@ -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()
Expand All @@ -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

Expand Down
62 changes: 35 additions & 27 deletions carla_manual_control/src/carla_manual_control/carla_manual_control.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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(
Expand All @@ -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
)

Expand Down Expand Up @@ -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):
Expand All @@ -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):
Expand Down Expand Up @@ -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:
Expand Down
2 changes: 1 addition & 1 deletion carla_msgs
Submodule carla_msgs updated 6 files
+1 −1 CMakeLists.txt
+21 −0 msg/CarlaV2XCustomData.idl
+19 −0 msg/CarlaV2XCustomDataList.idl
+21 −0 msg/CarlaV2XCustomMessage.idl
+3 −26 msg/CarlaVehicleControlStatus.idl
+0 −11 msg/CarlaVehicleControlStatus.msg
30 changes: 15 additions & 15 deletions docs/images/ad_demo.dot
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"];
Expand All @@ -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"];
Expand All @@ -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"];
Expand Down Expand Up @@ -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"];
Expand All @@ -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"];
Expand Down Expand Up @@ -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"];
Expand All @@ -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"];
Expand All @@ -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"];
Expand Down
Loading

0 comments on commit 357c535

Please sign in to comment.