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	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 36 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);
 
  }

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, and object_visualizer::Config::roadway_obstacles_viz_ns.

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 166 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:

◆ 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 94 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;
 
      marker.header = msg->header;
 
      marker.ns = config_.external_objects_viz_ns;
 
      // Pedestrian's will be represented as red box
      if (obj.object_type == carma_perception_msgs::msg::ExternalObject::PEDESTRIAN) {
 
        marker.color.r = 1.0;
        marker.color.g = 0.0;
        marker.color.b = 0.0;
        marker.color.a = 1.0;
 
      } // All other detected objects will be blue
      else {
 
        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_, 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 64 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);
 
    // 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;
  }

References config_, object_visualizer::Config::enable_external_objects_viz, object_visualizer::Config::enable_roadway_objects_viz, external_objects_callback(), external_objects_sub_, object_visualizer::Config::external_objects_viz_ns, external_objects_viz_pub_, object_visualizer::Config::marker_shape, parameter_update_callback(), roadway_obstacles_callback(), roadway_obstacles_sub_, object_visualizer::Config::roadway_obstacles_viz_ns, and roadway_obstacles_viz_pub_.

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 37 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}
      }, 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}
      }, parameters);
 
    auto error3 = update_params<uint8_t>(
      {
        {"marker_shape", config_.marker_shape}
      }, parameters);
 
    rcl_interfaces::msg::SetParametersResult result;
 
    result.successful = !error && !error2 && !error3;
 
    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, and object_visualizer::Config::roadway_obstacles_viz_ns.

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 185 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;
 
      marker.header = obj.object.header;
 
      marker.ns = config_.roadway_obstacles_viz_ns;
 
      // Pedestrian's will be represented as a red box
      if (obj.object.object_type == carma_perception_msgs::msg::ExternalObject::PEDESTRIAN) {
 
        marker.color.r = 1.0;
        marker.color.g = 0.0;
        marker.color.b = 0.0;
        marker.color.a = 1.0;
 
      } // Connected vehicles will be green
      else if (obj.connected_vehicle_type.type != carma_perception_msgs::msg::ConnectedVehicleType::NOT_CONNECTED){
        marker.color.r = 0.0;
        marker.color.g = 1.0;
        marker.color.b = 0.0;
        marker.color.a = 1.0;
      } // All other detected objects will be blue
      else {
 
        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; // 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;
 
      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);
 
  }