This node provides RViz visualization messages for recieved ExternalObject and RoadwayObstacle messages in carma-platform. More...

#include <object_visualizer_node.hpp>

Inheritance diagram for object_visualizer::Node:

Collaboration diagram for object_visualizer::Node:

Public Member Functions
	Node (const rclcpp::NodeOptions &)
	Node constructor. More...

rcl_interfaces::msg::SetParametersResult	parameter_update_callback (const std::vector< rclcpp::Parameter > &parameters)
	Example callback for dynamic parameter updates. More...

void	external_objects_callback (carma_perception_msgs::msg::ExternalObjectList::UniquePtr msg)
	External objects callback. Converts the message to a visualization message to republish. More...

void	roadway_obstacles_callback (carma_perception_msgs::msg::RoadwayObstacleList::UniquePtr msg)
	Roadway obstacles callback. Converts the message to a visualization message to republish. More...

carma_ros2_utils::CallbackReturn	handle_on_configure (const rclcpp_lifecycle::State &)

Private Member Functions
void	createPedestrianMarker (visualization_msgs::msg::Marker &marker, const std_msgs::msg::Header &header, const geometry_msgs::msg::Pose &pose, size_t id, const std::string &ns, const geometry_msgs::msg::Vector3 &size)
	Creates a pedestrian marker with specialized visualization. More...

void	clear_and_update_old_objects (visualization_msgs::msg::MarkerArray &viz_msg, size_t &old_size)
	Adds a marker deletion for a number of markers which is the delta between the new marker count and the previously published count. More...

Private Attributes
carma_ros2_utils::SubPtr< carma_perception_msgs::msg::ExternalObjectList >	external_objects_sub_

carma_ros2_utils::SubPtr< carma_perception_msgs::msg::RoadwayObstacleList >	roadway_obstacles_sub_

carma_ros2_utils::PubPtr< visualization_msgs::msg::MarkerArray >	external_objects_viz_pub_

carma_ros2_utils::PubPtr< visualization_msgs::msg::MarkerArray >	roadway_obstacles_viz_pub_

Config	config_

size_t	prev_external_objects_size_ = 0

size_t	prev_roadway_obstacles_size_ = 0

Detailed Description

This node provides RViz visualization messages for recieved ExternalObject and RoadwayObstacle messages in carma-platform.

Definition at line 38 of file object_visualizer_node.hpp.

Constructor & Destructor Documentation

◆ Node()

object_visualizer::Node::Node ( const rclcpp::NodeOptions & options )

explicit

Node constructor.

Definition at line 22 of file object_visualizer_node.cpp.

      : carma_ros2_utils::CarmaLifecycleNode(options)
  {
    // Create initial config
    config_ = Config();
 
    // Declare parameters
    config_.enable_external_objects_viz = declare_parameter<bool>("enable_external_objects_viz", config_.enable_external_objects_viz);
    config_.enable_roadway_objects_viz = declare_parameter<bool>("enable_roadway_objects_viz", config_.enable_roadway_objects_viz);
    config_.external_objects_viz_ns = declare_parameter<std::string>("external_objects_viz_ns", config_.external_objects_viz_ns);
    config_.roadway_obstacles_viz_ns = declare_parameter<std::string>("roadway_obstacles_viz_ns", config_.roadway_obstacles_viz_ns);
    config_.marker_shape = declare_parameter<uint8_t>("marker_shape", config_.marker_shape);
 
    // Declare new parameters for pedestrian visualization
    config_.use_pedestrian_icon = declare_parameter<bool>("use_pedestrian_icon", config_.use_pedestrian_icon);
    config_.pedestrian_icon_path = declare_parameter<std::string>("pedestrian_icon_path", config_.pedestrian_icon_path);
    config_.pedestrian_icon_scale = declare_parameter<double>("pedestrian_icon_scale", config_.pedestrian_icon_scale);
  }

References config_, object_visualizer::Config::enable_external_objects_viz, object_visualizer::Config::enable_roadway_objects_viz, object_visualizer::Config::external_objects_viz_ns, object_visualizer::Config::marker_shape, object_visualizer::Config::pedestrian_icon_path, object_visualizer::Config::pedestrian_icon_scale, object_visualizer::Config::roadway_obstacles_viz_ns, and object_visualizer::Config::use_pedestrian_icon.

Member Function Documentation

◆ clear_and_update_old_objects()

void object_visualizer::Node::clear_and_update_old_objects	(	visualization_msgs::msg::MarkerArray &	viz_msg,
		size_t &	old_size
	)

private

Adds a marker deletion for a number of markers which is the delta between the new marker count and the previously published count.

ASSUMPTION: The marker array's ids are already recomputed starting from zero, such that tracking ids is not needed.

Parameters

[in/out]	viz_msg The marker array to add deletion markers to
[in/out]	old_size The size of the previous published marker array. The positive delta between this and the size of the viz_msg determines the number of deletions added

Definition at line 220 of file object_visualizer_node.cpp.

  {
    size_t size_swap = viz_msg.markers.size();
 
    // Delete any markers from the previous message that are no longer in the current message
    if (viz_msg.markers.size() < old_size) {
      for (size_t i = viz_msg.markers.size(); i < old_size; ++i) {
        visualization_msgs::msg::Marker marker;
        marker.id = i;
        marker.action = visualization_msgs::msg::Marker::DELETE;
        viz_msg.markers.push_back(marker);
      }
    }
 
    old_size = size_swap; // Assign the new size of added objects to the old size
  }

References process_bag::i.

Referenced by external_objects_callback(), and roadway_obstacles_callback().

Here is the caller graph for this function:

◆ createPedestrianMarker()

void object_visualizer::Node::createPedestrianMarker	(	visualization_msgs::msg::Marker &	marker,
		const std_msgs::msg::Header &	header,
		const geometry_msgs::msg::Pose &	pose,
		size_t	id,
		const std::string &	ns,
		const geometry_msgs::msg::Vector3 &	size
	)

private

Creates a pedestrian marker with specialized visualization.

Parameters

[out]	marker	The marker to be configured for pedestrian visualization
[in]	header	The header to use for the marker
[in]	pose	The pose to use for the marker
[in]	id	The ID for the marker
[in]	ns	The namespace for the marker
[in]	size	The size vector for the pedestrian (x, y, z)

Definition at line 108 of file object_visualizer_node.cpp.

  {
    marker.header = header;
    marker.ns = ns;
    marker.id = id;
    marker.pose = pose;
 
    if (config_.use_pedestrian_icon && !config_.pedestrian_icon_path.empty()) {
      // Use a mesh resource marker for pedestrian representation
      marker.type = visualization_msgs::msg::Marker::MESH_RESOURCE;
      marker.mesh_resource = config_.pedestrian_icon_path;
      marker.mesh_use_embedded_materials = true;
 
      // Scale the pedestrian mesh appropriately
      marker.scale.x = config_.pedestrian_icon_scale;
      marker.scale.y = - config_.pedestrian_icon_scale; // Invert Y to fix the STL orientation
      marker.scale.z = config_.pedestrian_icon_scale;
 
      // White color for the pedestrian icon
      marker.color.r = 1.0;
      marker.color.g = 1.0;
      marker.color.b = 1.0;
      marker.color.a = 1.0;
    } else {
      // Fallback to a person-shaped marker using cylinder (if mesh isn't available)
      marker.type = visualization_msgs::msg::Marker::CYLINDER;
 
      // Use the provided size or default to a human-like proportion
      marker.scale.x = std::max(0.5, size.x * 2.0);  // Width
      marker.scale.y = std::max(0.5, size.y * 2.0);  // Depth
      marker.scale.z = std::max(1.7, size.z * 2.0);  // Height - typical human height
 
      // Set the color to red for the pedestrian marker
      marker.color.r = 1.0;
      marker.color.g = 0.0;
      marker.color.b = 0.0;
      marker.color.a = 1.0;
    }
 
    marker.action = visualization_msgs::msg::Marker::ADD;
  }

References config_, object_visualizer::Config::pedestrian_icon_path, object_visualizer::Config::pedestrian_icon_scale, and object_visualizer::Config::use_pedestrian_icon.

Referenced by external_objects_callback(), and roadway_obstacles_callback().

Here is the caller graph for this function:

◆ external_objects_callback()

void object_visualizer::Node::external_objects_callback ( carma_perception_msgs::msg::ExternalObjectList::UniquePtr msg )

External objects callback. Converts the message to a visualization message to republish.

Parameters

msg	The received message

Definition at line 155 of file object_visualizer_node.cpp.

  {
    RCLCPP_DEBUG_STREAM(get_logger(), "external_objects_callback called");
 
    if (!config_.enable_external_objects_viz) {
      RCLCPP_DEBUG_STREAM(get_logger(), "external_objects_callback called, but visualization is not enabled.");
      return;
    }
 
    visualization_msgs::msg::MarkerArray viz_msg;
    //delete all markers before adding new ones
    visualization_msgs::msg::Marker marker;
    viz_msg.markers.reserve(msg->objects.size() + 1); //+1 to account for delete all marker
    marker.id = 0;
    marker.action = visualization_msgs::msg::Marker::DELETEALL;
    viz_msg.markers.push_back(marker);
 
 
    size_t id = 0; // We always count the id from zero so we can delete markers later in a consistent manner
 
    for (auto obj : msg->objects) {
      visualization_msgs::msg::Marker marker;
 
      // Pedestrian's will be represented as specialized icon/marker
      if (obj.object_type == carma_perception_msgs::msg::ExternalObject::PEDESTRIAN) {
        createPedestrianMarker(marker, msg->header, obj.pose.pose, id, config_.external_objects_viz_ns, obj.size);
 
        // Adjust Z position to prevent clipping with ground
        marker.pose.position.z = std::max(1.0, obj.size.z);
      }
      // All other detected objects will use the configured marker shape
      else {
        marker.header = msg->header;
        marker.ns = config_.external_objects_viz_ns;
        marker.color.r = 0.0;
        marker.color.g = 0.0;
        marker.color.b = 1.0;
        marker.color.a = 1.0;
 
        marker.pose = obj.pose.pose;
        marker.pose.position.z = std::max(1.0, obj.size.z); // lifts the marker above ground so that it doesn't clip
 
        marker.id = id;
        if (static_cast<int>(config_.marker_shape) < 1 || static_cast<int>(config_.marker_shape) > 3)
        {
          throw std::invalid_argument("Marker shape is not valid: " + std::to_string(static_cast<int>(config_.marker_shape)) + ". Please choose from 1: CUBE, 2: SPHERE, 3: CYLINDER");
        }
        marker.type = config_.marker_shape;
        marker.action = visualization_msgs::msg::Marker::ADD;
 
        // overwrite size in case any previous stack doesn't provide size
        // such as carma_cooperative_perception at the moment
        marker.scale.x = std::max(1.0, obj.size.x) * 2;  // Size in carma is half the length/width/height
        marker.scale.y = std::max(1.0, obj.size.y) * 2;
        marker.scale.z = std::max(1.0, obj.size.z) * 2;
      }
 
      viz_msg.markers.push_back(marker);
      id++;
    }
 
    clear_and_update_old_objects(viz_msg, prev_external_objects_size_);
    external_objects_viz_pub_->publish(viz_msg);
  }

References clear_and_update_old_objects(), config_, createPedestrianMarker(), object_visualizer::Config::enable_external_objects_viz, object_visualizer::Config::external_objects_viz_ns, external_objects_viz_pub_, object_visualizer::Config::marker_shape, prev_external_objects_size_, and carma_cooperative_perception::to_string().

Referenced by handle_on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ handle_on_configure()

carma_ros2_utils::CallbackReturn object_visualizer::Node::handle_on_configure ( const rclcpp_lifecycle::State & )

Definition at line 75 of file object_visualizer_node.cpp.

  {
    // Reset config
    config_ = Config();
 
    // Load parameters
    get_parameter<bool>("enable_external_objects_viz", config_.enable_external_objects_viz);
    get_parameter<bool>("enable_roadway_objects_viz", config_.enable_roadway_objects_viz);
    get_parameter<std::string>("external_objects_viz_ns", config_.external_objects_viz_ns);
    get_parameter<std::string>("roadway_obstacles_viz_ns", config_.roadway_obstacles_viz_ns);
    get_parameter<uint8_t>("marker_shape", config_.marker_shape);
    get_parameter<bool>("use_pedestrian_icon", config_.use_pedestrian_icon);
    get_parameter<std::string>("pedestrian_icon_path", config_.pedestrian_icon_path);
    get_parameter<double>("pedestrian_icon_scale", config_.pedestrian_icon_scale);
 
    // Register runtime parameter update callback
    add_on_set_parameters_callback(std::bind(&Node::parameter_update_callback, this, std_ph::_1));
 
    // Setup subscribers
    external_objects_sub_ = create_subscription<carma_perception_msgs::msg::ExternalObjectList>("external_objects", 10,
                                                              std::bind(&Node::external_objects_callback, this, std_ph::_1));
 
    roadway_obstacles_sub_ = create_subscription<carma_perception_msgs::msg::RoadwayObstacleList>("roadway_obstacles", 10,
                                                              std::bind(&Node::roadway_obstacles_callback, this, std_ph::_1));
 
    // Setup publishers
    external_objects_viz_pub_ = create_publisher<visualization_msgs::msg::MarkerArray>("external_objects_viz", 10);
    roadway_obstacles_viz_pub_ = create_publisher<visualization_msgs::msg::MarkerArray>("roadway_obstacles_viz", 10);
 
    // Return success if everthing initialized successfully
    return CallbackReturn::SUCCESS;
  }

Here is the call graph for this function:

◆ parameter_update_callback()

rcl_interfaces::msg::SetParametersResult object_visualizer::Node::parameter_update_callback ( const std::vector< rclcpp::Parameter > & parameters )

Example callback for dynamic parameter updates.

Definition at line 41 of file object_visualizer_node.cpp.

  {
    // Update parameters
    auto error = update_params<bool>(
      {
        {"enable_external_objects_viz", config_.enable_external_objects_viz},
        {"enable_roadway_objects_viz", config_.enable_roadway_objects_viz},
        {"use_pedestrian_icon", config_.use_pedestrian_icon}
      }, parameters);
 
    auto error2 = update_params<std::string>(
      {
        {"external_objects_viz_ns", config_.external_objects_viz_ns},
        {"roadway_obstacles_viz_ns", config_.roadway_obstacles_viz_ns},
        {"pedestrian_icon_path", config_.pedestrian_icon_path}
      }, parameters);
 
    auto error3 = update_params<uint8_t>(
      {
        {"marker_shape", config_.marker_shape}
      }, parameters);
 
    auto error4 = update_params<double>(
      {
        {"pedestrian_icon_scale", config_.pedestrian_icon_scale}
      }, parameters);
 
    rcl_interfaces::msg::SetParametersResult result;
 
    result.successful = !error && !error2 && !error3 && !error4;
 
    return result;
  }

References config_, object_visualizer::Config::enable_external_objects_viz, object_visualizer::Config::enable_roadway_objects_viz, object_visualizer::Config::external_objects_viz_ns, object_visualizer::Config::marker_shape, object_visualizer::Config::pedestrian_icon_path, object_visualizer::Config::pedestrian_icon_scale, object_visualizer::Config::roadway_obstacles_viz_ns, and object_visualizer::Config::use_pedestrian_icon.

Referenced by handle_on_configure().

Here is the caller graph for this function:

◆ roadway_obstacles_callback()

void object_visualizer::Node::roadway_obstacles_callback ( carma_perception_msgs::msg::RoadwayObstacleList::UniquePtr msg )

Roadway obstacles callback. Converts the message to a visualization message to republish.

Parameters

msg	The received message

Definition at line 237 of file object_visualizer_node.cpp.

  {
    RCLCPP_DEBUG_STREAM(get_logger(), "roadway_obstacles_callback called");
 
    if (!config_.enable_roadway_objects_viz) {
      RCLCPP_DEBUG_STREAM(get_logger(), "roadway_obstacles_callback called, but visualization is not enabled.");
      return;
    }
 
    visualization_msgs::msg::MarkerArray viz_msg;
    //delete all markers before adding new ones
    visualization_msgs::msg::Marker marker;
    viz_msg.markers.reserve(msg->roadway_obstacles.size() + 1); //+1 to account for delete all marker
    marker.id = 0;
    marker.action = visualization_msgs::msg::Marker::DELETEALL;
    viz_msg.markers.push_back(marker);
 
    size_t id = 0; // We always count the id from zero so we can delete markers later in a consistent manner
    for (auto obj : msg->roadway_obstacles) {
      visualization_msgs::msg::Marker marker;
 
      // Pedestrian's will be represented with a specialized icon
      if (obj.object.object_type == carma_perception_msgs::msg::ExternalObject::PEDESTRIAN) {
        createPedestrianMarker(marker, obj.object.header, obj.object.pose.pose, id,
                              config_.roadway_obstacles_viz_ns, obj.object.size);
      }
      // Connected vehicles will be green
      else if (obj.connected_vehicle_type.type != carma_perception_msgs::msg::ConnectedVehicleType::NOT_CONNECTED) {
        marker.header = obj.object.header;
        marker.ns = config_.roadway_obstacles_viz_ns;
        marker.color.r = 0.0;
        marker.color.g = 1.0;
        marker.color.b = 0.0;
        marker.color.a = 1.0;
 
        marker.id = id;
        marker.type = visualization_msgs::msg::Marker::CUBE;
        marker.action = visualization_msgs::msg::Marker::ADD;
        marker.pose = obj.object.pose.pose;
        marker.scale.x = obj.object.size.x * 2.0; // Size in carma is half the length/width/height
        marker.scale.y = obj.object.size.y * 2.0;
        marker.scale.z = obj.object.size.z * 2.0;
      }
      // All other detected objects will be blue
      else {
        marker.header = obj.object.header;
        marker.ns = config_.roadway_obstacles_viz_ns;
        marker.color.r = 0.0;
        marker.color.g = 0.0;
        marker.color.b = 1.0;
        marker.color.a = 1.0;
 
        marker.id = id;
        marker.type = visualization_msgs::msg::Marker::CUBE;
        marker.action = visualization_msgs::msg::Marker::ADD;
        marker.pose = obj.object.pose.pose;
        marker.scale.x = obj.object.size.x * 2.0;
        marker.scale.y = obj.object.size.y * 2.0;
        marker.scale.z = obj.object.size.z * 2.0;
      }
 
      viz_msg.markers.push_back(marker);
      id++;
    }
 
    clear_and_update_old_objects(viz_msg, prev_roadway_obstacles_size_);
    roadway_obstacles_viz_pub_->publish(viz_msg);
  }