motion_computation::conversion Namespace Reference

Namespaces
namespace	impl

Functions
void	convert (const carma_v2x_msgs::msg::PSM &in_msg, carma_perception_msgs::msg::ExternalObject &out_msg, const std::string &map_frame_id, double pred_period, double pred_step_size, const lanelet::projection::LocalFrameProjector &map_projector, const tf2::Quaternion &ned_in_map_rotation, rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock)
void	convert (const carma_v2x_msgs::msg::BSM &in_msg, carma_perception_msgs::msg::ExternalObject &out_msg, const std::string &map_frame_id, double pred_period, double pred_step_size, const lanelet::projection::LocalFrameProjector &map_projector, tf2::Quaternion ned_in_map_rotation)
void	convert (const carma_v2x_msgs::msg::MobilityPath &in_msg, carma_perception_msgs::msg::ExternalObject &out_msg, const lanelet::projection::LocalFrameProjector &map_projector)

Function Documentation

convert() [1/3]

void motion_computation::conversion::convert	(	const carma_v2x_msgs::msg::BSM &	in_msg,
		carma_perception_msgs::msg::ExternalObject &	out_msg,
		const std::string &	map_frame_id,
		double	pred_period,
		double	pred_step_size,
		const lanelet::projection::LocalFrameProjector &	map_projector,
		tf2::Quaternion	ned_in_map_rotation
	)

Definition at line 28 of file bsm_to_external_object_convertor.cpp.

{
  out_msg.presence_vector |= carma_perception_msgs::msg::ExternalObject::BSM_ID_PRESENCE_VECTOR;
 
  // assume any BSM is a dynamic object since its sent be a vehicle
  out_msg.dynamic_obj = true;
  out_msg.presence_vector |= carma_perception_msgs::msg::ExternalObject::DYNAMIC_OBJ_PRESENCE;
 
  // Generate a unique object id from the bsm id
  out_msg.id = 0;
  for (int i = in_msg.core_data.id.size() - 1; i >= 0;
       i--) {  // using signed iterator to handle empty case
    // each byte of the bsm id gets placed in one byte of the object id.
    // This should result in very large numbers which will be unlikely to
    // conflict with standard detections
    out_msg.id |= in_msg.core_data.id[i] << (8 * i);
  }
  out_msg.presence_vector |= carma_perception_msgs::msg::ExternalObject::ID_PRESENCE_VECTOR;
 
  out_msg.bsm_id = in_msg.core_data.id;
  out_msg.presence_vector |= carma_perception_msgs::msg::ExternalObject::BSM_ID_PRESENCE_VECTOR;
 
  // Compute the timestamp
  out_msg.header.stamp = in_msg.header.stamp;
  out_msg.header.frame_id = map_frame_id;
 
 
  if (
    in_msg.core_data.size.presence_vector &
    carma_v2x_msgs::msg::VehicleSize::VEHICLE_LENGTH_AVAILABLE) {
    // ExternalObject size is half of each dimension
    out_msg.size.x = in_msg.core_data.size.vehicle_length / 2;
  } else {
    out_msg.size.x = 1.0;  // value from mob path to external obj conversion
  }
 
  if (
    in_msg.core_data.size.presence_vector &
    carma_v2x_msgs::msg::VehicleSize::VEHICLE_WIDTH_AVAILABLE) {
    // ExternalObject size is half of each dimension
    out_msg.size.y = in_msg.core_data.size.vehicle_width / 2;
  } else {
    out_msg.size.y = 1.0;  // value from mob path to external obj conversion
  }
 
  out_msg.size.z = 2.0;  // value from mob path to external obj conversion
 
  out_msg.object_type = carma_perception_msgs::msg::ExternalObject::SMALL_VEHICLE;
 
  // Set the velocity
  if (in_msg.core_data.presence_vector & carma_v2x_msgs::msg::BSMCoreData::SPEED_AVAILABLE) {
    out_msg.presence_vector |= carma_perception_msgs::msg::ExternalObject::VELOCITY_PRESENCE_VECTOR;
    float_t speed_mps = in_msg.core_data.speed;
 
    out_msg.velocity.twist.linear.x = std::min(speed_mps, in_msg.core_data.SPEED_MAX);
  }
 
  // NOTE: The velocity covariance is not provided in the BSM. In order to
  // compute it you need at least two BSM messages
  //     Tracking and associating BSM messages would be an increase in
  //     complexity for this conversion which is not warranted without an
  //     existing use case for the velocity covariance. If a use case is
  //     presented for it, such an addition can be made at that time.
 
  // Compute the position covariance
  // For computing confidence we will use the largest provided standard
  // deviation of position
  double largest_position_std =
    std::max(in_msg.core_data.accuracy.semi_major, in_msg.core_data.accuracy.semi_minor);
 
  double lat_variance = in_msg.core_data.accuracy.semi_minor * in_msg.core_data.accuracy.semi_minor;
 
  double lon_variance = in_msg.core_data.accuracy.semi_major * in_msg.core_data.accuracy.semi_major;
 
  double heading_variance =
    in_msg.core_data.accuracy.orientation * in_msg.core_data.accuracy.orientation;
 
  double position_confidence = 0.1;  // Default will be 10% confidence. If the position accuracy is
                                     // available then this value will be updated
 
  // A standard deviation which is larger than the acceptable value to give
  // 95% confidence interval on fitting the pedestrian within one 3.7m lane
  constexpr double MAX_POSITION_STD = 1.85;
 
  if (
    (in_msg.core_data.accuracy.presence_vector |
     carma_v2x_msgs::msg::PositionalAccuracy::ACCURACY_AVAILABLE) &&
    (in_msg.core_data.accuracy.presence_vector |
     carma_v2x_msgs::msg::PositionalAccuracy::ACCURACY_ORIENTATION_AVAILABLE)) {
    // Both accuracies available
 
    // NOTE: ExternalObject.msg does not clearly define what is meant by
    // position confidence
    //     Here we are providing a linear scale based on the positional accuracy
    //     where 0 confidence would denote A standard deviation which is larger
    //     than the acceptable value to give 95% confidence interval on fitting
    //     the pedestrian within one 3.7m lane
    // Set the confidence
    // Without a way of getting the velocity confidence from the BSM we will use
    // the position confidence for both
    out_msg.confidence = 1.0 - std::min(1.0, fabs(largest_position_std / MAX_POSITION_STD));
    out_msg.presence_vector |=
      carma_perception_msgs::msg::ExternalObject::CONFIDENCE_PRESENCE_VECTOR;
  } else if (
    in_msg.core_data.accuracy.presence_vector |
    carma_v2x_msgs::msg::PositionalAccuracy::ACCURACY_AVAILABLE) {
    // Position accuracy available
 
    heading_variance = 1.0;  // Yaw variance is not available so mark as
                             // impossible perfect case
 
    // Same calculation as shown in above condition. See that for description
    out_msg.confidence = 1.0 - std::min(1.0, fabs(largest_position_std / MAX_POSITION_STD));
    out_msg.presence_vector |=
      carma_perception_msgs::msg::ExternalObject::CONFIDENCE_PRESENCE_VECTOR;
  } else if (
    in_msg.core_data.accuracy.presence_vector |
    carma_v2x_msgs::msg::PositionalAccuracy::ACCURACY_ORIENTATION_AVAILABLE) {
    // Orientation accuracy available
 
    lat_variance = 1.0;
    lon_variance = 1.0;
  }
  // Else: No accuracies available
 
  // Compute the pose
 
  if (
    (in_msg.core_data.presence_vector & carma_v2x_msgs::msg::BSMCoreData::LATITUDE_AVAILABLE) &&
    (in_msg.core_data.presence_vector & carma_v2x_msgs::msg::BSMCoreData::LONGITUDE_AVAILABLE) &&
    (in_msg.core_data.presence_vector & carma_v2x_msgs::msg::BSMCoreData::ELEVATION_AVAILABLE) &&
    (in_msg.core_data.presence_vector & carma_v2x_msgs::msg::BSMCoreData::HEADING_AVAILABLE)) {
    double longitude = std::min(in_msg.core_data.longitude, in_msg.core_data.LONGITUDE_MAX);
    longitude = std::max(longitude, in_msg.core_data.LONGITUDE_MIN);
 
    double latitude = std::min(in_msg.core_data.latitude, in_msg.core_data.LATITUDE_MAX);
    // latitude =  std::max(latitude, in_msg.core_data.LATITUDE_MIN);
 
    double elev = std::min(in_msg.core_data.elev, in_msg.core_data.ELEVATION_MAX);
    // elev = std::max(elev, (float)in_msg.core_data.ELEVATION_MIN);
 
    double heading = std::min(in_msg.core_data.heading, in_msg.core_data.HEADING_MAX);
    // heading = std::max(heading, in_msg.core_data.HEADING_MIN);
 
    out_msg.pose = impl::pose_from_gnss(
      map_projector, ned_in_map_rotation, {latitude, longitude, elev}, heading, lat_variance,
      lon_variance, heading_variance);
    out_msg.presence_vector |= carma_perception_msgs::msg::ExternalObject::POSE_PRESENCE_VECTOR;
  }
  // Else: No pose available
 
  out_msg.presence_vector |= carma_perception_msgs::msg::ExternalObject::POSE_PRESENCE_VECTOR;
 
  // Compute predictions
  // For prediction, if the prediction is available we will sample it
  // If not then assume linear motion
 
  std::vector<geometry_msgs::msg::Pose> predicted_poses;
 
  predicted_poses = impl::sample_2d_linear_motion(
    out_msg.pose.pose, out_msg.velocity.twist.linear.x, pred_period, pred_step_size);
 
  out_msg.predictions = impl::predicted_poses_to_predicted_state(
    predicted_poses, out_msg.velocity.twist.linear.x, rclcpp::Time(out_msg.header.stamp),
    rclcpp::Duration(pred_step_size * 1e9), map_frame_id, out_msg.confidence, out_msg.confidence);
  out_msg.presence_vector |= carma_perception_msgs::msg::ExternalObject::PREDICTION_PRESENCE_VECTOR;
}

References process_bag::i, motion_computation::conversion::impl::pose_from_gnss(), motion_computation::conversion::impl::predicted_poses_to_predicted_state(), and motion_computation::conversion::impl::sample_2d_linear_motion().

Here is the call graph for this function:

convert() [2/3]

void motion_computation::conversion::convert	(	const carma_v2x_msgs::msg::MobilityPath &	in_msg,
		carma_perception_msgs::msg::ExternalObject &	out_msg,
		const lanelet::projection::LocalFrameProjector &	map_projector
	)

Definition at line 31 of file mobility_path_to_external_object.cpp.

{
  constexpr double mobility_path_points_timestep_size =
    0.1;  // Mobility path timestep size per message spec
 
  out_msg.size.x = 2.5;  // TODO(carma) identify better approach for object size in mobility path
  out_msg.size.y = 2.25;
  out_msg.size.z = 2.0;
 
  // get reference origin in ECEF (convert from cm to m)
  double ecef_x = static_cast<double>(in_msg.trajectory.location.ecef_x) / 100.0;
  double ecef_y = static_cast<double>(in_msg.trajectory.location.ecef_y) / 100.0;
  double ecef_z = static_cast<double>(in_msg.trajectory.location.ecef_z) / 100.0;
 
  // Convert general information
  // clang-off
  out_msg.presence_vector |= carma_perception_msgs::msg::ExternalObject::ID_PRESENCE_VECTOR;
  out_msg.presence_vector |= carma_perception_msgs::msg::ExternalObject::POSE_PRESENCE_VECTOR;
  out_msg.presence_vector |= carma_perception_msgs::msg::ExternalObject::VELOCITY_PRESENCE_VECTOR;
  out_msg.presence_vector |=
    carma_perception_msgs::msg::ExternalObject::OBJECT_TYPE_PRESENCE_VECTOR;
  out_msg.presence_vector |= carma_perception_msgs::msg::ExternalObject::BSM_ID_PRESENCE_VECTOR;
  out_msg.presence_vector |= carma_perception_msgs::msg::ExternalObject::DYNAMIC_OBJ_PRESENCE;
  out_msg.presence_vector |= carma_perception_msgs::msg::ExternalObject::PREDICTION_PRESENCE_VECTOR;
  // clang-on
 
  out_msg.object_type = carma_perception_msgs::msg::ExternalObject::SMALL_VEHICLE;
  std::hash<std::string> hasher;
 
  // TODO(carma) hasher returns size_t, message accept uint32_t which we might lose info
  auto hashed = hasher(in_msg.m_header.sender_id);
  out_msg.id = static_cast<uint32_t>(hashed);
 
  // convert hex std::string to uint8_t array
  for (size_t i = 0; i < in_msg.m_header.sender_bsm_id.size(); i += 2) {
    unsigned int num = 0;
    sscanf(in_msg.m_header.sender_bsm_id.substr(i, i + 2).c_str(), "%x", &num);
    out_msg.bsm_id.push_back((uint8_t)num);
  }
 
  // first point's timestamp
  out_msg.header.stamp =
    rclcpp::Time((uint64_t)in_msg.m_header.timestamp * 1e6);  // ms to nanoseconds
 
  // If it is a static object, we finished processing
  if (in_msg.trajectory.offsets.size() < 2) {
    out_msg.dynamic_obj = false;
    return;
  }
  out_msg.dynamic_obj = true;
 
  // get planned trajectory points
  carma_perception_msgs::msg::PredictedState prev_state;
  tf2::Vector3 prev_pt_ecef{ecef_x, ecef_y, ecef_z};
 
  auto prev_pt_map = impl::transform_to_map_frame(prev_pt_ecef, map_projector);
  double prev_yaw = 0.0;
 
  double message_offset_x = 0.0;  // units cm
  double message_offset_y = 0.0;
  double message_offset_z = 0.0;
 
  rclcpp::Duration mobility_path_point_delta_t(mobility_path_points_timestep_size * 1e9);
 
  // Note the usage of current vs previous in this loop can be a bit confusing
  // The intended behavior is we our always storing our prev_point but using
  // curr_pt for computing velocity at prev_point
  for (size_t i = 0; i < in_msg.trajectory.offsets.size(); i++) {
    auto curr_pt_msg = in_msg.trajectory.offsets[i];
 
    message_offset_x = static_cast<double>(curr_pt_msg.offset_x) + message_offset_x;
    message_offset_y = static_cast<double>(curr_pt_msg.offset_y) + message_offset_y;
    message_offset_z = static_cast<double>(curr_pt_msg.offset_z) + message_offset_z;
 
    tf2::Vector3 curr_pt_ecef{
      ecef_x + message_offset_x / 100.0, ecef_y + message_offset_y / 100.0,
      ecef_z + message_offset_z /
                 100.0};  // ecef_x is in m while message_offset_x is in cm. Want m as final result
    auto curr_pt_map = impl::transform_to_map_frame(curr_pt_ecef, map_projector);
 
    carma_perception_msgs::msg::PredictedState curr_state;
 
    if (i == 0)  // First point's state should be stored outside "predictions"
    {
      rclcpp::Time prev_stamp_as_time = rclcpp::Time(out_msg.header.stamp);
      rclcpp::Time updated_time_step = prev_stamp_as_time + mobility_path_point_delta_t;
 
      auto res = impl::composePredictedState(
        curr_pt_map, prev_pt_map, prev_stamp_as_time, updated_time_step,
        prev_yaw);  // Position returned is that of prev_pt_map NOT curr_pt_map
      curr_state = std::get<0>(res);
      prev_yaw = std::get<1>(res);
      // Compute out_msg pose
      out_msg.pose.pose = curr_state.predicted_position;  // Orientation computed from first point
                                                          // in offsets with location
      out_msg.velocity.twist = curr_state.predicted_velocity;  // Velocity derived from first point
 
    } else {
      // NOTE: This is where the time increment happens but it feels incorrect.
      // since the corresponding data is actually from the original prev_state stamp
      rclcpp::Time prev_stamp_as_time =
        rclcpp::Time(prev_state.header.stamp) + mobility_path_point_delta_t;
      rclcpp::Time updated_time_step = prev_stamp_as_time + mobility_path_point_delta_t;
 
      auto res = impl::composePredictedState(
        curr_pt_map, prev_pt_map, prev_stamp_as_time, updated_time_step, prev_yaw);
      curr_state = std::get<0>(res);
      prev_yaw = std::get<1>(res);
      out_msg.predictions.push_back(curr_state);
    }
 
    if (
      i == in_msg.trajectory.offsets.size() -
             1)  // if last point, copy the prev_state velocity & orientation to the last point too
    {
      curr_state.predicted_position.position.x = curr_pt_map.x();
      curr_state.predicted_position.position.y = curr_pt_map.y();
      curr_state.predicted_position.position.z = curr_pt_map.z();
      out_msg.predictions.push_back(curr_state);
    }
 
    prev_state = curr_state;
    prev_pt_map = curr_pt_map;
  }
 
  impl::calculateAngVelocityOfPredictedStates(out_msg);
 
  return;
}

References motion_computation::conversion::impl::calculateAngVelocityOfPredictedStates(), motion_computation::conversion::impl::composePredictedState(), process_bag::i, and motion_computation::conversion::impl::transform_to_map_frame().

Here is the call graph for this function:

convert() [3/3]

void motion_computation::conversion::convert	(	const carma_v2x_msgs::msg::PSM &	in_msg,
		carma_perception_msgs::msg::ExternalObject &	out_msg,
		const std::string &	map_frame_id,
		double	pred_period,
		double	pred_step_size,
		const lanelet::projection::LocalFrameProjector &	map_projector,
		const tf2::Quaternion &	ned_in_map_rotation,
		rclcpp::node_interfaces::NodeClockInterface::SharedPtr	node_clock
	)

Definition at line 38 of file psm_to_external_object_convertor.cpp.

{
  out_msg.dynamic_obj = true;  // If a PSM is sent then the object is dynamic
                               // since its a living thing
  out_msg.presence_vector |= carma_perception_msgs::msg::ExternalObject::DYNAMIC_OBJ_PRESENCE;
 
  // Generate a unique object id from the psm id
  out_msg.id = 0;
  for (int i = in_msg.id.id.size() - 1; i >= 0;
       i--) {  // using signed iterator to handle empty case
    // each byte of the psm id gets placed in one byte of the object id.
    // This should result in very large numbers which will be unlikely to
    // conflict with standard detections
    out_msg.id |= in_msg.id.id[i] << (8 * i);
  }
  out_msg.presence_vector |= carma_perception_msgs::msg::ExternalObject::ID_PRESENCE_VECTOR;
 
  // Additionally, store the id in the bsm_id field
  out_msg.bsm_id = in_msg.id.id;
  out_msg.presence_vector |= carma_perception_msgs::msg::ExternalObject::BSM_ID_PRESENCE_VECTOR;
 
  // Compute the timestamp
 
  auto psm_timestamp = impl::get_psm_timestamp(in_msg, node_clock->get_clock());
  out_msg.header.stamp = builtin_interfaces::msg::Time(psm_timestamp);
  out_msg.header.frame_id = map_frame_id;
 
  // Set the type
 
  if (
    in_msg.basic_type.type == j2735_v2x_msgs::msg::PersonalDeviceUserType::A_PEDESTRIAN ||
    in_msg.basic_type.type == j2735_v2x_msgs::msg::PersonalDeviceUserType::A_PUBLIC_SAFETY_WORKER ||
    in_msg.basic_type.type ==
      j2735_v2x_msgs::msg::PersonalDeviceUserType::AN_ANIMAL)  // Treat animals
                                                               // like people
                                                               // since we have
                                                               // no internal
                                                               // class for that
  {
    out_msg.object_type = carma_perception_msgs::msg::ExternalObject::PEDESTRIAN;
 
    // Default pedestrian size
    // Assume a
    // ExternalObject dimensions are half actual size
    // Here we assume 1.0, 1.0, 2.0
    out_msg.size.x = 0.5;
    out_msg.size.y = 0.5;
    out_msg.size.z = 1.0;
 
  } else if (
    in_msg.basic_type.type == j2735_v2x_msgs::msg::PersonalDeviceUserType::A_PEDALCYCLIST) {
    out_msg.object_type = carma_perception_msgs::msg::ExternalObject::
      MOTORCYCLE;  // Currently external object cannot represent bicycles,
                   // but motor cycle seems like the next best choice
 
    // Default bicycle size
    out_msg.size.x = 1.0;
    out_msg.size.y = 0.5;
    out_msg.size.z = 1.0;
  } else {
    out_msg.object_type = carma_perception_msgs::msg::ExternalObject::UNKNOWN;
 
    // Default pedestrian size
    out_msg.size.x = 0.5;
    out_msg.size.y = 0.5;
    out_msg.size.z = 1.0;
  }
  out_msg.presence_vector |= carma_perception_msgs::msg::ExternalObject::SIZE_PRESENCE_VECTOR;
 
  // Set the velocity
 
  out_msg.velocity.twist.linear.x = in_msg.speed.velocity;
  out_msg.presence_vector |= carma_perception_msgs::msg::ExternalObject::VELOCITY_PRESENCE_VECTOR;
  // NOTE: The velocity covariance is not provided in the PSM. In order to
  // compute it you need at least two PSM messages
  //     Tracking and associating PSM messages would be an increase in
  //     complexity for this conversion which is not warranted without an
  //     existing use case for the velocity covariance. If a use case is
  //     presented for it, such an addition can be made at that time.
 
 
  double largest_position_std = 0.0;
  double lat_variance = 0.0;
  double lon_variance = 0.0;
  double heading_variance = 0.0;     // Yaw variance is not available so mark as
                                     // impossible perfect case
  double position_confidence = 0.0;  // Default will be 10% confidence. If the position accuracy is
                                     // available then this value will be updated
 
  // A standard deviation which is larger than the acceptable value to give
  // 95% confidence interval on fitting the pedestrian within one 3.7m lane
  constexpr double MAX_POSITION_STD = 1.85;
 
  if (
    (in_msg.accuracy.presence_vector |
     carma_v2x_msgs::msg::PositionalAccuracy::ACCURACY_AVAILABLE) &&
    (in_msg.accuracy.presence_vector |
     carma_v2x_msgs::msg::PositionalAccuracy::ACCURACY_ORIENTATION_AVAILABLE)) {
    // Both accuracies available
 
    // Compute the position covariance
    // For computing confidence we will use the largest provided standard
    // deviation of position
    largest_position_std = std::max(in_msg.accuracy.semi_major, in_msg.accuracy.semi_minor);
    lat_variance = in_msg.accuracy.semi_minor * in_msg.accuracy.semi_minor;
    lon_variance = in_msg.accuracy.semi_major * in_msg.accuracy.semi_major;
    heading_variance = in_msg.accuracy.orientation * in_msg.accuracy.orientation;
 
    // NOTE: ExternalObject.msg does not clearly define what is meant by
    // position confidence
    //     Here we are providing a linear scale based on the positional accuracy
    //     where 0 confidence would denote A standard deviation which is larger
    //     than the acceptable value to give 95% confidence interval on fitting
    //     the pedestrian within one 3.7m lane
    // Set the confidence
    // Without a way of getting the velocity confidence from the PSM we will use
    // the position confidence for both
    out_msg.confidence = 1.0 - std::min(1.0, fabs(largest_position_std / MAX_POSITION_STD));
    out_msg.presence_vector |=
      carma_perception_msgs::msg::ExternalObject::CONFIDENCE_PRESENCE_VECTOR;
  } else if (
    in_msg.accuracy.presence_vector | carma_v2x_msgs::msg::PositionalAccuracy::ACCURACY_AVAILABLE) {
    // Position accuracy available
 
    largest_position_std = std::max(in_msg.accuracy.semi_major, in_msg.accuracy.semi_minor);
    lat_variance = in_msg.accuracy.semi_minor * in_msg.accuracy.semi_minor;
    lon_variance = in_msg.accuracy.semi_major * in_msg.accuracy.semi_major;
    heading_variance = 1.0;  // Yaw variance is not available so mark as
                             // impossible perfect case
 
    // Same calculation as shown in above condition. See that for description
    out_msg.confidence = 1.0 - std::min(1.0, fabs(largest_position_std / MAX_POSITION_STD));
    out_msg.presence_vector |=
      carma_perception_msgs::msg::ExternalObject::CONFIDENCE_PRESENCE_VECTOR;
 
  } else if (
    in_msg.accuracy.presence_vector |
    carma_v2x_msgs::msg::PositionalAccuracy::ACCURACY_ORIENTATION_AVAILABLE) {
    // Orientation accuracy available
 
    lat_variance = 1.0;
    lon_variance = 1.0;
 
    heading_variance = in_msg.accuracy.orientation * in_msg.accuracy.orientation;
  } else {  // No accuracies available
    lat_variance = 1.0;
    lon_variance = 1.0;
    heading_variance = 1.0;
  }
 
  // Compute the pose
  lanelet::GPSPoint gps_point(
    {in_msg.position.latitude, in_msg.position.longitude, in_msg.position.elevation});
  out_msg.pose = impl::pose_from_gnss(
    map_projector, ned_in_map_rotation, gps_point, in_msg.heading.heading, lat_variance,
    lon_variance, heading_variance);
  out_msg.presence_vector |= carma_perception_msgs::msg::ExternalObject::POSE_PRESENCE_VECTOR;
 
  // Compute predictions
  // For prediction, if the prediction is available we will sample it
  // If not then assume linear motion
 
  std::vector<geometry_msgs::msg::Pose> predicted_poses;
 
  if (in_msg.presence_vector & carma_v2x_msgs::msg::PSM::HAS_PATH_PREDICTION) {
    // Based on the vehicle frame used in j2735 positive should be to the right
    // and negative to the left
    predicted_poses = impl::sample_2d_path_from_radius(
      out_msg.pose.pose, out_msg.velocity.twist.linear.x,
      -in_msg.path_prediction.radius_of_curvature, pred_period, pred_step_size);
  } else {
    predicted_poses = impl::sample_2d_linear_motion(
      out_msg.pose.pose, out_msg.velocity.twist.linear.x, pred_period, pred_step_size);
  }
 
  out_msg.predictions = impl::predicted_poses_to_predicted_state(
    predicted_poses, out_msg.velocity.twist.linear.x, rclcpp::Time(out_msg.header.stamp),
    rclcpp::Duration(pred_step_size * 1e9), map_frame_id, out_msg.confidence, out_msg.confidence);
  out_msg.presence_vector |= carma_perception_msgs::msg::ExternalObject::PREDICTION_PRESENCE_VECTOR;
}

References motion_computation::conversion::impl::get_psm_timestamp(), process_bag::i, motion_computation::conversion::impl::pose_from_gnss(), motion_computation::conversion::impl::predicted_poses_to_predicted_state(), motion_computation::conversion::impl::sample_2d_linear_motion(), and motion_computation::conversion::impl::sample_2d_path_from_radius().

Referenced by motion_computation::MotionComputationWorker::bsmCallback(), gnss_to_map_convertor::GNSSToMapConvertor::gnssFixCb(), motion_computation::MotionComputationWorker::mobilityPathCallback(), motion_computation::MotionComputationWorker::psmCallback(), carma_wm::geometry::rpyFromQuaternion(), and carma_cloud_client::CarmaCloudClient::XMLconversion().

Here is the call graph for this function:

Here is the caller graph for this function: