The class containing the primary business logic for the Motion Computation Package. More...

#include <motion_computation_worker.hpp>

Collaboration diagram for motion_computation::MotionComputationWorker:

Public Types
using	PublishObjectCallback = std::function< void(const carma_perception_msgs::msg::ExternalObjectList &)>

using	LookUpTransform = std::function< void()>

Public Member Functions
	MotionComputationWorker (const PublishObjectCallback &obj_pub, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger, rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock)
	Constructor for MotionComputationWorker. More...

void	predictionLogic (carma_perception_msgs::msg::ExternalObjectList::UniquePtr obj_list)
	Function to populate duplicated detected objects along with their velocity, yaw, yaw_rate and static/dynamic class to the provided ExternalObjectList message. More...

void	setPredictionTimeStep (double time_step)

void	setPredictionPeriod (double period)

void	setXAccelerationNoise (double noise)

void	setYAccelerationNoise (double noise)

void	setProcessNoiseMax (double noise_max)

void	setConfidenceDropRate (double drop_rate)

void	setDetectionInputFlags (bool enable_sensor_processing, bool enable_bsm_processing, bool enable_psm_processing, bool enable_mobility_path_processing)

void	setDetectionMotionModelFlags (bool enable_ctrv_for_unknown_obj, bool enable_ctrv_for_motorcycle_obj, bool enable_ctrv_for_small_vehicle_obj, bool enable_ctrv_for_large_vehicle_obj, bool enable_ctrv_for_pedestrian_obj)

void	mobilityPathCallback (const carma_v2x_msgs::msg::MobilityPath::UniquePtr msg)

void	bsmCallback (const carma_v2x_msgs::msg::BSM::UniquePtr msg)

void	psmCallback (const carma_v2x_msgs::msg::PSM::UniquePtr msg)

void	georeferenceCallback (const std_msgs::msg::String::UniquePtr msg)
	Callback for map projection string to define lat/lon -> map conversion. More...

carma_perception_msgs::msg::ExternalObject	mobilityPathToExternalObject (const carma_v2x_msgs::msg::MobilityPath::UniquePtr &msg) const
	Converts from MobilityPath's predicted points in ECEF to local map and other fields in an ExternalObject object. More...

carma_perception_msgs::msg::ExternalObjectList	synchronizeAndAppend (const carma_perception_msgs::msg::ExternalObjectList &base_objects, carma_perception_msgs::msg::ExternalObjectList new_objects) const
	Appends external objects list behind base_objects. This does not do sensor fusion. When doing so, it drops the predictions points that start before the first prediction is sensor list. And interpolates the remaining predictions points to match the timestep using its average speed between points. More...

carma_perception_msgs::msg::ExternalObject	matchAndInterpolateTimeStamp (carma_perception_msgs::msg::ExternalObject path, const rclcpp::Time &time_to_match) const
	It cuts ExternalObject's prediction points before the time_to_match. And uses the average velocity in its predictions to match the starting point to the point it would have crossed at time_to_match It uses mobility_path_time_step between prediction points to interpolate. More...

Private Member Functions
	FRIEND_TEST (MotionComputationWorker, MobilityPathToExternalObject)

	FRIEND_TEST (MotionComputationWorker, PsmToExternalObject)

	FRIEND_TEST (MotionComputationWorker, BSMtoExternalObject)

Private Attributes
PublishObjectCallback	obj_pub_

double	prediction_time_step_ = 0.1

double	prediction_period_ = 2.0

double	cv_x_accel_noise_ = 9.0

double	cv_y_accel_noise_ = 9.0

double	prediction_process_noise_max_ = 1000.0

double	prediction_confidence_drop_rate_ = 0.9

bool	enable_sensor_processing_ = true

bool	enable_bsm_processing_ = false

bool	enable_psm_processing_ = false

bool	enable_mobility_path_processing_ = false

bool	enable_ctrv_for_unknown_obj_ = true

bool	enable_ctrv_for_motorcycle_obj_ = true

bool	enable_ctrv_for_small_vehicle_obj_ = true

bool	enable_ctrv_for_large_vehicle_obj_ = true

bool	enable_ctrv_for_pedestrian_obj_ = false

std::string	map_frame_id_ = "map"

rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr	logger_

rclcpp::node_interfaces::NodeClockInterface::SharedPtr	node_clock_

carma_perception_msgs::msg::ExternalObjectList	mobility_path_list_

carma_perception_msgs::msg::ExternalObjectList	bsm_list_

carma_perception_msgs::msg::ExternalObjectList	psm_list_

std::unordered_map< uint32_t, size_t >	mobility_path_obj_id_map_

std::unordered_map< uint32_t, size_t >	bsm_obj_id_map_

std::unordered_map< uint32_t, size_t >	psm_obj_id_map_

std::string	georeference_ {""}

std::shared_ptr< lanelet::projection::LocalFrameProjector >	map_projector_

tf2::Quaternion	ned_in_map_rotation_

Detailed Description

The class containing the primary business logic for the Motion Computation Package.

Definition at line 46 of file motion_computation_worker.hpp.

Member Typedef Documentation

◆ LookUpTransform

using motion_computation::MotionComputationWorker::LookUpTransform = std::function<void()>

Definition at line 51 of file motion_computation_worker.hpp.

◆ PublishObjectCallback

using motion_computation::MotionComputationWorker::PublishObjectCallback = std::function<void(const carma_perception_msgs::msg::ExternalObjectList &)>

Definition at line 49 of file motion_computation_worker.hpp.

Constructor & Destructor Documentation

◆ MotionComputationWorker()

motion_computation::MotionComputationWorker::MotionComputationWorker	(	const PublishObjectCallback &	obj_pub,
		rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr	logger,
		rclcpp::node_interfaces::NodeClockInterface::SharedPtr	node_clock
	)

Constructor for MotionComputationWorker.

Definition at line 25 of file motion_computation_worker.cpp.

: obj_pub_(obj_pub), logger_(logger), node_clock_(node_clock)
{
}

Member Function Documentation

◆ bsmCallback()

void motion_computation::MotionComputationWorker::bsmCallback ( const carma_v2x_msgs::msg::BSM::UniquePtr msg )

Definition at line 272 of file motion_computation_worker.cpp.

{
  if (!map_projector_) {
    RCLCPP_DEBUG_STREAM(
      logger_->get_logger(), "Map projection not available yet so ignoring PSM messages");
    return;
  }
 
  if (!enable_bsm_processing_) {
    RCLCPP_DEBUG_STREAM(
      logger_->get_logger(), "enable_bsm_processing is false so ignoring BSM messages");
    return;
  }
 
  carma_perception_msgs::msg::ExternalObject obj_msg;
  conversion::convert(
    *msg, obj_msg, map_frame_id_, prediction_period_, prediction_time_step_, *map_projector_,
    ned_in_map_rotation_);
 
  // Check if this bsm is from an object already being queded.
  // If so then update the existing object, if not add it to the queue
  if (bsm_obj_id_map_.find(obj_msg.id) != bsm_obj_id_map_.end()) {
    bsm_list_.objects[bsm_obj_id_map_[obj_msg.id]] = obj_msg;
 
  } else {
    // Add the new object to the queue and save its index
    bsm_obj_id_map_[obj_msg.id] = bsm_list_.objects.size();
    bsm_list_.objects.push_back(obj_msg);
  }
}

References bsm_list_, bsm_obj_id_map_, motion_computation::conversion::convert(), enable_bsm_processing_, logger_, map_frame_id_, map_projector_, ned_in_map_rotation_, prediction_period_, and prediction_time_step_.

Referenced by motion_computation::MotionComputationNode::handle_on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ FRIEND_TEST() [1/3]

motion_computation::MotionComputationWorker::FRIEND_TEST	(	MotionComputationWorker	,
		BSMtoExternalObject
	)

private

◆ FRIEND_TEST() [2/3]

motion_computation::MotionComputationWorker::FRIEND_TEST	(	MotionComputationWorker	,
		MobilityPathToExternalObject
	)

private

◆ FRIEND_TEST() [3/3]

motion_computation::MotionComputationWorker::FRIEND_TEST	(	MotionComputationWorker	,
		PsmToExternalObject
	)

private

◆ georeferenceCallback()

void motion_computation::MotionComputationWorker::georeferenceCallback ( const std_msgs::msg::String::UniquePtr msg )

Callback for map projection string to define lat/lon -> map conversion.

msg The proj string defining the projection.

Definition at line 143 of file motion_computation_worker.cpp.

{
  // Build projector from proj string
  if (georeference_ != msg->data)
  {
    georeference_ = msg->data;
    map_projector_ = std::make_shared<lanelet::projection::LocalFrameProjector>(msg->data.c_str());
 
    std::string axis =
      wgs84_utils::proj_tools::getAxisFromProjString(msg->data);  // Extract axis for orientation
 
    RCLCPP_INFO_STREAM(logger_->get_logger(), "Extracted Axis: " << axis);
 
    ned_in_map_rotation_ =
      wgs84_utils::proj_tools::getRotationOfNEDFromProjAxis(axis);  // Extract map rotation
 
    RCLCPP_DEBUG_STREAM(
      logger_->get_logger(), "Extracted NED in Map Rotation (x,y,z,w) : ( "
                              << ned_in_map_rotation_.getX() << ", " << ned_in_map_rotation_.getY()
                              << ", " << ned_in_map_rotation_.getZ() << ", "
                              << ned_in_map_rotation_.getW());
  }
}

References georeference_, logger_, map_projector_, and ned_in_map_rotation_.

Referenced by motion_computation::MotionComputationNode::handle_on_configure().

Here is the caller graph for this function:

◆ matchAndInterpolateTimeStamp()

carma_perception_msgs::msg::ExternalObject motion_computation::MotionComputationWorker::matchAndInterpolateTimeStamp	(	carma_perception_msgs::msg::ExternalObject	path,
		const rclcpp::Time &	time_to_match
	)		const

It cuts ExternalObject's prediction points before the time_to_match. And uses the average velocity in its predictions to match the starting point to the point it would have crossed at time_to_match It uses mobility_path_time_step between prediction points to interpolate.

Parameters

path	External object with predictions to modify
time_to_match	time stamp to have the object start at

Returns: carma_perception_msgs::msg::ExternalObject

Note: It assumes time_to_match falls in prediction time's whole interval.

Definition at line 326 of file motion_computation_worker.cpp.

{
  carma_perception_msgs::msg::ExternalObject output = path;
  // empty predictions
  output.predictions = {};
 
  // add the first point to start of the predictions to easily loop over
  carma_perception_msgs::msg::PredictedState prev_state;
  prev_state.header.stamp = output.header.stamp;
  prev_state.predicted_position.orientation = output.pose.pose.orientation;
  prev_state.predicted_velocity = output.velocity.twist;
  prev_state.predicted_position.position.x = output.pose.pose.position.x;
  prev_state.predicted_position.position.y = output.pose.pose.position.y;
  prev_state.predicted_position.position.z = output.pose.pose.position.z;
  path.predictions.insert(path.predictions.begin(), prev_state);
 
  rclcpp::Time curr_time_to_match = time_to_match;
 
  // because of this logic, we would not encounter mobility path
  // that starts later than the time we are trying to match (which is starting
  // time of sensed objects)
  bool is_first_point = true;
  carma_perception_msgs::msg::PredictedState new_state;
  for (auto const & curr_state : path.predictions) {
    if (curr_time_to_match > curr_state.header.stamp) {
      prev_state = curr_state;
      continue;
    }
 
    if (is_first_point)  // we store in the body if it is the first point, not predictions
    {
      // reaching here means curr_state starts later than the time we are trying to match
      // copy old unchanged parts
      new_state.header.stamp = curr_time_to_match;
      new_state.predicted_velocity = prev_state.predicted_velocity;
      new_state.predicted_position.orientation = prev_state.predicted_position.orientation;
 
      // interpolate position
      rclcpp::Duration delta_t = rclcpp::Time(curr_state.header.stamp) - curr_time_to_match;
      rclcpp::Duration pred_delta_t =
        rclcpp::Time(curr_state.header.stamp) - rclcpp::Time(prev_state.header.stamp);
      double ratio;
      if (pred_delta_t.seconds() < 0.00000001) {  // Divide by zero check
        // This can only happen if effectively all 3 points are on top of each other
        // which is extremely unlikely
        ratio = 0.0;
      } else {
        ratio = delta_t.seconds() / pred_delta_t.seconds();
      }
 
      double delta_x =
        curr_state.predicted_position.position.x - prev_state.predicted_position.position.x;
      double delta_y =
        curr_state.predicted_position.position.y - prev_state.predicted_position.position.y;
      double delta_z =
        curr_state.predicted_position.position.z - prev_state.predicted_position.position.z;
      // we are "stepping back in time" to match the position
      new_state.predicted_position.position.x =
        curr_state.predicted_position.position.x - delta_x * ratio;
      new_state.predicted_position.position.y =
        curr_state.predicted_position.position.y - delta_y * ratio;
      new_state.predicted_position.position.z =
        curr_state.predicted_position.position.z - delta_z * ratio;
 
      output.header.stamp = curr_time_to_match;
      output.pose.pose.orientation = new_state.predicted_position.orientation;
      output.velocity.twist = new_state.predicted_velocity;
      output.pose.pose.position.x = new_state.predicted_position.position.x;
      output.pose.pose.position.y = new_state.predicted_position.position.y;
      output.pose.pose.position.z = new_state.predicted_position.position.z;
      is_first_point = false;
 
    } else {
      output.predictions.push_back(curr_state);
    }
  }
 
  return output;
}

Referenced by synchronizeAndAppend().

Here is the caller graph for this function:

◆ mobilityPathCallback()

void motion_computation::MotionComputationWorker::mobilityPathCallback ( const carma_v2x_msgs::msg::MobilityPath::UniquePtr msg )

Definition at line 210 of file motion_computation_worker.cpp.

{
  if (!map_projector_) {
    RCLCPP_ERROR(
      logger_->get_logger(), "Map projection not available yet so ignoring MobilityPath messages");
    return;
  }
 
  if (!enable_mobility_path_processing_) {
    RCLCPP_WARN(
      logger_->get_logger(),
      "enable_mobility_path_processing is false so ignoring MobilityPath messages");
    return;
  }
 
  carma_perception_msgs::msg::ExternalObject obj_msg;
  conversion::convert(*msg, obj_msg, *map_projector_);
 
  // Check if this mobility path is from an object already being queded.
  // If so then update the existing object, if not add it to the queue
  if (mobility_path_obj_id_map_.find(obj_msg.id) != mobility_path_obj_id_map_.end()) {
    mobility_path_list_.objects[mobility_path_obj_id_map_[obj_msg.id]] = obj_msg;
 
  } else {
    // Add the new object to the queue and save its index
    mobility_path_obj_id_map_[obj_msg.id] = mobility_path_list_.objects.size();
    mobility_path_list_.objects.push_back(obj_msg);
  }
}

References motion_computation::conversion::convert(), enable_mobility_path_processing_, logger_, map_projector_, mobility_path_list_, and mobility_path_obj_id_map_.

Referenced by motion_computation::MotionComputationNode::handle_on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ mobilityPathToExternalObject()

carma_perception_msgs::msg::ExternalObject motion_computation::MotionComputationWorker::mobilityPathToExternalObject ( const carma_v2x_msgs::msg::MobilityPath::UniquePtr & msg ) const

Converts from MobilityPath's predicted points in ECEF to local map and other fields in an ExternalObject object.

Parameters

msg	MobilityPath message to convert

Returns: ExternalObject object

◆ predictionLogic()

void motion_computation::MotionComputationWorker::predictionLogic ( carma_perception_msgs::msg::ExternalObjectList::UniquePtr obj_list )

Function to populate duplicated detected objects along with their velocity, yaw, yaw_rate and static/dynamic class to the provided ExternalObjectList message.

Parameters

obj_list ExternalObjectList message

Definition at line 33 of file motion_computation_worker.cpp.

{
  carma_perception_msgs::msg::ExternalObjectList sensor_list;
  sensor_list.header = obj_list->header;
 
  if (!obj_list || obj_list->objects.empty()) {
    return;
  }
 
  for (auto obj : obj_list->objects) {
    // Header contains the frame rest of the fields will use
    // obj.header = obj_list.objects[i].header;
 
    // Object id. Matching ids on a topic should refer to the same object within
    // some time period, expanded
    // obj.id = obj_list.objects[i].id;
 
    // Update the object type and generate predictions using CV or CTRV vehicle models.
    // If the object is a bicycle or motor vehicle use CTRV otherwise use CV.
 
    bool use_ctrv_model;
 
    if (obj.object_type == obj.UNKNOWN) {
      use_ctrv_model = enable_ctrv_for_unknown_obj_;
    } else if (obj.object_type == obj.MOTORCYCLE) {
      use_ctrv_model = enable_ctrv_for_motorcycle_obj_;
    } else if (obj.object_type == obj.SMALL_VEHICLE) {
      use_ctrv_model = enable_ctrv_for_small_vehicle_obj_;
    } else if (obj.object_type == obj.LARGE_VEHICLE) {
      use_ctrv_model = enable_ctrv_for_large_vehicle_obj_;
    } else if (obj.object_type == obj.PEDESTRIAN) {
      use_ctrv_model = enable_ctrv_for_pedestrian_obj_;
    } else {
      obj.object_type = obj.UNKNOWN;
      use_ctrv_model = enable_ctrv_for_unknown_obj_;
    }  // end if-else
 
    if (use_ctrv_model == true) {
      obj.predictions = motion_predict::ctrv::predictPeriod(
        obj, prediction_time_step_, prediction_period_, prediction_process_noise_max_,
        prediction_confidence_drop_rate_);
    } else {
      obj.predictions = motion_predict::cv::predictPeriod(
        obj, prediction_time_step_, prediction_period_, cv_x_accel_noise_, cv_y_accel_noise_,
        prediction_process_noise_max_, prediction_confidence_drop_rate_);
    }
 
    sensor_list.objects.emplace_back(obj);
  }  // end for-loop
 
  // Synchronize all data to the current sensor data timestamp
  carma_perception_msgs::msg::ExternalObjectList synchronization_base_objects;
  synchronization_base_objects.header =
    sensor_list.header;  // Use the current sensing stamp as the sync point even if sensor data is
                         // not used
 
  if (enable_sensor_processing_) {
    // If using sensor data add it to the base synchronization list since it
    // already is at the desired time
 
    synchronization_base_objects.objects = sensor_list.objects;
 
  } else if (enable_bsm_processing_ || enable_psm_processing_ || enable_mobility_path_processing_) {
    // Since we use the new sensor data as the sync point we will still be
    // synchronizing but to an empty list
    synchronization_base_objects.objects.clear();
 
  } else {
    RCLCPP_WARN_STREAM(
      logger_->get_logger(),
      "Not configured to publish any data publishing empty "
      "object list. Operating like this is NOT "
      "advised.");
 
    obj_pub_(synchronization_base_objects);
    bsm_list_.objects.clear();
    bsm_obj_id_map_.clear();
    psm_list_.objects.clear();
    psm_obj_id_map_.clear();
    mobility_path_list_.objects.clear();
    mobility_path_obj_id_map_.clear();
 
    return;
  }
 
  // Start synchronizing all the enabled data streams
  if (enable_bsm_processing_) {
    synchronization_base_objects = synchronizeAndAppend(synchronization_base_objects, bsm_list_);
  }
 
  if (enable_psm_processing_) {
    synchronization_base_objects = synchronizeAndAppend(synchronization_base_objects, psm_list_);
  }
 
  if (enable_mobility_path_processing_) {
    synchronization_base_objects =
      synchronizeAndAppend(synchronization_base_objects, mobility_path_list_);
  }
 
  obj_pub_(synchronization_base_objects);
  // Clear msg queue since it is published
  mobility_path_list_.objects.clear();
  mobility_path_obj_id_map_.clear();
  bsm_list_.objects.clear();
  bsm_obj_id_map_.clear();
  psm_list_.objects.clear();
  psm_obj_id_map_.clear();
}

References bsm_list_, bsm_obj_id_map_, cv_x_accel_noise_, cv_y_accel_noise_, enable_bsm_processing_, enable_ctrv_for_large_vehicle_obj_, enable_ctrv_for_motorcycle_obj_, enable_ctrv_for_pedestrian_obj_, enable_ctrv_for_small_vehicle_obj_, enable_ctrv_for_unknown_obj_, enable_mobility_path_processing_, enable_psm_processing_, enable_sensor_processing_, logger_, mobility_path_list_, mobility_path_obj_id_map_, obj_pub_, prediction_confidence_drop_rate_, prediction_period_, prediction_process_noise_max_, prediction_time_step_, psm_list_, psm_obj_id_map_, and synchronizeAndAppend().

Referenced by motion_computation::MotionComputationNode::handle_on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ psmCallback()

void motion_computation::MotionComputationWorker::psmCallback ( const carma_v2x_msgs::msg::PSM::UniquePtr msg )

Definition at line 241 of file motion_computation_worker.cpp.

{
  if (!map_projector_) {
    RCLCPP_DEBUG_STREAM(
      logger_->get_logger(), "Map projection not available yet so ignoring PSM messages");
    return;
  }
 
  if (!enable_psm_processing_) {
    RCLCPP_DEBUG_STREAM(
      logger_->get_logger(), "enable_psm_processing is false so ignoring PSM messages");
    return;
  }
 
  carma_perception_msgs::msg::ExternalObject obj_msg;
  conversion::convert(
    *msg, obj_msg, map_frame_id_, prediction_period_, prediction_time_step_, *map_projector_,
    ned_in_map_rotation_, node_clock_);
 
  // Check if this psm is from an object already being queded.
  // If so then update the existing object, if not add it to the queue
  if (psm_obj_id_map_.find(obj_msg.id) != psm_obj_id_map_.end()) {
    psm_list_.objects[psm_obj_id_map_[obj_msg.id]] = obj_msg;
 
  } else {
    // Add the new object to the queue and save its index
    psm_obj_id_map_[obj_msg.id] = psm_list_.objects.size();
    psm_list_.objects.push_back(obj_msg);
  }
}

References motion_computation::conversion::convert(), enable_psm_processing_, logger_, map_frame_id_, map_projector_, ned_in_map_rotation_, node_clock_, prediction_period_, prediction_time_step_, psm_list_, and psm_obj_id_map_.

Referenced by motion_computation::MotionComputationNode::handle_on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ setConfidenceDropRate()

void motion_computation::MotionComputationWorker::setConfidenceDropRate ( double drop_rate )

Definition at line 183 of file motion_computation_worker.cpp.

{
  prediction_confidence_drop_rate_ = drop_rate;
}

References prediction_confidence_drop_rate_.

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

Here is the caller graph for this function:

◆ setDetectionInputFlags()

void motion_computation::MotionComputationWorker::setDetectionInputFlags	(	bool	enable_sensor_processing,
		bool	enable_bsm_processing,
		bool	enable_psm_processing,
		bool	enable_mobility_path_processing
	)

Definition at line 188 of file motion_computation_worker.cpp.

{
  enable_sensor_processing_ = enable_sensor_processing;
  enable_bsm_processing_ = enable_bsm_processing;
  enable_psm_processing_ = enable_psm_processing;
  enable_mobility_path_processing_ = enable_mobility_path_processing;
}

References enable_bsm_processing_, enable_mobility_path_processing_, enable_psm_processing_, and enable_sensor_processing_.

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

Here is the caller graph for this function:

◆ setDetectionMotionModelFlags()

void motion_computation::MotionComputationWorker::setDetectionMotionModelFlags	(	bool	enable_ctrv_for_unknown_obj,
		bool	enable_ctrv_for_motorcycle_obj,
		bool	enable_ctrv_for_small_vehicle_obj,
		bool	enable_ctrv_for_large_vehicle_obj,
		bool	enable_ctrv_for_pedestrian_obj
	)

Definition at line 198 of file motion_computation_worker.cpp.

{
  enable_ctrv_for_unknown_obj_ = enable_ctrv_for_unknown_obj;
  enable_ctrv_for_motorcycle_obj_ = enable_ctrv_for_motorcycle_obj;
  enable_ctrv_for_small_vehicle_obj_ = enable_ctrv_for_small_vehicle_obj;
  enable_ctrv_for_large_vehicle_obj_ = enable_ctrv_for_large_vehicle_obj;
  enable_ctrv_for_pedestrian_obj_ = enable_ctrv_for_pedestrian_obj;
}

References enable_ctrv_for_large_vehicle_obj_, enable_ctrv_for_motorcycle_obj_, enable_ctrv_for_pedestrian_obj_, enable_ctrv_for_small_vehicle_obj_, and enable_ctrv_for_unknown_obj_.

Referenced by motion_computation::MotionComputationNode::parameter_update_callback().

Here is the caller graph for this function:

◆ setPredictionPeriod()

void motion_computation::MotionComputationWorker::setPredictionPeriod ( double period )

Definition at line 172 of file motion_computation_worker.cpp.

172{ prediction_period_ = period; }

References prediction_period_.

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

Here is the caller graph for this function:

◆ setPredictionTimeStep()

void motion_computation::MotionComputationWorker::setPredictionTimeStep ( double time_step )

Definition at line 167 of file motion_computation_worker.cpp.

{
  prediction_time_step_ = time_step;
}

References prediction_time_step_.

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

Here is the caller graph for this function:

◆ setProcessNoiseMax()

void motion_computation::MotionComputationWorker::setProcessNoiseMax ( double noise_max )

Definition at line 178 of file motion_computation_worker.cpp.

{
  prediction_process_noise_max_ = noise_max;
}

References prediction_process_noise_max_.

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

Here is the caller graph for this function:

◆ setXAccelerationNoise()

void motion_computation::MotionComputationWorker::setXAccelerationNoise ( double noise )

Definition at line 174 of file motion_computation_worker.cpp.

174{ cv_x_accel_noise_ = noise; }

References cv_x_accel_noise_.

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

Here is the caller graph for this function:

◆ setYAccelerationNoise()

void motion_computation::MotionComputationWorker::setYAccelerationNoise ( double noise )

Definition at line 176 of file motion_computation_worker.cpp.

176{ cv_y_accel_noise_ = noise; }

References cv_y_accel_noise_.

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

Here is the caller graph for this function:

◆ synchronizeAndAppend()

carma_perception_msgs::msg::ExternalObjectList motion_computation::MotionComputationWorker::synchronizeAndAppend	(	const carma_perception_msgs::msg::ExternalObjectList &	base_objects,
		carma_perception_msgs::msg::ExternalObjectList	new_objects
	)		const

Appends external objects list behind base_objects. This does not do sensor fusion. When doing so, it drops the predictions points that start before the first prediction is sensor list. And interpolates the remaining predictions points to match the timestep using its average speed between points.

Parameters

base_objects	object detections to append to and synchronize with
new_objects	new objects to add and be synchronized

Returns: append and synchronized list of external objects

Definition at line 303 of file motion_computation_worker.cpp.

{
  carma_perception_msgs::msg::ExternalObjectList output_list;
  output_list.header = base_objects.header;
  output_list.objects.reserve(base_objects.objects.size() + new_objects.objects.size());
 
  // Compare time_stamps of first elements of each list as they are guaranteed to be the
  // earliest of the respective lists
 
  for (auto & obj : new_objects.objects) {
    // interpolate and match timesteps
    obj = matchAndInterpolateTimeStamp(obj, rclcpp::Time(base_objects.header.stamp));
  }
 
  output_list.objects.insert(
    output_list.objects.begin(), base_objects.objects.begin(), base_objects.objects.end());
  output_list.objects.insert(
    output_list.objects.end(), new_objects.objects.begin(), new_objects.objects.end());
  return output_list;
}

References matchAndInterpolateTimeStamp().

Referenced by predictionLogic().

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

◆ bsm_list_

carma_perception_msgs::msg::ExternalObjectList motion_computation::MotionComputationWorker::bsm_list_

private

Definition at line 166 of file motion_computation_worker.hpp.

Referenced by bsmCallback(), and predictionLogic().

◆ bsm_obj_id_map_

std::unordered_map<uint32_t, size_t> motion_computation::MotionComputationWorker::bsm_obj_id_map_

private

Definition at line 171 of file motion_computation_worker.hpp.

Referenced by bsmCallback(), and predictionLogic().

◆ cv_x_accel_noise_

double motion_computation::MotionComputationWorker::cv_x_accel_noise_ = 9.0

private

Definition at line 138 of file motion_computation_worker.hpp.

Referenced by predictionLogic(), and setXAccelerationNoise().

◆ cv_y_accel_noise_

double motion_computation::MotionComputationWorker::cv_y_accel_noise_ = 9.0

private

Definition at line 139 of file motion_computation_worker.hpp.

Referenced by predictionLogic(), and setYAccelerationNoise().

◆ enable_bsm_processing_

bool motion_computation::MotionComputationWorker::enable_bsm_processing_ = false

private

Definition at line 145 of file motion_computation_worker.hpp.

Referenced by bsmCallback(), predictionLogic(), and setDetectionInputFlags().

◆ enable_ctrv_for_large_vehicle_obj_

bool motion_computation::MotionComputationWorker::enable_ctrv_for_large_vehicle_obj_ = true

private

Definition at line 153 of file motion_computation_worker.hpp.

Referenced by predictionLogic(), and setDetectionMotionModelFlags().

◆ enable_ctrv_for_motorcycle_obj_

bool motion_computation::MotionComputationWorker::enable_ctrv_for_motorcycle_obj_ = true

private

Definition at line 151 of file motion_computation_worker.hpp.

Referenced by predictionLogic(), and setDetectionMotionModelFlags().

◆ enable_ctrv_for_pedestrian_obj_

bool motion_computation::MotionComputationWorker::enable_ctrv_for_pedestrian_obj_ = false

private

Definition at line 154 of file motion_computation_worker.hpp.

Referenced by predictionLogic(), and setDetectionMotionModelFlags().

◆ enable_ctrv_for_small_vehicle_obj_

bool motion_computation::MotionComputationWorker::enable_ctrv_for_small_vehicle_obj_ = true

private

Definition at line 152 of file motion_computation_worker.hpp.

Referenced by predictionLogic(), and setDetectionMotionModelFlags().

◆ enable_ctrv_for_unknown_obj_

bool motion_computation::MotionComputationWorker::enable_ctrv_for_unknown_obj_ = true

private

Definition at line 150 of file motion_computation_worker.hpp.

Referenced by predictionLogic(), and setDetectionMotionModelFlags().

◆ enable_mobility_path_processing_

bool motion_computation::MotionComputationWorker::enable_mobility_path_processing_ = false

private

Definition at line 147 of file motion_computation_worker.hpp.

Referenced by mobilityPathCallback(), predictionLogic(), and setDetectionInputFlags().

◆ enable_psm_processing_

bool motion_computation::MotionComputationWorker::enable_psm_processing_ = false

private

Definition at line 146 of file motion_computation_worker.hpp.

Referenced by predictionLogic(), psmCallback(), and setDetectionInputFlags().

◆ enable_sensor_processing_

bool motion_computation::MotionComputationWorker::enable_sensor_processing_ = true

private

Definition at line 144 of file motion_computation_worker.hpp.

Referenced by predictionLogic(), and setDetectionInputFlags().

◆ georeference_

std::string motion_computation::MotionComputationWorker::georeference_ {""}

private

Definition at line 174 of file motion_computation_worker.hpp.

Referenced by georeferenceCallback().

◆ logger_

rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr motion_computation::MotionComputationWorker::logger_

private

Definition at line 160 of file motion_computation_worker.hpp.

Referenced by bsmCallback(), georeferenceCallback(), mobilityPathCallback(), predictionLogic(), and psmCallback().

◆ map_frame_id_

std::string motion_computation::MotionComputationWorker::map_frame_id_ = "map"

private

Definition at line 157 of file motion_computation_worker.hpp.

Referenced by bsmCallback(), and psmCallback().

◆ map_projector_

std::shared_ptr<lanelet::projection::LocalFrameProjector> motion_computation::MotionComputationWorker::map_projector_

private

Definition at line 175 of file motion_computation_worker.hpp.

Referenced by bsmCallback(), georeferenceCallback(), mobilityPathCallback(), and psmCallback().

◆ mobility_path_list_

carma_perception_msgs::msg::ExternalObjectList motion_computation::MotionComputationWorker::mobility_path_list_

private

Definition at line 165 of file motion_computation_worker.hpp.

Referenced by mobilityPathCallback(), and predictionLogic().

◆ mobility_path_obj_id_map_

std::unordered_map<uint32_t, size_t> motion_computation::MotionComputationWorker::mobility_path_obj_id_map_

private

Definition at line 170 of file motion_computation_worker.hpp.

Referenced by mobilityPathCallback(), and predictionLogic().

◆ ned_in_map_rotation_

tf2::Quaternion motion_computation::MotionComputationWorker::ned_in_map_rotation_

private

Definition at line 178 of file motion_computation_worker.hpp.

Referenced by bsmCallback(), georeferenceCallback(), and psmCallback().

◆ node_clock_

rclcpp::node_interfaces::NodeClockInterface::SharedPtr motion_computation::MotionComputationWorker::node_clock_

private

Definition at line 162 of file motion_computation_worker.hpp.

Referenced by psmCallback().

◆ obj_pub_

PublishObjectCallback motion_computation::MotionComputationWorker::obj_pub_

private

Definition at line 132 of file motion_computation_worker.hpp.

Referenced by predictionLogic().

◆ prediction_confidence_drop_rate_

double motion_computation::MotionComputationWorker::prediction_confidence_drop_rate_ = 0.9

private

Definition at line 141 of file motion_computation_worker.hpp.

Referenced by predictionLogic(), and setConfidenceDropRate().

◆ prediction_period_

double motion_computation::MotionComputationWorker::prediction_period_ = 2.0

private

Definition at line 137 of file motion_computation_worker.hpp.

Referenced by bsmCallback(), predictionLogic(), psmCallback(), and setPredictionPeriod().

◆ prediction_process_noise_max_

double motion_computation::MotionComputationWorker::prediction_process_noise_max_ = 1000.0

private

Definition at line 140 of file motion_computation_worker.hpp.

Referenced by predictionLogic(), and setProcessNoiseMax().

◆ prediction_time_step_

double motion_computation::MotionComputationWorker::prediction_time_step_ = 0.1

private

Definition at line 136 of file motion_computation_worker.hpp.

Referenced by bsmCallback(), predictionLogic(), psmCallback(), and setPredictionTimeStep().

◆ psm_list_

carma_perception_msgs::msg::ExternalObjectList motion_computation::MotionComputationWorker::psm_list_

private

Definition at line 167 of file motion_computation_worker.hpp.

Referenced by predictionLogic(), and psmCallback().

◆ psm_obj_id_map_

std::unordered_map<uint32_t, size_t> motion_computation::MotionComputationWorker::psm_obj_id_map_

private

Definition at line 172 of file motion_computation_worker.hpp.

Referenced by predictionLogic(), and psmCallback().

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

motion_computation/include/motion_computation/motion_computation_worker.hpp
motion_computation/src/motion_computation_worker.cpp

Public Types

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Member Typedef Documentation

◆ LookUpTransform

◆ PublishObjectCallback

Constructor & Destructor Documentation

◆ MotionComputationWorker()

Member Function Documentation

◆ bsmCallback()

◆ FRIEND_TEST() [1/3]

◆ FRIEND_TEST() [2/3]

◆ FRIEND_TEST() [3/3]

◆ georeferenceCallback()

◆ matchAndInterpolateTimeStamp()

◆ mobilityPathCallback()

◆ mobilityPathToExternalObject()

◆ predictionLogic()

◆ psmCallback()

◆ setConfidenceDropRate()

◆ setDetectionInputFlags()

◆ setDetectionMotionModelFlags()

◆ setPredictionPeriod()

◆ setPredictionTimeStep()

◆ setProcessNoiseMax()

◆ setXAccelerationNoise()

◆ setYAccelerationNoise()

◆ synchronizeAndAppend()

Member Data Documentation

◆ bsm_list_

◆ bsm_obj_id_map_

◆ cv_x_accel_noise_

◆ cv_y_accel_noise_

◆ enable_bsm_processing_

◆ enable_ctrv_for_large_vehicle_obj_

◆ enable_ctrv_for_motorcycle_obj_

◆ enable_ctrv_for_pedestrian_obj_

◆ enable_ctrv_for_small_vehicle_obj_

◆ enable_ctrv_for_unknown_obj_

◆ enable_mobility_path_processing_

◆ enable_psm_processing_

◆ enable_sensor_processing_

◆ georeference_

◆ logger_

◆ map_frame_id_

◆ map_projector_

◆ mobility_path_list_

◆ mobility_path_obj_id_map_

◆ ned_in_map_rotation_

◆ node_clock_

◆ obj_pub_

◆ prediction_confidence_drop_rate_

◆ prediction_period_

◆ prediction_process_noise_max_

◆ prediction_time_step_

◆ psm_list_

◆ psm_obj_id_map_