From e3073008f794edecdbbc1e567a2eef57496560d9 Mon Sep 17 00:00:00 2001 From: R4C00n Date: Sun, 15 Feb 2026 14:44:10 +0800 Subject: [PATCH] update pedestrian model for rviz --- _meta/repos/arena.repos | 10 +- .../scripts/pedestrian_marker_publisher.py | 196 +++++---- utils/rviz_utils/rviz_utils/utils.py | 391 +++++++++--------- 3 files changed, 314 insertions(+), 283 deletions(-) diff --git a/_meta/repos/arena.repos b/_meta/repos/arena.repos index 7119eee..09d1741 100644 --- a/_meta/repos/arena.repos +++ b/_meta/repos/arena.repos @@ -78,7 +78,15 @@ repositories: deps/hunav/arena_hunav_sim_bridge: type: git url: https://github.com/ductaingn/arena_hunav_sim_bridge.git - version: humble@dc66bfd + version: humble@e647ad9 + deps/hunav/arena_text_crowd: + type: git + url: https://github.com/ductaingn/arena_text_crowd.git + version: humble + deps/pal_gazebo_worlds: + type: git + url: https://github.com/ductaingn/pal_gazebo_worlds.git + version: humble-devel gazebo/turtlebot4_simulator: type: git diff --git a/utils/rviz_utils/rviz_utils/scripts/pedestrian_marker_publisher.py b/utils/rviz_utils/rviz_utils/scripts/pedestrian_marker_publisher.py index 3f91f14..3188209 100644 --- a/utils/rviz_utils/rviz_utils/scripts/pedestrian_marker_publisher.py +++ b/utils/rviz_utils/rviz_utils/scripts/pedestrian_marker_publisher.py @@ -2,12 +2,18 @@ import rclpy from rclpy.node import Node -from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSDurabilityPolicy, QoSHistoryPolicy +from rclpy.qos import ( + QoSProfile, + QoSReliabilityPolicy, + QoSDurabilityPolicy, + QoSHistoryPolicy, +) from arena_people_msgs.msg import Pedestrians, Pedestrian from visualization_msgs.msg import MarkerArray, Marker from geometry_msgs.msg import Vector3 from std_msgs.msg import ColorRGBA +from tf_transformations import quaternion_from_euler, quaternion_multiply import math @@ -18,7 +24,7 @@ class PedestrianMarkerPublisher(Node): """ def __init__(self): - super().__init__('pedestrian_marker_publisher') + super().__init__("pedestrian_marker_publisher") # Get namespace parameter for multi-environment support namespace = self.get_namespace() @@ -28,7 +34,7 @@ def __init__(self): reliability=QoSReliabilityPolicy.BEST_EFFORT, durability=QoSDurabilityPolicy.VOLATILE, history=QoSHistoryPolicy.KEEP_LAST, - depth=10 + depth=10, ) # QoS Settings for marker output @@ -36,39 +42,42 @@ def __init__(self): reliability=QoSReliabilityPolicy.RELIABLE, durability=QoSDurabilityPolicy.VOLATILE, history=QoSHistoryPolicy.KEEP_LAST, - depth=10 + depth=10, ) # Build namespaced topic names - pedestrians_topic = f'{namespace}/arena_peds' - marker_topic = f'{namespace}/pedestrian_markers' + pedestrians_topic = f"{namespace}/arena_peds" + marker_topic = f"{namespace}/pedestrian_markers" # Subscriber to arena_peds topic self.pedestrians_subscriber = self.create_subscription( - Pedestrians, - pedestrians_topic, - self.pedestrians_callback, - pedestrians_qos + Pedestrians, pedestrians_topic, self.pedestrians_callback, pedestrians_qos ) # Publisher for pedestrian markers self.marker_publisher = self.create_publisher( - MarkerArray, - marker_topic, - marker_qos + MarkerArray, marker_topic, marker_qos ) # Parameters for visualization - self.declare_parameter('body_height', 1.6) # Height of pedestrian body - self.declare_parameter('body_radius', 0.25) # Radius of pedestrian body - self.declare_parameter('head_radius', 0.15) # Radius of pedestrian head - self.declare_parameter('arrow_length', 0.6) # Length of direction arrow - self.declare_parameter('show_labels', True) # Show name labels - self.declare_parameter('show_velocity_arrows', True) # Show velocity arrows - self.declare_parameter('show_orientation_arrows', True) # Show orientation arrows + self.declare_parameter("body_height", 1.6) # Height of pedestrian body + self.declare_parameter("body_radius", 0.25) # Radius of pedestrian body + self.declare_parameter("head_radius", 0.15) # Radius of pedestrian head + self.declare_parameter("arrow_length", 0.6) # Length of direction arrow + self.declare_parameter("show_labels", True) # Show name labels + self.declare_parameter("show_velocity_arrows", True) # Show velocity arrows + self.declare_parameter( + "show_orientation_arrows", True + ) # Show orientation arrows + self.declare_parameter( + "mesh_resource", + "package://pal_gazebo_worlds/models/citizen_extras_male_03/meshes/mesh.dae", + ) - self.get_logger().info('Pedestrian Marker Publisher initialized') - self.get_logger().info(f'Subscribing to {pedestrians_topic}, publishing to {marker_topic}') + self.get_logger().info("Pedestrian Marker Publisher initialized") + self.get_logger().info( + f"Subscribing to {pedestrians_topic}, publishing to {marker_topic}" + ) def pedestrians_callback(self, msg: Pedestrians): """Convert Pedestrians message to MarkerArray with stylized human figures""" @@ -77,13 +86,12 @@ def pedestrians_callback(self, msg: Pedestrians): markers = [] # Get parameters - body_height = self.get_parameter('body_height').value - body_radius = self.get_parameter('body_radius').value - head_radius = self.get_parameter('head_radius').value - arrow_length = self.get_parameter('arrow_length').value - show_labels = self.get_parameter('show_labels').value - show_velocity_arrows = self.get_parameter('show_velocity_arrows').value - show_orientation_arrows = self.get_parameter('show_orientation_arrows').value + body_height = self.get_parameter("body_height").value + body_radius = self.get_parameter("body_radius").value + arrow_length = self.get_parameter("arrow_length").value + show_labels = self.get_parameter("show_labels").value + show_velocity_arrows = self.get_parameter("show_velocity_arrows").value + show_orientation_arrows = self.get_parameter("show_orientation_arrows").value # Clear existing markers first (important for dynamic number of people) delete_marker = Marker() @@ -98,34 +106,40 @@ def pedestrians_callback(self, msg: Pedestrians): # Determine colors based on animation state body_color, head_color = self._get_pedestrian_colors(pedestrian) - # 1. Body (Cylinder) - body_marker = self._create_body_marker(pedestrian, pedestrian_id, body_color, - body_height, body_radius, msg.header) + # 1. Body (mesh) + body_marker = self._create_body_marker( + pedestrian, + pedestrian_id, + body_color, + body_height, + body_radius, + msg.header, + ) markers.append(body_marker) - # 2. Head (Sphere) - head_marker = self._create_head_marker(pedestrian, pedestrian_id, head_color, - head_radius, body_height, msg.header) - markers.append(head_marker) - # 3. Velocity Arrow (if pedestrian is moving) if show_velocity_arrows: - velocity_magnitude = math.sqrt(pedestrian.twist.linear.x**2 + pedestrian.twist.linear.y**2) + velocity_magnitude = math.sqrt( + pedestrian.twist.linear.x**2 + pedestrian.twist.linear.y**2 + ) if velocity_magnitude > 0.1: # Only show if moving fast enough - arrow_marker = self._create_velocity_arrow(pedestrian, pedestrian_id, - arrow_length, body_height, msg.header) + arrow_marker = self._create_velocity_arrow( + pedestrian, pedestrian_id, arrow_length, body_height, msg.header + ) markers.append(arrow_marker) # 4. Orientation Arrow (shows where pedestrian is facing) if show_orientation_arrows: - orientation_marker = self._create_orientation_arrow(pedestrian, pedestrian_id, - arrow_length, body_height, msg.header) + orientation_marker = self._create_orientation_arrow( + pedestrian, pedestrian_id, arrow_length, body_height, msg.header + ) markers.append(orientation_marker) # 5. Name Label if show_labels: - label_marker = self._create_name_label(pedestrian, pedestrian_id, - body_height, msg.header) + label_marker = self._create_name_label( + pedestrian, pedestrian_id, body_height, msg.header + ) markers.append(label_marker) marker_array.markers = markers @@ -160,62 +174,63 @@ def _get_pedestrian_colors(self, pedestrian: Pedestrian): return body_color, head_color - def _create_body_marker(self, pedestrian: Pedestrian, pedestrian_id: int, color: ColorRGBA, - body_height: float, body_radius: float, header) -> Marker: - """Create cylinder marker for pedestrian body""" + def _create_body_marker( + self, + pedestrian: Pedestrian, + pedestrian_id: int, + color: ColorRGBA, + body_height: float, + body_radius: float, + header, + ) -> Marker: + """Create mesh marker for pedestrian body""" marker = Marker() marker.header = header - marker.ns = "pedestrian_bodies" + marker.ns = "pedestrian_meshes" marker.id = pedestrian_id - marker.type = Marker.CYLINDER + marker.type = Marker.MESH_RESOURCE + marker.mesh_resource = self.get_parameter("mesh_resource").value + marker.mesh_use_embedded_materials = True marker.action = Marker.ADD # Position - use pedestrian position marker.pose.position.x = pedestrian.pose.position.x marker.pose.position.y = pedestrian.pose.position.y - marker.pose.position.z = body_height / 2 # Center of cylinder - marker.pose.orientation.w = 1.0 + marker.pose.position.z = body_height / 2 # Center - # Size - marker.scale = Vector3(x=body_radius * 2, y=body_radius * 2, z=body_height) + # Align the mesh model's yaw with the true ped's pose (pi/2 offset) + q_org = [ + pedestrian.pose.orientation.x, + pedestrian.pose.orientation.y, + pedestrian.pose.orientation.z, + pedestrian.pose.orientation.w, + ] + rotation = quaternion_from_euler(0, 0, math.pi / 2) + q = quaternion_multiply(q_org, rotation) - # Color - marker.color = color - - # Lifetime - marker.lifetime.sec = 1 - - return marker + marker.pose.orientation.x = q[0] + marker.pose.orientation.y = q[1] + marker.pose.orientation.z = q[2] + marker.pose.orientation.w = q[3] - def _create_head_marker(self, pedestrian: Pedestrian, pedestrian_id: int, color: ColorRGBA, - head_radius: float, body_height: float, header) -> Marker: - """Create sphere marker for pedestrian head""" - marker = Marker() - marker.header = header - marker.ns = "pedestrian_heads" - marker.id = pedestrian_id - marker.type = Marker.SPHERE - marker.action = Marker.ADD - - # Position on top of body - marker.pose.position.x = pedestrian.pose.position.x - marker.pose.position.y = pedestrian.pose.position.y - marker.pose.position.z = body_height + head_radius - marker.pose.orientation.w = 1.0 - - # Size - marker.scale = Vector3(x=head_radius * 2, y=head_radius * 2, z=head_radius * 2) + marker.scale = Vector3(x=body_height, y=body_height, z=body_height) # Color - marker.color = color + # marker.color = color # Lifetime marker.lifetime.sec = 1 return marker - def _create_orientation_arrow(self, pedestrian: Pedestrian, pedestrian_id: int, - arrow_length: float, body_height: float, header) -> Marker: + def _create_orientation_arrow( + self, + pedestrian: Pedestrian, + pedestrian_id: int, + arrow_length: float, + body_height: float, + header, + ) -> Marker: """Create arrow marker showing pedestrian orientation""" marker = Marker() marker.header = header @@ -243,8 +258,14 @@ def _create_orientation_arrow(self, pedestrian: Pedestrian, pedestrian_id: int, return marker - def _create_velocity_arrow(self, pedestrian: Pedestrian, pedestrian_id: int, - arrow_length: float, body_height: float, header) -> Marker: + def _create_velocity_arrow( + self, + pedestrian: Pedestrian, + pedestrian_id: int, + arrow_length: float, + body_height: float, + header, + ) -> Marker: """Create arrow marker showing pedestrian velocity direction""" marker = Marker() marker.header = header @@ -279,8 +300,9 @@ def _create_velocity_arrow(self, pedestrian: Pedestrian, pedestrian_id: int, return marker - def _create_name_label(self, pedestrian: Pedestrian, pedestrian_id: int, - body_height: float, header) -> Marker: + def _create_name_label( + self, pedestrian: Pedestrian, pedestrian_id: int, body_height: float, header + ) -> Marker: """Create text marker with pedestrian name""" marker = Marker() marker.header = header @@ -318,11 +340,11 @@ def main(args=None): try: rclpy.spin(node) except KeyboardInterrupt: - node.get_logger().info('Shutting down pedestrian marker publisher') + node.get_logger().info("Shutting down pedestrian marker publisher") finally: node.destroy_node() rclpy.shutdown() -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/utils/rviz_utils/rviz_utils/utils.py b/utils/rviz_utils/rviz_utils/utils.py index 35a38cb..a58fc47 100644 --- a/utils/rviz_utils/rviz_utils/utils.py +++ b/utils/rviz_utils/rviz_utils/utils.py @@ -16,10 +16,10 @@ def get_random_rviz_color(cls): @classmethod def get_sensor_color(cls, sensor_type, index=0): """Generate appropriate colors for different sensor types""" - if sensor_type == 'sensor_msgs/msg/Imu': + if sensor_type == "sensor_msgs/msg/Imu": return "204; 51; 204" # Consistent purple for IMU - elif 'FootContact' in sensor_type: - return "255; 140; 0" # Consistent orange for FootContact + elif "FootContact" in sensor_type: + return "255; 140; 0" # Consistent orange for FootContact else: # Generate unique colors for LaserScan and PointCloud r = (index * 67) % 200 + 55 @@ -32,274 +32,275 @@ class Displays: def laser_scan(cls, topic_name, sensor_color, queue_size=20): """Create LaserScan display configuration""" return { - 'Class': 'rviz_default_plugins/LaserScan', - 'Name': f'LaserScan: {os.path.basename(topic_name)}', - 'Enabled': True, - 'Topic': { - 'Value': topic_name, - 'Depth': queue_size, - 'History Policy': 'Keep Last', - 'Reliability Policy': 'Best Effort', - 'Durability Policy': 'Volatile', + "Class": "rviz_default_plugins/LaserScan", + "Name": f"LaserScan: {os.path.basename(topic_name)}", + "Enabled": True, + "Topic": { + "Value": topic_name, + "Depth": queue_size, + "History Policy": "Keep Last", + "Reliability Policy": "Best Effort", + "Durability Policy": "Volatile", }, - 'Color': sensor_color, - 'Size (m)': 0.05, - 'Style': 'Points', - 'Alpha': 1.0, - 'Decay Time': 0.0 + "Color": sensor_color, + "Size (m)": 0.05, + "Style": "Points", + "Alpha": 1.0, + "Decay Time": 0.0, } @classmethod def pointcloud(cls, topic_name, sensor_color, queue_size=20): """Create PointCloud2 display configuration""" return { - 'Class': 'rviz_default_plugins/PointCloud2', - 'Name': f'PointCloud: {os.path.basename(topic_name)}', - 'Enabled': True, - 'Topic': { - 'Value': topic_name, - 'Depth': queue_size, - 'History Policy': 'Keep Last', - 'Reliability Policy': 'Best Effort', - 'Durability Policy': 'Volatile', + "Class": "rviz_default_plugins/PointCloud2", + "Name": f"PointCloud: {os.path.basename(topic_name)}", + "Enabled": True, + "Topic": { + "Value": topic_name, + "Depth": queue_size, + "History Policy": "Keep Last", + "Reliability Policy": "Best Effort", + "Durability Policy": "Volatile", }, - 'Color': sensor_color, - 'Size (m)': 0.03, - 'Style': 'Flat Squares', - 'Alpha': 1.0, - 'Decay Time': 0.0 + "Color": sensor_color, + "Size (m)": 0.03, + "Style": "Flat Squares", + "Alpha": 1.0, + "Decay Time": 0.0, } @classmethod def pointcloud_legacy(cls, topic_name, sensor_color, queue_size=20): """Create PointCloud (legacy) display configuration""" return { - 'Class': 'rviz_default_plugins/PointCloud', - 'Name': f'PointCloud: {os.path.basename(topic_name)}', - 'Enabled': True, - 'Topic': { - 'Value': topic_name, - 'Depth': queue_size, - 'History Policy': 'Keep Last', - 'Reliability Policy': 'Best Effort', - 'Durability Policy': 'Volatile', + "Class": "rviz_default_plugins/PointCloud", + "Name": f"PointCloud: {os.path.basename(topic_name)}", + "Enabled": True, + "Topic": { + "Value": topic_name, + "Depth": queue_size, + "History Policy": "Keep Last", + "Reliability Policy": "Best Effort", + "Durability Policy": "Volatile", }, - 'Color': sensor_color, - 'Size (m)': 0.03, - 'Alpha': 1.0, - 'Decay Time': 0.0 + "Color": sensor_color, + "Size (m)": 0.03, + "Alpha": 1.0, + "Decay Time": 0.0, } @classmethod def imu(cls, topic_name, sensor_color, queue_size=20): """Create IMU display configuration""" return { - 'Class': 'rviz_default_plugins/Imu', - 'Name': f'IMU: {os.path.basename(topic_name)}', - 'Enabled': True, - 'Topic': { - 'Value': topic_name, - 'Depth': queue_size, - 'History Policy': 'Keep Last', - 'Reliability Policy': 'Best Effort', - 'Durability Policy': 'Volatile', + "Class": "rviz_default_plugins/Imu", + "Name": f"IMU: {os.path.basename(topic_name)}", + "Enabled": True, + "Topic": { + "Value": topic_name, + "Depth": queue_size, + "History Policy": "Keep Last", + "Reliability Policy": "Best Effort", + "Durability Policy": "Volatile", }, - 'Axes Length': 0.3, - 'Axes Radius': 0.03, - 'Color': sensor_color + "Axes Length": 0.3, + "Axes Radius": 0.03, + "Color": sensor_color, } @classmethod def footcontact(cls, topic_name, sensor_color, queue_size=20): """Create FootContact display configuration""" return { - 'Class': 'rviz_default_plugins/Marker', - 'Name': f'FootContact: {os.path.basename(topic_name)}', - 'Enabled': True, - 'Topic': { - 'Value': topic_name, - 'Depth': queue_size, - 'History Policy': 'Keep Last', - 'Reliability Policy': 'Best Effort', - 'Durability Policy': 'Volatile', + "Class": "rviz_default_plugins/Marker", + "Name": f"FootContact: {os.path.basename(topic_name)}", + "Enabled": True, + "Topic": { + "Value": topic_name, + "Depth": queue_size, + "History Policy": "Keep Last", + "Reliability Policy": "Best Effort", + "Durability Policy": "Volatile", }, - 'Color': sensor_color + "Color": sensor_color, } @classmethod def robot_footprint(cls, topic, color, queue_size=20): return { - 'Class': 'rviz_default_plugins/Polygon', - 'Name': 'Robot Footprint', - 'Enabled': True, - 'Topic': { - 'Value': topic, - 'Depth': queue_size, - 'History Policy': 'Keep Last', - 'Reliability Policy': 'Reliable', - 'Durability Policy': 'Volatile', + "Class": "rviz_default_plugins/Polygon", + "Name": "Robot Footprint", + "Enabled": True, + "Topic": { + "Value": topic, + "Depth": queue_size, + "History Policy": "Keep Last", + "Reliability Policy": "Reliable", + "Durability Policy": "Volatile", }, - 'Color': color, - 'Alpha': 1.0 + "Color": color, + "Alpha": 1.0, } @classmethod def local_path(cls, topic, queue_size=20): return { - 'Class': 'rviz_default_plugins/Path', - 'Name': 'Local Plan', - 'Enabled': True, - 'Topic': { - 'Value': topic, - 'Depth': queue_size, - 'History Policy': 'Keep Last', - 'Reliability Policy': 'Reliable', - 'Durability Policy': 'Volatile', + "Class": "rviz_default_plugins/Path", + "Name": "Local Plan", + "Enabled": True, + "Topic": { + "Value": topic, + "Depth": queue_size, + "History Policy": "Keep Last", + "Reliability Policy": "Reliable", + "Durability Policy": "Volatile", }, - 'Color': '255; 0; 0', # Red for local path - 'Line Width': 0.05 + "Color": "255; 0; 0", # Red for local path + "Line Width": 0.05, } @classmethod def global_path(cls, topic, color, queue_size=20): return { - 'Class': 'rviz_default_plugins/Path', - 'Name': 'Global Plan', - 'Enabled': True, - 'Topic': { - 'Value': topic, - 'Depth': queue_size, - 'History Policy': 'Keep Last', - 'Reliability Policy': 'Reliable', - 'Durability Policy': 'Volatile', + "Class": "rviz_default_plugins/Path", + "Name": "Global Plan", + "Enabled": True, + "Topic": { + "Value": topic, + "Depth": queue_size, + "History Policy": "Keep Last", + "Reliability Policy": "Reliable", + "Durability Policy": "Volatile", }, - 'Color': color, - 'Line Width': 0.05 + "Color": color, + "Line Width": 0.05, } @classmethod def global_costmap(cls, topic, queue_size=20): return { - 'Class': 'rviz_default_plugins/Map', - 'Name': 'Global Costmap', - 'Enabled': True, - 'Topic': { - 'Value': topic, - 'Depth': queue_size, - 'History Policy': 'Keep Last', - 'Reliability Policy': 'Reliable', - 'Durability Policy': 'Transient Local', + "Class": "rviz_default_plugins/Map", + "Name": "Global Costmap", + "Enabled": True, + "Topic": { + "Value": topic, + "Depth": queue_size, + "History Policy": "Keep Last", + "Reliability Policy": "Reliable", + "Durability Policy": "Transient Local", }, - 'Color Scheme': 'costmap', - 'Draw Behind': False, - 'Alpha': 0.7 + "Color Scheme": "costmap", + "Draw Behind": False, + "Alpha": 0.7, } @classmethod def local_costmap(cls, topic, queue_size=20): return { - 'Class': 'rviz_default_plugins/Map', - 'Name': 'Local Costmap', - 'Enabled': True, - 'Topic': { - 'Value': topic, - 'Depth': queue_size, - 'History Policy': 'Keep Last', - 'Reliability Policy': 'Reliable', - 'Durability Policy': 'Transient Local', + "Class": "rviz_default_plugins/Map", + "Name": "Local Costmap", + "Enabled": True, + "Topic": { + "Value": topic, + "Depth": queue_size, + "History Policy": "Keep Last", + "Reliability Policy": "Reliable", + "Durability Policy": "Transient Local", }, - 'Color Scheme': 'costmap', - 'Draw Behind': False, - 'Alpha': 0.7 + "Color Scheme": "costmap", + "Draw Behind": False, + "Alpha": 0.7, } @classmethod def odom(cls, topic, color, queue_size=20): return { - 'Class': 'rviz_default_plugins/Odometry', - 'Name': 'Odometry', - 'Enabled': True, - 'Topic': { - 'Value': topic, - 'Depth': queue_size, - 'History Policy': 'Keep Last', - 'Reliability Policy': 'Reliable', - 'Durability Policy': 'Volatile', + "Class": "rviz_default_plugins/Odometry", + "Name": "Odometry", + "Enabled": True, + "Topic": { + "Value": topic, + "Depth": queue_size, + "History Policy": "Keep Last", + "Reliability Policy": "Reliable", + "Durability Policy": "Volatile", }, - 'Shape': 'Arrow', - 'Color': color, - 'Position Tolerance': 0.1, - 'Angle Tolerance': 0.1, - 'Keep': 1, - 'Shaft Length': 0.5, - 'Shaft Radius': 0.05, - 'Head Length': 0.2, - 'Head Radius': 0.1 + "Shape": "Arrow", + "Color": color, + "Position Tolerance": 0.1, + "Angle Tolerance": 0.1, + "Keep": 1, + "Shaft Length": 0.5, + "Shaft Radius": 0.05, + "Head Length": 0.2, + "Head Radius": 0.1, } @classmethod def robot_model(cls, topic, robot_name, queue_size=20): return { - 'Class': 'rviz_default_plugins/RobotModel', - 'Name': 'Robot Model', - 'Enabled': True, - 'TF Prefix': robot_name, - 'Description Topic': { - 'Value': topic, - 'Depth': queue_size, - 'History Policy': 'Keep Last', - 'Reliability Policy': 'Reliable', - 'Durability Policy': 'Transient Local', + "Class": "rviz_default_plugins/RobotModel", + "Name": "Robot Model", + "Enabled": True, + "TF Prefix": robot_name, + "Description Topic": { + "Value": topic, + "Depth": queue_size, + "History Policy": "Keep Last", + "Reliability Policy": "Reliable", + "Durability Policy": "Transient Local", }, - 'Visual Enabled': True, - 'Collision Enabled': False + "Visual Enabled": True, + "Collision Enabled": False, } @classmethod def image(cls, topic, queue_size=20): return { - 'Class': 'rviz_default_plugins/Image', - 'Enabled': True, - 'Max Value': 1, - 'Median window': 5, - 'Min Value': 0, - 'Name': 'Image', - 'Normalize Range': False, - 'Topic': { - 'Value': topic, - 'Depth': queue_size, - 'History Policy': 'Keep Last', - 'Reliability Policy': 'Reliable', - 'Durability Policy': 'Volatile', + "Class": "rviz_default_plugins/Image", + "Enabled": True, + "Max Value": 1, + "Median window": 5, + "Min Value": 0, + "Name": "Image", + "Normalize Range": False, + "Topic": { + "Value": topic, + "Depth": queue_size, + "History Policy": "Keep Last", + "Reliability Policy": "Reliable", + "Durability Policy": "Volatile", }, - 'Value': True, + "Value": True, } # Hunavsim-Pedestrian Display Methods @classmethod - def pedestrians(cls, topic, queue_size=20, name: str = "Pedestrians", enabled: bool = True): + def pedestrians( + cls, topic, queue_size=20, name: str = "Pedestrians", enabled: bool = True + ): """ Create a MarkerArray display specifically for pedestrians using people_msgs/msg/People topic. This will be used with a custom node that converts People messages to MarkerArray. """ return { - 'Class': 'rviz_default_plugins/MarkerArray', - 'Name': name, - 'Enabled': enabled, - 'Topic': { - 'Value': topic, - 'Depth': queue_size, - 'History Policy': 'Keep Last', - 'Reliability Policy': 'Best Effort', - 'Durability Policy': 'Volatile', + "Class": "rviz_default_plugins/MarkerArray", + "Name": name, + "Enabled": enabled, + "Topic": { + "Value": topic, + "Depth": queue_size, + "History Policy": "Keep Last", + "Reliability Policy": "Best Effort", + "Durability Policy": "Volatile", }, - 'Namespaces': { - 'pedestrian_bodies': True, - 'pedestrian_heads': True, - 'pedestrian_arrows': True, - 'pedestrian_labels': True + "Namespaces": { + "pedestrian_meshes": True, + "pedestrian_arrows": True, + "pedestrian_labels": True, }, - 'Value': True + "Value": True, } @classmethod @@ -309,17 +310,17 @@ def pedestrians_raw(cls, topic, queue_size=20): Fallback if MarkerArray conversion node is not available """ return { - 'Class': 'rviz_default_plugins/Marker', - 'Name': 'Pedestrians (Raw)', - 'Enabled': True, - 'Topic': { - 'Value': topic, - 'Depth': queue_size, - 'History Policy': 'Keep Last', - 'Reliability Policy': 'Best Effort', - 'Durability Policy': 'Volatile', + "Class": "rviz_default_plugins/Marker", + "Name": "Pedestrians (Raw)", + "Enabled": True, + "Topic": { + "Value": topic, + "Depth": queue_size, + "History Policy": "Keep Last", + "Reliability Policy": "Best Effort", + "Durability Policy": "Volatile", }, - 'Color': '50; 150; 255', # Light blue for pedestrians - 'Alpha': 0.8, - 'Value': True + "Color": "50; 150; 255", # Light blue for pedestrians + "Alpha": 0.8, + "Value": True, }