motion_computation::MotionComputationNode Class Reference

The class responsible for publishing external object predictions. More...

#include <motion_computation_node.hpp>

Inheritance diagram for motion_computation::MotionComputationNode:

Collaboration diagram for motion_computation::MotionComputationNode:

Public Member Functions
	MotionComputationNode (const rclcpp::NodeOptions &)
	MotionComputationNode constructor. More...
rcl_interfaces::msg::SetParametersResult	parameter_update_callback (const std::vector< rclcpp::Parameter > &parameters)
	Function callback for dynamic parameter updates. More...
void	publishObject (const carma_perception_msgs::msg::ExternalObjectList &obj_pred_msg) const
	Function to publish ExternalObjectList. More...
carma_ros2_utils::CallbackReturn	handle_on_configure (const rclcpp_lifecycle::State &)

Private Attributes
carma_ros2_utils::SubPtr< carma_perception_msgs::msg::ExternalObjectList >	motion_comp_sub_
carma_ros2_utils::SubPtr< carma_v2x_msgs::msg::MobilityPath >	mobility_path_sub_
carma_ros2_utils::SubPtr< carma_v2x_msgs::msg::BSM >	bsm_sub_
carma_ros2_utils::SubPtr< carma_v2x_msgs::msg::PSM >	psm_sub_
carma_ros2_utils::SubPtr< std_msgs::msg::String >	georeference_sub_
carma_ros2_utils::PubPtr< carma_perception_msgs::msg::ExternalObjectList >	carma_obj_pub_
MotionComputationWorker	motion_worker_
Config	config_

Detailed Description

The class responsible for publishing external object predictions.

Definition at line 39 of file motion_computation_node.hpp.

Constructor & Destructor Documentation

MotionComputationNode()

motion_computation::MotionComputationNode::MotionComputationNode ( const rclcpp::NodeOptions &  options )

explicit

MotionComputationNode constructor.

Definition at line 21 of file motion_computation_node.cpp.

: carma_ros2_utils::CarmaLifecycleNode(options),
  motion_worker_(
    std::bind(&MotionComputationNode::publishObject, this, std_ph::_1),
    get_node_logging_interface(), get_node_clock_interface())
{
  // Create initial config
  config_ = Config();
 
  // Declare parameters
  config_.prediction_time_step =
    declare_parameter<double>("prediction_time_step", config_.prediction_time_step);
  config_.prediction_period =
    declare_parameter<double>("prediction_period", config_.prediction_period);
  config_.cv_x_accel_noise =
    declare_parameter<double>("cv_x_accel_noise", config_.cv_x_accel_noise);
  config_.cv_y_accel_noise =
    declare_parameter<double>("cv_y_accel_noise", config_.cv_y_accel_noise);
  config_.prediction_process_noise_max =
    declare_parameter<double>("prediction_process_noise_max", config_.prediction_process_noise_max);
  config_.prediction_confidence_drop_rate = declare_parameter<double>(
    "prediction_confidence_drop_rate", config_.prediction_confidence_drop_rate);
  config_.enable_bsm_processing =
    declare_parameter<bool>("enable_bsm_processing", config_.enable_bsm_processing);
  config_.enable_psm_processing =
    declare_parameter<bool>("enable_psm_processing", config_.enable_psm_processing);
  config_.enable_mobility_path_processing = declare_parameter<bool>(
    "enable_mobility_path_processing", config_.enable_mobility_path_processing);
  config_.enable_sensor_processing =
    declare_parameter<bool>("enable_sensor_processing", config_.enable_sensor_processing);
  config_.enable_ctrv_for_unknown_obj =
    declare_parameter<bool>("enable_ctrv_for_unknown_obj", config_.enable_ctrv_for_unknown_obj);
  config_.enable_ctrv_for_motorcycle_obj = declare_parameter<bool>(
    "enable_ctrv_for_motorcycle_obj", config_.enable_ctrv_for_motorcycle_obj);
  config_.enable_ctrv_for_small_vehicle_obj = declare_parameter<bool>(
    "enable_ctrv_for_small_vehicle_obj", config_.enable_ctrv_for_small_vehicle_obj);
  config_.enable_ctrv_for_large_vehicle_obj = declare_parameter<bool>(
    "enable_ctrv_for_large_vehicle_obj", config_.enable_ctrv_for_large_vehicle_obj);
  config_.enable_ctrv_for_pedestrian_obj = declare_parameter<bool>(
    "enable_ctrv_for_pedestrian_obj", config_.enable_ctrv_for_pedestrian_obj);
}

References config_, motion_computation::Config::cv_x_accel_noise, motion_computation::Config::cv_y_accel_noise, motion_computation::Config::enable_bsm_processing, motion_computation::Config::enable_ctrv_for_large_vehicle_obj, motion_computation::Config::enable_ctrv_for_motorcycle_obj, motion_computation::Config::enable_ctrv_for_pedestrian_obj, motion_computation::Config::enable_ctrv_for_small_vehicle_obj, motion_computation::Config::enable_ctrv_for_unknown_obj, motion_computation::Config::enable_mobility_path_processing, motion_computation::Config::enable_psm_processing, motion_computation::Config::enable_sensor_processing, motion_computation::Config::prediction_confidence_drop_rate, motion_computation::Config::prediction_period, motion_computation::Config::prediction_process_noise_max, and motion_computation::Config::prediction_time_step.

Member Function Documentation

handle_on_configure()

carma_ros2_utils::CallbackReturn motion_computation::MotionComputationNode::handle_on_configure

(

const rclcpp_lifecycle::State &

)

Definition at line 111 of file motion_computation_node.cpp.

{
  RCLCPP_INFO_STREAM(get_logger(), "MotionComputationNode trying to configure");
 
  // Reset config
  config_ = Config();
 
  // Load parameters
  get_parameter<double>("prediction_time_step", config_.prediction_time_step);
  get_parameter<double>("prediction_period", config_.prediction_period);
  get_parameter<double>("cv_x_accel_noise", config_.cv_x_accel_noise);
  get_parameter<double>("cv_y_accel_noise", config_.cv_y_accel_noise);
  get_parameter<double>("prediction_process_noise_max", config_.prediction_process_noise_max);
  get_parameter<double>("prediction_confidence_drop_rate", config_.prediction_confidence_drop_rate);
  get_parameter<bool>("enable_bsm_processing", config_.enable_bsm_processing);
  get_parameter<bool>("enable_psm_processing", config_.enable_psm_processing);
  get_parameter<bool>("enable_mobility_path_processing", config_.enable_mobility_path_processing);
  get_parameter<bool>("enable_sensor_processing", config_.enable_sensor_processing);
  get_parameter<bool>("enable_ctrv_for_unknown_obj", config_.enable_ctrv_for_unknown_obj);
  get_parameter<bool>("enable_ctrv_for_motorcycle_obj", config_.enable_ctrv_for_motorcycle_obj);
  get_parameter<bool>(
    "enable_ctrv_for_small_vehicle_obj", config_.enable_ctrv_for_small_vehicle_obj);
  get_parameter<bool>(
    "enable_ctrv_for_large_vehicle_obj", config_.enable_ctrv_for_large_vehicle_obj);
  get_parameter<bool>("enable_ctrv_for_pedestrian_obj", config_.enable_ctrv_for_pedestrian_obj);
 
  RCLCPP_INFO_STREAM(get_logger(), "Loaded params: " << config_);
 
  // Register runtime parameter update callback
  add_on_set_parameters_callback(
    std::bind(&MotionComputationNode::parameter_update_callback, this, std_ph::_1));
 
  // Setup subscribers
  motion_comp_sub_ = create_subscription<carma_perception_msgs::msg::ExternalObjectList>(
    "external_objects", 1,
    std::bind(&MotionComputationWorker::predictionLogic, &motion_worker_, std_ph::_1));
 
  mobility_path_sub_ = create_subscription<carma_v2x_msgs::msg::MobilityPath>(
    "incoming_mobility_path", 100,
    std::bind(&MotionComputationWorker::mobilityPathCallback, &motion_worker_, std_ph::_1));
 
  bsm_sub_ = create_subscription<carma_v2x_msgs::msg::BSM>(
    "incoming_bsm", 100,
    std::bind(&MotionComputationWorker::bsmCallback, &motion_worker_, std_ph::_1));
 
  psm_sub_ = create_subscription<carma_v2x_msgs::msg::PSM>(
    "incoming_psm", 100,
    std::bind(&MotionComputationWorker::psmCallback, &motion_worker_, std_ph::_1));
 
  georeference_sub_ = create_subscription<std_msgs::msg::String>(
    "georeference", 1,
    std::bind(&MotionComputationWorker::georeferenceCallback, &motion_worker_, std_ph::_1));
 
  // Setup publishers
  carma_obj_pub_ = create_publisher<carma_perception_msgs::msg::ExternalObjectList>(
    "external_object_predictions", 2);
 
  // Set motion_worker_'s prediction parameters
  motion_worker_.setPredictionTimeStep(config_.prediction_time_step);
  motion_worker_.setPredictionPeriod(config_.prediction_period);
  motion_worker_.setXAccelerationNoise(config_.cv_x_accel_noise);
  motion_worker_.setYAccelerationNoise(config_.cv_y_accel_noise);
  motion_worker_.setProcessNoiseMax(config_.prediction_process_noise_max);
  motion_worker_.setConfidenceDropRate(config_.prediction_confidence_drop_rate);
  motion_worker_.setDetectionInputFlags(
    config_.enable_sensor_processing, config_.enable_bsm_processing, config_.enable_psm_processing,
    config_.enable_mobility_path_processing);
 
  // Return success if everything initialized successfully
  return CallbackReturn::SUCCESS;
}

References bsm_sub_, motion_computation::MotionComputationWorker::bsmCallback(), carma_obj_pub_, config_, motion_computation::Config::cv_x_accel_noise, motion_computation::Config::cv_y_accel_noise, motion_computation::Config::enable_bsm_processing, motion_computation::Config::enable_ctrv_for_large_vehicle_obj, motion_computation::Config::enable_ctrv_for_motorcycle_obj, motion_computation::Config::enable_ctrv_for_pedestrian_obj, motion_computation::Config::enable_ctrv_for_small_vehicle_obj, motion_computation::Config::enable_ctrv_for_unknown_obj, motion_computation::Config::enable_mobility_path_processing, motion_computation::Config::enable_psm_processing, motion_computation::Config::enable_sensor_processing, georeference_sub_, motion_computation::MotionComputationWorker::georeferenceCallback(), mobility_path_sub_, motion_computation::MotionComputationWorker::mobilityPathCallback(), motion_comp_sub_, motion_worker_, parameter_update_callback(), motion_computation::Config::prediction_confidence_drop_rate, motion_computation::Config::prediction_period, motion_computation::Config::prediction_process_noise_max, motion_computation::Config::prediction_time_step, motion_computation::MotionComputationWorker::predictionLogic(), psm_sub_, motion_computation::MotionComputationWorker::psmCallback(), motion_computation::MotionComputationWorker::setConfidenceDropRate(), motion_computation::MotionComputationWorker::setDetectionInputFlags(), motion_computation::MotionComputationWorker::setPredictionPeriod(), motion_computation::MotionComputationWorker::setPredictionTimeStep(), motion_computation::MotionComputationWorker::setProcessNoiseMax(), motion_computation::MotionComputationWorker::setXAccelerationNoise(), and motion_computation::MotionComputationWorker::setYAccelerationNoise().

Here is the call graph for this function:

parameter_update_callback()

rcl_interfaces::msg::SetParametersResult motion_computation::MotionComputationNode::parameter_update_callback

(

const std::vector< rclcpp::Parameter > &

parameters

)

Function callback for dynamic parameter updates.

Definition at line 63 of file motion_computation_node.cpp.

{
  auto error = update_params<double>(
    {{"prediction_time_step", config_.prediction_time_step},
     {"prediction_period", config_.prediction_period},
     {"cv_x_accel_noise", config_.cv_x_accel_noise},
     {"cv_y_accel_noise", config_.cv_y_accel_noise},
     {"prediction_process_noise_max", config_.prediction_process_noise_max},
     {"prediction_confidence_drop_rate", config_.prediction_confidence_drop_rate}},
    parameters);
 
  auto error_2 = update_params<bool>(
    {{"enable_bsm_processing", config_.enable_bsm_processing},
     {"enable_psm_processing", config_.enable_psm_processing},
     {"enable_mobility_path_processing", config_.enable_mobility_path_processing},
     {"enable_sensor_processing", config_.enable_sensor_processing},
     {"enable_ctrv_for_unknown_obj", config_.enable_ctrv_for_unknown_obj},
     {"enable_ctrv_for_motorcycle_obj", config_.enable_ctrv_for_motorcycle_obj},
     {"enable_ctrv_for_small_vehicle_obj", config_.enable_ctrv_for_small_vehicle_obj},
     {"enable_ctrv_for_large_vehicle_obj", config_.enable_ctrv_for_large_vehicle_obj},
     {"enable_ctrv_for_pedestrian_obj", config_.enable_ctrv_for_pedestrian_obj}},
    parameters);
 
  rcl_interfaces::msg::SetParametersResult result;
 
  result.successful = !error && !error_2;
 
  if (result.successful) {
    // Set motion_worker_'s prediction parameters
    motion_worker_.setPredictionTimeStep(config_.prediction_time_step);
    motion_worker_.setPredictionPeriod(config_.prediction_period);
    motion_worker_.setXAccelerationNoise(config_.cv_x_accel_noise);
    motion_worker_.setYAccelerationNoise(config_.cv_y_accel_noise);
    motion_worker_.setProcessNoiseMax(config_.prediction_process_noise_max);
    motion_worker_.setConfidenceDropRate(config_.prediction_confidence_drop_rate);
    motion_worker_.setDetectionInputFlags(
      config_.enable_sensor_processing, config_.enable_bsm_processing,
      config_.enable_psm_processing, config_.enable_mobility_path_processing);
    motion_worker_.setDetectionMotionModelFlags(
      config_.enable_ctrv_for_unknown_obj, config_.enable_ctrv_for_motorcycle_obj,
      config_.enable_ctrv_for_small_vehicle_obj, config_.enable_ctrv_for_large_vehicle_obj,
      config_.enable_ctrv_for_pedestrian_obj);
  }
 
  return result;
}

References config_, motion_computation::Config::cv_x_accel_noise, motion_computation::Config::cv_y_accel_noise, motion_computation::Config::enable_bsm_processing, motion_computation::Config::enable_ctrv_for_large_vehicle_obj, motion_computation::Config::enable_ctrv_for_motorcycle_obj, motion_computation::Config::enable_ctrv_for_pedestrian_obj, motion_computation::Config::enable_ctrv_for_small_vehicle_obj, motion_computation::Config::enable_ctrv_for_unknown_obj, motion_computation::Config::enable_mobility_path_processing, motion_computation::Config::enable_psm_processing, motion_computation::Config::enable_sensor_processing, motion_worker_, motion_computation::Config::prediction_confidence_drop_rate, motion_computation::Config::prediction_period, motion_computation::Config::prediction_process_noise_max, motion_computation::Config::prediction_time_step, motion_computation::MotionComputationWorker::setConfidenceDropRate(), motion_computation::MotionComputationWorker::setDetectionInputFlags(), motion_computation::MotionComputationWorker::setDetectionMotionModelFlags(), motion_computation::MotionComputationWorker::setPredictionPeriod(), motion_computation::MotionComputationWorker::setPredictionTimeStep(), motion_computation::MotionComputationWorker::setProcessNoiseMax(), motion_computation::MotionComputationWorker::setXAccelerationNoise(), and motion_computation::MotionComputationWorker::setYAccelerationNoise().

Referenced by handle_on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

publishObject()

void motion_computation::MotionComputationNode::publishObject

(

const carma_perception_msgs::msg::ExternalObjectList &

obj_pred_msg

)

const

Function to publish ExternalObjectList.

Parameters

obj_pred_msg

ExternalObjectList message to be published

Definition at line 184 of file motion_computation_node.cpp.

{
  carma_obj_pub_->publish(obj_pred_msg);
}

References carma_obj_pub_.

Member Data Documentation

bsm_sub_

carma_ros2_utils::SubPtr<carma_v2x_msgs::msg::BSM> motion_computation::MotionComputationNode::bsm_sub_

private

Definition at line 45 of file motion_computation_node.hpp.

Referenced by handle_on_configure().

carma_obj_pub_

carma_ros2_utils::PubPtr<carma_perception_msgs::msg::ExternalObjectList> motion_computation::MotionComputationNode::carma_obj_pub_

private

Definition at line 50 of file motion_computation_node.hpp.

Referenced by handle_on_configure(), and publishObject().

config_

Config motion_computation::MotionComputationNode::config_

private

Definition at line 56 of file motion_computation_node.hpp.

Referenced by MotionComputationNode(), handle_on_configure(), and parameter_update_callback().

georeference_sub_

carma_ros2_utils::SubPtr<std_msgs::msg::String> motion_computation::MotionComputationNode::georeference_sub_

private

Definition at line 47 of file motion_computation_node.hpp.

Referenced by handle_on_configure().

mobility_path_sub_

carma_ros2_utils::SubPtr<carma_v2x_msgs::msg::MobilityPath> motion_computation::MotionComputationNode::mobility_path_sub_

private

Definition at line 44 of file motion_computation_node.hpp.

Referenced by handle_on_configure().

motion_comp_sub_

carma_ros2_utils::SubPtr<carma_perception_msgs::msg::ExternalObjectList> motion_computation::MotionComputationNode::motion_comp_sub_

private

Definition at line 43 of file motion_computation_node.hpp.

Referenced by handle_on_configure().

motion_worker_

MotionComputationWorker motion_computation::MotionComputationNode::motion_worker_

private

Definition at line 53 of file motion_computation_node.hpp.

Referenced by handle_on_configure(), and parameter_update_callback().

psm_sub_

carma_ros2_utils::SubPtr<carma_v2x_msgs::msg::PSM> motion_computation::MotionComputationNode::psm_sub_

private

Definition at line 46 of file motion_computation_node.hpp.

Referenced by handle_on_configure().

---

The documentation for this class was generated from the following files:

• motion_computation/include/motion_computation/motion_computation_node.hpp
• motion_computation/src/motion_computation_node.cpp