#include <sdsm_to_detection_list_component.hpp>

Inheritance diagram for carma_cooperative_perception::SdsmToDetectionListNode:

Collaboration diagram for carma_cooperative_perception::SdsmToDetectionListNode:

Public Member Functions
auto	handle_on_configure (const rclcpp_lifecycle::State &) -> carma_ros2_utils::CallbackReturn override

	SdsmToDetectionListNode (const rclcpp::NodeOptions &options)

auto	sdsm_msg_callback (const input_msg_type &msg) const -> void

Private Types
using	input_msg_type = carma_v2x_msgs::msg::SensorDataSharingMessage

using	input_msg_shared_pointer = typename input_msg_type::SharedPtr

using	output_msg_type = carma_cooperative_perception_interfaces::msg::DetectionList

Private Member Functions
rcl_interfaces::msg::SetParametersResult	update_parameters (const std::vector< rclcpp::Parameter > &parameters)

void	update_config_double_parameter (const rclcpp::Parameter &parameter)

Private Attributes
rclcpp::Publisher< output_msg_type >::SharedPtr	publisher_

rclcpp::Subscription< input_msg_type >::SharedPtr	subscription_

rclcpp::Subscription< std_msgs::msg::String >::SharedPtr	georeference_subscription_

rclcpp::Subscription< rosgraph_msgs::msg::Clock >::SharedPtr	cdasim_clock_sub_

std::optional< rclcpp::Time >	cdasim_time_ {std::nullopt}

std::string	georeference_ {""}

SdsmToDetectionListConfig	config_ {}

OnSetParametersCallbackHandle::SharedPtr	on_set_parameters_callback_ {nullptr}

std::unordered_set< std::string >	valid_parameter_names_

Detailed Description

Definition at line 32 of file sdsm_to_detection_list_component.hpp.

Member Typedef Documentation

◆ input_msg_shared_pointer

using carma_cooperative_perception::SdsmToDetectionListNode::input_msg_shared_pointer = typename input_msg_type::SharedPtr

private

Definition at line 35 of file sdsm_to_detection_list_component.hpp.

◆ input_msg_type

using carma_cooperative_perception::SdsmToDetectionListNode::input_msg_type = carma_v2x_msgs::msg::SensorDataSharingMessage

private

Definition at line 34 of file sdsm_to_detection_list_component.hpp.

◆ output_msg_type

using carma_cooperative_perception::SdsmToDetectionListNode::output_msg_type = carma_cooperative_perception_interfaces::msg::DetectionList

private

Definition at line 36 of file sdsm_to_detection_list_component.hpp.

Constructor & Destructor Documentation

◆ SdsmToDetectionListNode()

carma_cooperative_perception::SdsmToDetectionListNode::SdsmToDetectionListNode ( const rclcpp::NodeOptions & options )

inlineexplicit

Definition at line 81 of file sdsm_to_detection_list_component.hpp.

  : CarmaLifecycleNode{options},
    publisher_{create_publisher<output_msg_type>("output/detections", 1)},
    subscription_{create_subscription<input_msg_type>(
      "input/sdsm", 100, [this](input_msg_shared_pointer msg_ptr) { sdsm_msg_callback(*msg_ptr); })},
    georeference_subscription_{create_subscription<std_msgs::msg::String>(
      "input/georeference", 1,
      [this](std_msgs::msg::String::SharedPtr msg_ptr) { georeference_ = msg_ptr->data; })},
    cdasim_clock_sub_{create_subscription<rosgraph_msgs::msg::Clock>(
      "input/cdasim_clock", 1,
      [this](rosgraph_msgs::msg::Clock::ConstSharedPtr msg_ptr) { cdasim_time_ = msg_ptr->clock; })}
  {
    // Initialize the valid parameter names to check when setting the at runtime
    valid_parameter_names_ = {
      "overwrite_covariance",
      "pose_covariance_x",
      "pose_covariance_y",
      "pose_covariance_z",
      "pose_covariance_yaw",
      "twist_covariance_x",
      "twist_covariance_z",
      "twist_covariance_yaw",
      "adjust_pose",
      "x_offset",
      "y_offset",
      "yaw_offset"
    };
  }

References sdsm_msg_callback().

Here is the call graph for this function:

Member Function Documentation

◆ handle_on_configure()

auto carma_cooperative_perception::SdsmToDetectionListNode::handle_on_configure ( const rclcpp_lifecycle::State & ) -> carma_ros2_utils::CallbackReturn

inlineoverride

Definition at line 39 of file sdsm_to_detection_list_component.hpp.

  {
    // Declare parameters
    declare_parameter("overwrite_covariance", config_.overwrite_covariance);
    declare_parameter("pose_covariance_x", config_.pose_covariance_x);
    declare_parameter("pose_covariance_y", config_.pose_covariance_y);
    declare_parameter("pose_covariance_z", config_.pose_covariance_z);
    declare_parameter("pose_covariance_yaw", config_.pose_covariance_yaw);
    declare_parameter("twist_covariance_x", config_.twist_covariance_x);
    declare_parameter("twist_covariance_z", config_.twist_covariance_z);
    declare_parameter("twist_covariance_yaw", config_.twist_covariance_yaw);
    declare_parameter("adjust_pose", config_.adjust_pose);
    declare_parameter("x_offset", config_.x_offset);
    declare_parameter("y_offset", config_.y_offset);
    declare_parameter("yaw_offset", config_.yaw_offset);
 
    // Get parameters
    config_.overwrite_covariance = get_parameter("overwrite_covariance").as_bool();
    config_.pose_covariance_x = get_parameter("pose_covariance_x").as_double();
    config_.pose_covariance_y = get_parameter("pose_covariance_y").as_double();
    config_.pose_covariance_z = get_parameter("pose_covariance_z").as_double();
    config_.pose_covariance_yaw = get_parameter("pose_covariance_yaw").as_double();
    config_.twist_covariance_x = get_parameter("twist_covariance_x").as_double();
    config_.twist_covariance_z = get_parameter("twist_covariance_z").as_double();
    config_.adjust_pose = get_parameter("adjust_pose").as_bool();
    config_.x_offset = get_parameter("x_offset").as_double();
    config_.y_offset = get_parameter("y_offset").as_double();
    config_.yaw_offset = get_parameter("yaw_offset").as_double();
 
    // Set up parameter validation callback
    on_set_parameters_callback_ = add_on_set_parameters_callback(
      [this](const std::vector<rclcpp::Parameter> & parameters) {
        return update_parameters(parameters);
      });
 
    // Use the stream operator for debug output
    RCLCPP_INFO_STREAM(get_logger(), "SdsmToDetectionListNode Configuration: " << config_);
 
    return carma_ros2_utils::CallbackReturn::SUCCESS;
  }

Here is the call graph for this function:

◆ sdsm_msg_callback()

auto carma_cooperative_perception::SdsmToDetectionListNode::sdsm_msg_callback ( const input_msg_type & msg ) const -> void

inline

Definition at line 110 of file sdsm_to_detection_list_component.hpp.

  {
    if (georeference_.empty()){
      RCLCPP_WARN_STREAM(get_logger(), "Georeference not defined yet, ignoring SDSM.");
      return;
    }
 
    try {
      std::optional<SdsmToDetectionListConfig> conversion_adjustment = std::nullopt;
      if (config_.overwrite_covariance || config_.adjust_pose) {
        conversion_adjustment = config_;
      }
      auto detection_list_msg{
        to_detection_list_msg(msg, georeference_, cdasim_time_ != std::nullopt,
          conversion_adjustment)
      };
 
      if (cdasim_time_) {
        // When in simulation, ROS time is CARLA time, but SDSMs use CDASim time
        const auto time_delta{now() - cdasim_time_.value()};
 
        for (auto & detection : detection_list_msg.detections) {
          detection.header.stamp = rclcpp::Time(detection.header.stamp) + time_delta;
        }
      }
      RCLCPP_DEBUG_STREAM(
        get_logger(), "SDSM to detection list conversion: "
        << detection_list_msg.detections.size() << " detections");
      publisher_->publish(detection_list_msg);
    } catch (const std::runtime_error & e) {
      RCLCPP_ERROR_STREAM(get_logger(), "Failed to convert SDSM to detection list: " << e.what());
    }
  }

References carma_cooperative_perception::to_detection_list_msg().

Referenced by SdsmToDetectionListNode().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ update_config_double_parameter()

void carma_cooperative_perception::SdsmToDetectionListNode::update_config_double_parameter ( const rclcpp::Parameter & parameter )

inlineprivate

Definition at line 206 of file sdsm_to_detection_list_component.hpp.

                                                                         {
    const std::string & name = parameter.get_name();
    const double value = parameter.as_double();
 
    if (name == "pose_covariance_x") {
      config_.pose_covariance_x = value;
    } else if (name == "pose_covariance_y") {
      config_.pose_covariance_y = value;
    } else if (name == "pose_covariance_z") {
      config_.pose_covariance_z = value;
    } else if (name == "pose_covariance_yaw") {
      config_.pose_covariance_yaw = value;
    } else if (name == "twist_covariance_x") {
      config_.twist_covariance_x = value;
    } else if (name == "twist_covariance_z") {
      config_.twist_covariance_z = value;
    } else if (name == "twist_covariance_yaw") {
      config_.twist_covariance_yaw = value;
    } else if (name == "x_offset") {
      config_.x_offset = value;
    } else if (name == "y_offset") {
      config_.y_offset = value;
    } else if (name == "yaw_offset") {
      config_.yaw_offset = value;
    }
  }

◆ update_parameters()

rcl_interfaces::msg::SetParametersResult carma_cooperative_perception::SdsmToDetectionListNode::update_parameters ( const std::vector< rclcpp::Parameter > & parameters )

inlineprivate

Definition at line 160 of file sdsm_to_detection_list_component.hpp.

  {
    rcl_interfaces::msg::SetParametersResult result;
    result.successful = true;
    result.reason = "success";
 
    for (const auto & parameter : parameters) {
      // Check if parameter name is valid
      if (valid_parameter_names_.find(parameter.get_name()) == valid_parameter_names_.end()) {
        result.successful = false;
        result.reason = "Unexpected parameter name '" + parameter.get_name() + "'";
        RCLCPP_ERROR_STREAM(get_logger(), "Cannot change parameter: " << result.reason);
        break;
      }
 
      // Validate covariance values (must be non-negative)
      if (parameter.get_name() != "adjust_pose" &&
          parameter.get_name() != "overwrite_covariance") {
        if (const auto value{parameter.as_double()}; value < 0.0) {
          result.successful = false;
          result.reason = "Covariance parameter must be non-negative";
          RCLCPP_ERROR_STREAM(
            get_logger(),
            "Cannot change parameter '" << parameter.get_name() << "': " << result.reason);
          break;
        } else {
          // Update the appropriate parameter in the config
          update_config_double_parameter(parameter);
        }
      } else if (parameter.get_name() == "overwrite_covariance"){
        config_.overwrite_covariance = parameter.as_bool();
      } else if (parameter.get_name() == "adjust_pose"){
        config_.adjust_pose = parameter.as_bool();
      }
    }
 
    // If parameters were successfully updated, log the new configuration
    if (result.successful) {
      RCLCPP_DEBUG_STREAM(get_logger(), "Updated configuration: " << config_);
    }
 
    return result;
  }