motion_computation::conversion::impl Namespace Reference

Functions
std::pair< carma_perception_msgs::msg::PredictedState, double >	composePredictedState (const tf2::Vector3 &curr_pt, const tf2::Vector3 &prev_pt, const rclcpp::Time &prev_time_stamp, const rclcpp::Time &curr_time_stamp, double prev_yaw)
	Composes a PredictedState message form a provided current point and previous point. It calculates the speed from these points using mobility_path_time_step. More...
void	calculateAngVelocityOfPredictedStates (carma_perception_msgs::msg::ExternalObject &object)
	Helper function to fill in the angular velocity of the external object. More...
double	getYawFromQuaternionMsg (const geometry_msgs::msg::Quaternion &quaternion)
	Gets the yaw angle in degrees described by the provided quaternion. More...
tf2::Vector3	transform_to_map_frame (const tf2::Vector3 &ecef_point, const lanelet::projection::LocalFrameProjector &map_projector)
	Transforms ecef point to map frame using internally saved map transform. More...
std::vector< geometry_msgs::msg::Pose >	sample_2d_path_from_radius (const geometry_msgs::msg::Pose &pose, double velocity, double radius_of_curvature, double period, double step_size)
std::vector< geometry_msgs::msg::Pose >	sample_2d_linear_motion (const geometry_msgs::msg::Pose &pose, double velocity, double period, double step_size)
geometry_msgs::msg::PoseWithCovariance	pose_from_gnss (const lanelet::projection::LocalFrameProjector &projector, const tf2::Quaternion &ned_in_map_rotation, const lanelet::GPSPoint &gps_point, const double &heading, const double lat_variance, const double lon_variance, const double heading_variance)
std::vector< carma_perception_msgs::msg::PredictedState >	predicted_poses_to_predicted_state (const std::vector< geometry_msgs::msg::Pose > &poses, double constant_velocity, const rclcpp::Time &start_time, const rclcpp::Duration &step_size, const std::string &frame, double initial_pose_confidence, double initial_vel_confidence)
rclcpp::Time	get_psm_timestamp (const carma_v2x_msgs::msg::PSM &in_msg, rclcpp::Clock::SharedPtr clock)

Function Documentation

calculateAngVelocityOfPredictedStates()

void motion_computation::conversion::impl::calculateAngVelocityOfPredictedStates

(

carma_perception_msgs::msg::ExternalObject &

object

)

Helper function to fill in the angular velocity of the external object.

Parameters

ExternalObject

to fill in its angular velocities

Definition at line 212 of file mobility_path_to_external_object.cpp.

{
  if (!object.dynamic_obj || object.predictions.size() == 0) {
    return;
  }
 
  // Object's current angular velocity
  double dt = (rclcpp::Time(object.header.stamp) - rclcpp::Time(object.predictions[0].header.stamp))
                .seconds();
  object.velocity.twist.angular.z =
    (impl::getYawFromQuaternionMsg(object.pose.pose.orientation) -
     impl::getYawFromQuaternionMsg(object.predictions[0].predicted_position.orientation)) /
    dt;
 
  // Predictions' angular velocities
  auto prev_orient = object.pose.pose.orientation;
  auto prev_time = rclcpp::Time(object.predictions[0].header.stamp);
  for (auto & pred : object.predictions) {
    dt = (rclcpp::Time(pred.header.stamp) - prev_time).seconds();
    pred.predicted_velocity.angular.z =
      (impl::getYawFromQuaternionMsg(prev_orient) -
       impl::getYawFromQuaternionMsg(pred.predicted_position.orientation)) /
      dt;
  }
}

References getYawFromQuaternionMsg().

Referenced by motion_computation::conversion::convert().

Here is the call graph for this function:

Here is the caller graph for this function:

composePredictedState()

std::pair< carma_perception_msgs::msg::PredictedState, double > motion_computation::conversion::impl::composePredictedState	(	const tf2::Vector3 &	curr_pt,
		const tf2::Vector3 &	prev_pt,
		const rclcpp::Time &	prev_time_stamp,
		const rclcpp::Time &	curr_time_stamp,
		double	prev_yaw
	)

Composes a PredictedState message form a provided current point and previous point. It calculates the speed from these points using mobility_path_time_step.

Parameters

curr_pt	current point
prev_pt	prev_pt. this point is recorded in the state
prev_time_stamp	prev_pt's time stamp. This time is recorded in the state
curr_time_stamp	The timestamp of the second point when used to compute velocity at prev point
prev_yaw	A previous yaw value in radians to use if the two provided points are equivalent

Returns

std::pair<carma_perception_msgs::msg::PredictedState,double> where the first element is the prediction including linear velocity, last_time, orientation filled in and the second element is the yaw in radians used to compute the orientation

Definition at line 167 of file mobility_path_to_external_object.cpp.

{
  carma_perception_msgs::msg::PredictedState output_state;
  // Set Position
  output_state.predicted_position.position.x = prev_pt.x();
  output_state.predicted_position.position.y = prev_pt.y();
  output_state.predicted_position.position.z = prev_pt.z();
 
  // Set Orientation
  Eigen::Vector2d vehicle_vector = {curr_pt.x() - prev_pt.x(), curr_pt.y() - prev_pt.y()};
  Eigen::Vector2d x_axis = {1, 0};
  double yaw = 0.0;
  if (
    vehicle_vector.norm() <
    0.000001) {  // If there is zero magnitude use previous yaw to avoid divide by 0
    yaw = prev_yaw;
    RCLCPP_DEBUG_STREAM(
      rclcpp::get_logger("motion_computation::conversion"),
      "Two identical points sent for predicting heading. Forced to use previous yaw or 0 if "
      "first "
      "point");
  } else {
    yaw = std::acos(vehicle_vector.dot(x_axis) / (vehicle_vector.norm() * x_axis.norm()));
  }
 
  tf2::Quaternion vehicle_orientation;
  vehicle_orientation.setRPY(0, 0, yaw);
  output_state.predicted_position.orientation.x = vehicle_orientation.getX();
  output_state.predicted_position.orientation.y = vehicle_orientation.getY();
  output_state.predicted_position.orientation.z = vehicle_orientation.getZ();
  output_state.predicted_position.orientation.w = vehicle_orientation.getW();
 
  // Set velocity
  output_state.predicted_velocity.linear.x =
 
    vehicle_vector.norm() / (curr_time_stamp - prev_time_stamp).seconds();
 
  // Set timestamp
  output_state.header.stamp = builtin_interfaces::msg::Time(prev_time_stamp);
 
  return std::make_pair(output_state, yaw);
}

Referenced by motion_computation::conversion::convert().

Here is the caller graph for this function:

get_psm_timestamp()

rclcpp::Time motion_computation::conversion::impl::get_psm_timestamp	(	const carma_v2x_msgs::msg::PSM &	in_msg,
		rclcpp::Clock::SharedPtr	clock
	)

Definition at line 477 of file psm_to_external_object_convertor.cpp.

{
  boost::posix_time::ptime utc_time_of_current_psm;
 
  // Get the utc epoch start time
  static const boost::posix_time::ptime inception_boost(
    boost::posix_time::time_from_string("1970-01-01 00:00:00.000"));
 
  // Determine if the utc time of the path history can be used instead of the
  // sec_mark The sec_mark is susceptible to large error on minute transitions
  // due to missing "minute of the year" field If the second mark in the path
  // history is identical and the full utc time is provided with ms resolution
  // then it can be assumed the initial_position is the same as the PSM data and
  // the utc_time can be used instead of sec_mark
  // clang-format off
 
  const auto initial_position_time{in_msg.path_history.initial_position.utc_time};
  if ((in_msg.presence_vector &
        carma_v2x_msgs::msg::PSM::HAS_PATH_HISTORY) &&
        (in_msg.path_history.presence_vector &
        carma_v2x_msgs::msg::PathHistory::HAS_INITIAL_POSITION) &&
        (initial_position_time.presence_vector & j2735_v2x_msgs::msg::DDateTime::YEAR) &&
        (initial_position_time.presence_vector & j2735_v2x_msgs::msg::DDateTime::MONTH) &&
        (initial_position_time.presence_vector & j2735_v2x_msgs::msg::DDateTime::DAY) &&
        (initial_position_time.presence_vector & j2735_v2x_msgs::msg::DDateTime::HOUR) &&
        (initial_position_time.presence_vector & j2735_v2x_msgs::msg::DDateTime::MINUTE) &&
        (initial_position_time.presence_vector & j2735_v2x_msgs::msg::DDateTime::SECOND) &&
      (in_msg.sec_mark.millisecond == initial_position_time.second.millisecond)) {
    // clang-format on
    RCLCPP_DEBUG_STREAM(
      rclcpp::get_logger("motion_computation::conversion"),
      "Using UTC time of path history to determine PSM "
      "timestamp. Assumed valid since UTC is fully specified "
      "and sec_mark == utc_time.seconds in this message.");
 
    boost::posix_time::time_duration time_of_day =
      boost::posix_time::hours(in_msg.path_history.initial_position.utc_time.hour.hour) +
      boost::posix_time::minutes(in_msg.path_history.initial_position.utc_time.minute.minute) +
      boost::posix_time::milliseconds(
        in_msg.path_history.initial_position.utc_time.second.millisecond);
 
    boost::gregorian::date utc_day(
      in_msg.path_history.initial_position.utc_time.year.year,
      in_msg.path_history.initial_position.utc_time.month.month,
      in_msg.path_history.initial_position.utc_time.day.day);
 
    utc_time_of_current_psm = boost::posix_time::ptime(utc_day) + time_of_day;
  } else {  // If the utc time of the path history cannot be used to account for minute
            // change over, then we have to default to the sec mark
 
    RCLCPP_WARN_STREAM_THROTTLE(
      rclcpp::get_logger("motion_computation::conversion"), *clock.get(),
      rclcpp::Duration(5, 0).nanoseconds(),
      "PSM PathHistory utc timstamp does not match "
      "sec_mark. Unable to determine the minute of "
      "the year used for PSM data. Assuming local "
      "clock is exactly synced. This is NOT "
      "ADVISED.");
 
    // Get the current ROS time
    auto current_time = clock->now();
 
    // Convert the ros time to a boost duration
    boost::posix_time::time_duration duration_since_inception(
      lanelet::time::durationFromSec(current_time.seconds()));
 
    // Get the current ROS time in UTC
    auto curr_time_boost = inception_boost + duration_since_inception;
 
    // Get duration of current day
    auto duration_in_day_till_current_time = curr_time_boost.time_of_day();
 
    // Extract hours and minutes
    auto hour_count_in_day = duration_in_day_till_current_time.hours();
    auto minute_count_in_hour = duration_in_day_till_current_time.minutes();
 
    // Get the duration of the minute in the day
    auto start_of_minute_in_day = boost::posix_time::hours(hour_count_in_day) +
                                  boost::posix_time::minutes(minute_count_in_hour);
 
    // Get the start of the day in ROS time
    boost::posix_time::ptime start_of_day(curr_time_boost.date());
 
    // Ge the start of the current minute in ROS time
    boost::posix_time::ptime utc_start_of_current_minute = start_of_day + start_of_minute_in_day;
 
    // Compute the UTC PSM stamp from the sec_mark using ROS time as the clock
 
    boost::posix_time::time_duration s_in_cur_minute =
      boost::posix_time::milliseconds(in_msg.sec_mark.millisecond);
 
    utc_time_of_current_psm = utc_start_of_current_minute + s_in_cur_minute;
  }
 
  boost::posix_time::time_duration nsec_since_epoch = utc_time_of_current_psm - inception_boost;
 
  if (nsec_since_epoch.is_special()) {
    RCLCPP_ERROR_STREAM(
      rclcpp::get_logger("motion_computation::conversion"),
      "Computed psm nsec_since_epoch is special (computation "
      "failed). Value effectively undefined.");
  }
 
  return rclcpp::Time(nsec_since_epoch.total_nanoseconds());
}

Referenced by motion_computation::conversion::convert().

Here is the caller graph for this function:

getYawFromQuaternionMsg()

double motion_computation::conversion::impl::getYawFromQuaternionMsg

(

const geometry_msgs::msg::Quaternion &

quaternion

)

Gets the yaw angle in degrees described by the provided quaternion.

Parameters

quaternion

The quaternion to extract yaw from

Returns

The yaw in degrees

Definition at line 238 of file mobility_path_to_external_object.cpp.

{
  tf2::Quaternion orientation;
  orientation.setX(quaternion.x);
  orientation.setY(quaternion.y);
  orientation.setZ(quaternion.z);
  orientation.setW(quaternion.w);
 
  double roll;
  double pitch;
  double yaw;
  tf2::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
 
  return yaw;
}

Referenced by calculateAngVelocityOfPredictedStates().

Here is the caller graph for this function:

pose_from_gnss()

geometry_msgs::msg::PoseWithCovariance motion_computation::conversion::impl::pose_from_gnss	(	const lanelet::projection::LocalFrameProjector &	projector,
		const tf2::Quaternion &	ned_in_map_rotation,
		const lanelet::GPSPoint &	gps_point,
		const double &	heading,
		const double	lat_variance,
		const double	lon_variance,
		const double	heading_variance
	)

Convert the position information into the map frame using the proj library

Convert the position information into the map frame using the proj library

Definition at line 340 of file psm_to_external_object_convertor.cpp.

{
  lanelet::BasicPoint3d map_point = projector.forward(gps_point);
 
  RCLCPP_DEBUG_STREAM(
    rclcpp::get_logger("motion_computation::conversion"),
    "map_point: " << map_point.x() << ", " << map_point.y() << ", " << map_point.z());
 
  if (
    fabs(map_point.x()) > 10000.0 ||
    fabs(map_point.y()) > 10000.0) {  // Above 10km from map origin earth curvature will start to
                                      // have a negative impact on system performance
 
    RCLCPP_WARN_STREAM(
      rclcpp::get_logger("motion_computation::conversion"),
      "Distance from map origin is larger than supported by system "
      "assumptions. Strongly advise "
      "alternative map origin be used. ");
  }
 
  // This logic assumes that the orientation difference between an NED frame
  // located at the map origin and an NED frame located at the GNSS point are
  // sufficiently small that they can be ignored. Therefore it is assumed the
  // heading report of the GNSS system regardless of its postion in the map
  // without change in its orientation will give the same result (as far as we
  // are concered).
 
  tf2::Quaternion R_m_n(ned_in_map_rotation);  // Rotation of NED frame in map
                                               // frame
  tf2::Quaternion R_n_h;                       // Rotation of sensor heading report in NED frame
  R_n_h.setRPY(0, 0, heading * wgs84_utils::DEG2RAD);
 
  tf2::Quaternion R_m_s = R_m_n * R_n_h;  // Rotation of sensor in map frame under assumption that
                                          // distance from map origin is sufficiently small so as to
                                          // ignore local changes in NED orientation
 
  RCLCPP_DEBUG_STREAM(
    rclcpp::get_logger("motion_computation::conversion"),
    "R_m_n (x,y,z,w) : ( " << R_m_n.x() << ", " << R_m_n.y() << ", " << R_m_n.z() << ", "
                           << R_m_n.w());
 
  RCLCPP_DEBUG_STREAM(
    rclcpp::get_logger("motion_computation::conversion"),
    "R_n_h (x,y,z,w) : ( " << R_n_h.x() << ", " << R_n_h.y() << ", " << R_n_h.z() << ", "
                           << R_n_h.w());
 
  RCLCPP_DEBUG_STREAM(
    rclcpp::get_logger("motion_computation::conversion"),
    "R_m_s (x,y,z,w) : ( " << R_m_s.x() << ", " << R_m_s.y() << ", " << R_m_s.z() << ", "
                           << R_m_s.w());
 
  tf2::Transform T_m_s(
    R_m_s, tf2::Vector3(
             map_point.x(), map_point.y(),
             map_point.z()));  // Reported position and orientation
                               // of sensor frame in map frame
 
  tf2::Transform T_m_n_no_heading(
    R_m_n, tf2::Vector3(0, 0, 0));  // Used to transform the covariance
 
  // This covariance represents the covariance of the NED frame N-lat,
  // E-lon, heading- angle east of north This means that the covariance is
  // in the NED frame, and needs to be transformed to the map frame
  // clang-format off
        std::array<double, 36> input_covariance = {
          lat_variance, 0, 0,  0, 0, 0,
          0, lon_variance, 0,  0, 0, 0,
          0, 0, 1,  0, 0, 0,
          0,  0,  0,  1,  0, 0,
          0,  0,  0,  0,  1, 0,
          0,  0,  0,  0,  0, heading_variance
        };
  // clang-format on
 
  std::array<double, 36> new_cov = tf2::transformCovariance(
    input_covariance,
    // Per the usage of transformCovariance in tf2_geometry_msgs
    // this frame should be the transform between the map which the pose
    // is in an the target frame.
    T_m_n_no_heading);
 
  // Populate message
  geometry_msgs::msg::PoseWithCovariance pose;
  pose.pose.position.x = T_m_s.getOrigin().getX();
  pose.pose.position.y = T_m_s.getOrigin().getY();
  pose.pose.position.z = T_m_s.getOrigin().getZ();
 
  pose.pose.orientation.x = T_m_s.getRotation().getX();
  pose.pose.orientation.y = T_m_s.getRotation().getY();
  pose.pose.orientation.z = T_m_s.getRotation().getZ();
  pose.pose.orientation.w = T_m_s.getRotation().getW();
 
  pose.covariance = new_cov;
 
  return pose;
}

References covariance_helper::transformCovariance().

Referenced by motion_computation::conversion::convert().

Here is the call graph for this function:

Here is the caller graph for this function:

predicted_poses_to_predicted_state()

std::vector< carma_perception_msgs::msg::PredictedState > motion_computation::conversion::impl::predicted_poses_to_predicted_state	(	const std::vector< geometry_msgs::msg::Pose > &	poses,
		double	constant_velocity,
		const rclcpp::Time &	start_time,
		const rclcpp::Duration &	step_size,
		const std::string &	frame,
		double	initial_pose_confidence,
		double	initial_vel_confidence
	)

Definition at line 444 of file psm_to_external_object_convertor.cpp.

{
  std::vector<carma_perception_msgs::msg::PredictedState> output;
  output.reserve(poses.size());
 
  rclcpp::Time time(start_time);
 
  for (auto p : poses) {
    time += step_size;
    carma_perception_msgs::msg::PredictedState pred_state;
    pred_state.header.stamp = time;
    pred_state.header.frame_id = frame;
 
    pred_state.predicted_position = p;
    pred_state.predicted_position_confidence =
      0.9 * initial_pose_confidence;  // Reduce confidence by 10 % per timestep
    initial_pose_confidence = pred_state.predicted_position_confidence;
 
    pred_state.predicted_velocity.linear.x = constant_velocity;
    pred_state.predicted_velocity_confidence =
      0.9 * initial_vel_confidence;  // Reduce confidence by
                                     // 10 % per timestep
    initial_vel_confidence = pred_state.predicted_velocity_confidence;
 
    output.push_back(pred_state);
  }
 
  return output;
}

Referenced by motion_computation::conversion::convert().

Here is the caller graph for this function:

sample_2d_linear_motion()

std::vector< geometry_msgs::msg::Pose > motion_computation::conversion::impl::sample_2d_linear_motion	(	const geometry_msgs::msg::Pose &	pose,
		double	velocity,
		double	period,
		double	step_size
	)

Definition at line 294 of file psm_to_external_object_convertor.cpp.

{
  std::vector<geometry_msgs::msg::Pose> output;
  output.reserve((period / step_size) + 1);
 
  tf2::Vector3 pose_in_map_translation(pose.position.x, pose.position.y, pose.position.z);
  tf2::Quaternion pose_in_map_quat(
    pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
 
  tf2::Transform pose_in_map(pose_in_map_quat, pose_in_map_translation);
 
  double total_dt = 0;
 
  while (total_dt < period) {
    // Compute the 2d position and orientation in the Pose frame
    total_dt += step_size;
    double dx_from_start = velocity * total_dt;  // Assuming linear motion in pose frame
 
    double x = pose.position.x + dx_from_start;
 
    tf2::Vector3 position(x, 0, 0);
 
    // Convert the position and orientation in the pose frame to the map frame
    tf2::Transform pose_to_sample(tf2::Quaternion::getIdentity(), position);
    tf2::Transform map_to_sample = pose_in_map * pose_to_sample;
 
    geometry_msgs::msg::Pose sample_pose;
 
    sample_pose.position.x = map_to_sample.getOrigin().x();
    sample_pose.position.y = map_to_sample.getOrigin().y();
    sample_pose.position.z = map_to_sample.getOrigin().z();
 
    sample_pose.orientation.x = map_to_sample.getRotation().x();
    sample_pose.orientation.y = map_to_sample.getRotation().y();
    sample_pose.orientation.z = map_to_sample.getRotation().z();
    sample_pose.orientation.w = map_to_sample.getRotation().w();
 
    output.emplace_back(sample_pose);
  }
 
  return output;
}

References process_traj_logs::x.

Referenced by motion_computation::conversion::convert().

Here is the caller graph for this function:

sample_2d_path_from_radius()

std::vector< geometry_msgs::msg::Pose > motion_computation::conversion::impl::sample_2d_path_from_radius	(	const geometry_msgs::msg::Pose &	pose,
		double	velocity,
		double	radius_of_curvature,
		double	period,
		double	step_size
	)

Definition at line 237 of file psm_to_external_object_convertor.cpp.

{
  std::vector<geometry_msgs::msg::Pose> output;
  output.reserve((period / step_size) + 1);
 
  tf2::Vector3 pose_in_map_translation(pose.position.x, pose.position.y, pose.position.z);
  tf2::Quaternion pose_in_map_quat(
    pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
  tf2::Transform pose_in_map(pose_in_map_quat, pose_in_map_translation);
 
  // The radius of curvature originates from the frame of the provided pose
  // So the turning center is at (0, r)
  double center_x_in_pose = 0;
  double center_y_in_pose = radius_of_curvature;
 
  double total_dt = 0;
 
  while (total_dt < period) {
    // Compute the 2d position and orientation in the Pose frame
    total_dt += step_size;
    double delta_arc_length = velocity * total_dt;  // Assumes perfect point motion along curve
 
    double turning_angle = delta_arc_length / radius_of_curvature;
    double dx_from_center = radius_of_curvature * sin(turning_angle);
    double dy_from_center = radius_of_curvature * cos(turning_angle);
 
    double x = center_x_in_pose + dx_from_center;
    double y = center_y_in_pose + dy_from_center;
 
    tf2::Vector3 position(x, y, 0);
 
    tf2::Quaternion quat;
    quat.setRPY(0, 0, turning_angle);
 
    // Convert the position and orientation in the pose frame to the map frame
    tf2::Transform pose_to_sample(quat, position);
    tf2::Transform map_to_sample = pose_in_map * pose_to_sample;
 
    geometry_msgs::msg::Pose sample_pose;
 
    sample_pose.position.x = map_to_sample.getOrigin().x();
    sample_pose.position.y = map_to_sample.getOrigin().y();
    sample_pose.position.z = pose.orientation.z;  // Reuse the z position from the initial pose
 
    sample_pose.orientation.x = map_to_sample.getRotation().x();
    sample_pose.orientation.y = map_to_sample.getRotation().y();
    sample_pose.orientation.z = map_to_sample.getRotation().z();
    sample_pose.orientation.w = map_to_sample.getRotation().w();
 
    output.emplace_back(sample_pose);
  }
 
  return output;
}

References process_traj_logs::x, and process_traj_logs::y.

Referenced by motion_computation::conversion::convert().

Here is the caller graph for this function:

transform_to_map_frame()

tf2::Vector3 motion_computation::conversion::impl::transform_to_map_frame	(	const tf2::Vector3 &	ecef_point,
		const lanelet::projection::LocalFrameProjector &	map_projector
	)

Transforms ecef point to map frame using internally saved map transform.

Parameters

ecef_point	ecef_point to transform
map_projector	Projector used for frame conversion

Returns

point in map

Definition at line 254 of file mobility_path_to_external_object.cpp.

{
  lanelet::BasicPoint3d map_point = map_projector.projectECEF(
    {ecef_point.x(), ecef_point.y(), ecef_point.z()},
    -1);  // Input should already be converted to m
 
  return tf2::Vector3(map_point.x(), map_point.y(), map_point.z());
}

Referenced by motion_computation::conversion::convert().

Here is the caller graph for this function: