Classes
struct	AccelerationSet4Way

struct	DDateTime

class	DetectionListVizNode

class	ExternalObjectListToDetectionListNode

class	ExternalObjectListToSdsmNode

struct	Heading

class	HostVehicleFilterNode

struct	MapCoordinate

struct	MeasurementTimeOffset

struct	MetricSe2
	Calculates distance between a point and detection in SE(2) (special Euclidean) space. More...

class	Month

struct	MotionModelMapping

class	MultipleObjectTrackerNode

struct	Position3D

struct	PositionOffsetXYZ

class	SdsmToDetectionListConfig

class	SdsmToDetectionListNode

struct	SemanticDistance2dScore
	Calculate 2D Euclidean distance between track and detection. More...

struct	Speed

class	TrackListToExternalObjectListNode

struct	UtmCoordinate
	Represents a position using UTM coordinates. More...

struct	UtmDisplacement
	Represent a displacement from a UTM coordinate. More...

struct	UtmZone

struct	Wgs84Coordinate
	Represents a position using WGS-84 coordinates. More...

Typedefs
using	Detection = std::variant< multiple_object_tracking::CtrvDetection, multiple_object_tracking::CtraDetection >

using	Track = std::variant< multiple_object_tracking::CtrvTrack, multiple_object_tracking::CtraTrack >

Enumerations
enum class	Hemisphere { kNorth , kSouth }

Functions
auto	transform_from_map_to_utm (carma_cooperative_perception_interfaces::msg::DetectionList detection_list, const std::string &map_origin) -> carma_cooperative_perception_interfaces::msg::DetectionList

constexpr auto	operator+= (UtmCoordinate &coordinate, const UtmDisplacement &displacement) -> UtmCoordinate &
	Addition-assignment operator overload. More...

constexpr auto	operator+ (UtmCoordinate coordinate, const UtmDisplacement &displacement) -> UtmCoordinate
	Addition operator overload. More...

constexpr auto	operator+ (const UtmDisplacement &displacement, UtmCoordinate coordinate) -> UtmCoordinate
	Addition operator overload. More...

constexpr auto	operator-= (UtmCoordinate &coordinate, const UtmDisplacement &displacement) -> UtmCoordinate &
	Subtraction-assignment operator overload. More...

constexpr auto	operator- (UtmCoordinate coordinate, const UtmDisplacement &displacement) -> UtmCoordinate
	Subtraction operator overload. More...

constexpr auto	operator- (const UtmDisplacement &displacement, UtmCoordinate coordinate) -> UtmCoordinate
	Subtraction operator overload. More...

auto	calculate_utm_zone (const Wgs84Coordinate &coordinate) -> UtmZone
	Get the UTM zone number from a WGS-84 coordinate. More...

auto	project_to_carma_map (const Wgs84Coordinate &coordinate, std::string_view proj_string) -> MapCoordinate

auto	project_to_utm (const Wgs84Coordinate &coordinate) -> UtmCoordinate
	Projects a Wgs84Coordinate to its corresponding UTM zone. More...

auto	calculate_grid_convergence (const Wgs84Coordinate &position, const UtmZone &zone) -> units::angle::degree_t
	Calculate grid convergence at a given position. More...

auto	calculate_grid_convergence (const Wgs84Coordinate &position, std::string_view georeference) -> units::angle::degree_t

auto	euclidean_distance_squared (const geometry_msgs::msg::Pose &a, const geometry_msgs::msg::Pose &b) -> double

auto	to_time_msg (const DDateTime &d_date_time, bool is_simulation) -> builtin_interfaces::msg::Time

auto	calc_detection_time_stamp (DDateTime d_date_time, const MeasurementTimeOffset &offset) -> DDateTime

auto	to_ddate_time_msg (const builtin_interfaces::msg::Time &builtin_time) -> j2735_v2x_msgs::msg::DDateTime

auto	calc_sdsm_time_offset (const builtin_interfaces::msg::Time &external_object_list_time, const builtin_interfaces::msg::Time &external_object_time) -> carma_v2x_msgs::msg::MeasurementTimeOffset

auto	to_position_msg (const UtmCoordinate &position_utm) -> geometry_msgs::msg::Point

auto	heading_to_enu_yaw (const units::angle::degree_t &heading) -> units::angle::degree_t

auto	calc_relative_position (const geometry_msgs::msg::PoseStamped &current_pose, const carma_v2x_msgs::msg::PositionOffsetXYZ &detected_object_data) -> carma_v2x_msgs::msg::PositionOffsetXYZ

auto	transform_pose_from_map_to_wgs84 (const geometry_msgs::msg::PoseStamped &source_pose, const std::shared_ptr< lanelet::projection::LocalFrameProjector > &map_projection) -> carma_v2x_msgs::msg::Position3D

auto	to_detection_list_msg (const carma_v2x_msgs::msg::SensorDataSharingMessage &sdsm, std::string_view georeference, bool is_simulation, const std::optional< SdsmToDetectionListConfig > &conversion_adjustment) -> carma_cooperative_perception_interfaces::msg::DetectionList
	Converts a carma_v2x_msgs::msg::SensorDataSharingMessage (SDSM) to carma_cooperative_perception_interfaces::msg::DetectionList format. More...

auto	to_detection_msg (const carma_perception_msgs::msg::ExternalObject &object, const MotionModelMapping &motion_model_mapping) -> carma_cooperative_perception_interfaces::msg::Detection

auto	to_detection_list_msg (const carma_perception_msgs::msg::ExternalObjectList &object_list, const MotionModelMapping &motion_model_mapping) -> carma_cooperative_perception_interfaces::msg::DetectionList

auto	to_external_object_msg (const carma_cooperative_perception_interfaces::msg::Track &track) -> carma_perception_msgs::msg::ExternalObject

auto	to_external_object_list_msg (const carma_cooperative_perception_interfaces::msg::TrackList &track_list) -> carma_perception_msgs::msg::ExternalObjectList

auto	to_sdsm_msg (const carma_perception_msgs::msg::ExternalObjectList &external_object_list, const geometry_msgs::msg::PoseStamped &current_pose, const std::shared_ptr< lanelet::projection::LocalFrameProjector > &map_projection) -> carma_v2x_msgs::msg::SensorDataSharingMessage

auto	to_detected_object_data_msg (const carma_perception_msgs::msg::ExternalObject &external_object, const std::shared_ptr< lanelet::projection::LocalFrameProjector > &map_projection) -> carma_v2x_msgs::msg::DetectedObjectData

auto	enu_orientation_to_true_heading (double yaw, const lanelet::BasicPoint3d &obj_pose, const std::shared_ptr< lanelet::projection::LocalFrameProjector > &map_projection) -> units::angle::degree_t

auto	make_detection (const carma_cooperative_perception_interfaces::msg::Detection &msg) -> Detection

template<typename T >
constexpr auto	remove_units (const T &value)

constexpr auto	operator== (const UtmZone &lhs, const UtmZone &rhs) -> bool

constexpr auto	operator!= (const UtmZone &lhs, const UtmZone &rhs) -> bool

auto	to_string (const UtmZone &zone) -> std::string

auto	ned_to_enu (const PositionOffsetXYZ &offset_ned) noexcept

auto	to_position_msg (const MapCoordinate &position_map) -> geometry_msgs::msg::Point

std::string	to_string (const std::vector< std::uint8_t > &temporary_id)

void	convert_object_type (carma_cooperative_perception_interfaces::msg::Detection &detection, const j3224_v2x_msgs::msg::ObjectType &j3224_obj_type)

void	convert_covariances (carma_cooperative_perception_interfaces::msg::Detection &detection, const carma_v2x_msgs::msg::DetectedObjectCommonData &common_data, const std::optional< SdsmToDetectionListConfig > &conversion_adjustment)

auto	make_semantic_class (std::size_t numeric_value)

auto	semantic_class_to_numeric_value (mot::SemanticClass semantic_class)

auto	make_ctrv_detection (const carma_cooperative_perception_interfaces::msg::Detection &msg) -> Detection

auto	make_ctra_detection (const carma_cooperative_perception_interfaces::msg::Detection &msg) -> Detection

static auto	to_ros_msg (const mot::CtraTrack &track)

static auto	to_ros_msg (const mot::CtrvTrack &track)

static auto	to_ros_msg (const Track &track)

static auto	temporally_align_detections (std::vector< Detection > &detections, units::time::second_t end_time) -> void

static auto	predict_track_states (std::vector< Track > tracks, units::time::second_t end_time)

Variables
constexpr Month	January {1}

constexpr Month	February {2}

constexpr Month	March {3}

constexpr Month	April {4}

constexpr Month	May {5}

constexpr Month	June {6}

constexpr Month	July {7}

constexpr Month	August {8}

constexpr Month	September {9}

constexpr Month	October {10}

constexpr Month	November {11}

constexpr Month	December {12}

Detailed Description

This file contains functions and helper structs to facilitate transforming WGS-84 coordinates to UTM ones.

This file contains a Month class implementation that should be source-compatible with std::chrono::month. Until CARMA targets C++20, we will have to use this instead of the standard library.

Typedef Documentation

◆ Detection

using carma_cooperative_perception::Detection = typedef std::variant<multiple_object_tracking::CtrvDetection, multiple_object_tracking::CtraDetection>

Definition at line 34 of file multiple_object_tracker_component.hpp.

◆ Track

using carma_cooperative_perception::Track = typedef std::variant<multiple_object_tracking::CtrvTrack, multiple_object_tracking::CtraTrack>

Definition at line 36 of file multiple_object_tracker_component.hpp.

Enumeration Type Documentation

◆ Hemisphere

enum class carma_cooperative_perception::Hemisphere

strong

Enumerator
kNorth
kSouth

Definition at line 23 of file utm_zone.hpp.

{
  kNorth,
  kSouth
};

Function Documentation

◆ calc_detection_time_stamp()

auto carma_cooperative_perception::calc_detection_time_stamp	(	DDateTime	d_date_time,
		const MeasurementTimeOffset &	offset
	)		-> DDateTime

Definition at line 170 of file msg_conversion.cpp.

{
  sdsm_time.second.value() += offset.measurement_time_offset;
 
  return sdsm_time;
}

References carma_cooperative_perception::DDateTime::second.

Referenced by to_detection_list_msg().

Here is the caller graph for this function:

◆ calc_relative_position()

auto carma_cooperative_perception::calc_relative_position	(	const geometry_msgs::msg::PoseStamped &	current_pose,
		const carma_v2x_msgs::msg::PositionOffsetXYZ &	detected_object_data
	)		-> carma_v2x_msgs::msg::PositionOffsetXYZ

Definition at line 305 of file msg_conversion.cpp.

{
  carma_v2x_msgs::msg::PositionOffsetXYZ adjusted_offset;
 
  adjusted_offset.offset_x.object_distance =
    position_offset.offset_x.object_distance - source_pose.pose.position.x;
  adjusted_offset.offset_y.object_distance =
    position_offset.offset_y.object_distance - source_pose.pose.position.y;
  adjusted_offset.offset_z.object_distance =
    position_offset.offset_z.object_distance - source_pose.pose.position.z;
  adjusted_offset.presence_vector = carma_v2x_msgs::msg::PositionOffsetXYZ::HAS_OFFSET_Z;
 
  return adjusted_offset;
}

Referenced by to_sdsm_msg().

Here is the caller graph for this function:

◆ calc_sdsm_time_offset()

auto carma_cooperative_perception::calc_sdsm_time_offset	(	const builtin_interfaces::msg::Time &	external_object_list_time,
		const builtin_interfaces::msg::Time &	external_object_time
	)		-> carma_v2x_msgs::msg::MeasurementTimeOffset

Definition at line 228 of file msg_conversion.cpp.

{
  carma_v2x_msgs::msg::MeasurementTimeOffset time_offset;
 
  boost::posix_time::ptime external_object_list_time =
    boost::posix_time::from_time_t(external_object_list_stamp.sec) +
    boost::posix_time::nanosec(external_object_list_stamp.nanosec);
 
  boost::posix_time::ptime external_object_time =
    boost::posix_time::from_time_t(external_object_stamp.sec) +
    boost::posix_time::nanosec(external_object_stamp.nanosec);
 
  boost::posix_time::time_duration offset_duration =
    (external_object_list_time - external_object_time);
 
  time_offset.measurement_time_offset = offset_duration.total_seconds();
 
  return time_offset;
}

Referenced by to_sdsm_msg().

Here is the caller graph for this function:

◆ calculate_grid_convergence() [1/2]

auto carma_cooperative_perception::calculate_grid_convergence	(	const Wgs84Coordinate &	position,
		const UtmZone &	zone
	)		-> units::angle::degree_t

Calculate grid convergence at a given position.

This function calculates the grid convergence at a specific coordinate with respect to a specified UTM zone. Grid convergence is the angle between true north and grid north.

Parameters

[in]	position	Position represented in WGS-84 coordinates
[in]	zone	The UTM zone

Returns: Grid convergence angle

Definition at line 154 of file geodetic.cpp.

{
  // N.B. developers: PROJ and the related geodetic calculations seem particularly sensitive
  // to the parameters in this PROJ string. If you run into problems with you calculation
  // results, carefully check this or any other PROJ string.
  std::string proj_string{
    "+proj=utm +zone=" + std::to_string(zone.number) + " +datum=WGS84 +units=m +no_defs"};
 
  if (zone.hemisphere == Hemisphere::kSouth) {
    proj_string += " +south";
  }
 
  return calculate_grid_convergence(position, proj_string.c_str());
}

References calculate_grid_convergence(), kSouth, and to_string().

Referenced by calculate_grid_convergence(), and to_detection_list_msg().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ calculate_grid_convergence() [2/2]

auto carma_cooperative_perception::calculate_grid_convergence	(	const Wgs84Coordinate &	position,
		std::string_view	georeference
	)		-> units::angle::degree_t

Definition at line 125 of file geodetic.cpp.

{
  gsl::owner<PJ_CONTEXT *> context = proj_context_create();
  proj_log_level(context, PJ_LOG_NONE);
 
  if (context == nullptr) {
    const std::string error_string{proj_errno_string(proj_context_errno(context))};
    throw std::invalid_argument("Could not create PROJ context: " + error_string + '.');
  }
 
  gsl::owner<PJ *> transform = proj_create(context, georeference.data());
 
  const auto factors = proj_factors(
    transform, proj_coord(
                 proj_torad(carma_cooperative_perception::remove_units(position.longitude)),
                 proj_torad(carma_cooperative_perception::remove_units(position.latitude)), 0, 0));
 
  if (proj_context_errno(context) != 0) {
    const std::string error_string{proj_errno_string(proj_context_errno(context))};
    throw std::invalid_argument("Could not calculate PROJ factors: " + error_string + '.');
  }
 
  proj_destroy(transform);
  proj_context_destroy(context);
 
  return units::angle::degree_t{proj_todeg(factors.meridian_convergence)};
}

References remove_units().

Here is the call graph for this function:

◆ calculate_utm_zone()

auto carma_cooperative_perception::calculate_utm_zone ( const Wgs84Coordinate & coordinate ) -> UtmZone

Get the UTM zone number from a WGS-84 coordinate.

Note: This function will not work for coordinates in the special UTM zones Svalbard and Norway.

Parameters

[in] coordinate WGS-84 coordinate

Returns: The UTM zone containing the coordinate

Definition at line 27 of file geodetic.cpp.

{
  // Note: std::floor prevents this function from being constexpr (until C++23)
 
  static constexpr std::size_t zone_width{6};
  static constexpr std::size_t max_zones{60};
 
  // Works for longitudes [-180, 360). Longitude of 360 will assign 61.
  const auto number{
    static_cast<std::size_t>(
      (std::floor(carma_cooperative_perception::remove_units(coordinate.longitude) + 180) /
       zone_width)) +
    1};
 
  UtmZone zone;
 
  // std::min is used to handle the "UTM Zone 61" case.
  zone.number = std::min(number, max_zones);
 
  if (coordinate.latitude < units::angle::degree_t{0.0}) {
    zone.hemisphere = Hemisphere::kSouth;
  } else {
    zone.hemisphere = Hemisphere::kNorth;
  }
 
  return zone;
}

References carma_cooperative_perception::UtmZone::hemisphere, kNorth, kSouth, carma_cooperative_perception::UtmZone::number, and remove_units().

Referenced by project_to_utm(), and transform_from_map_to_utm().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ convert_covariances()

void carma_cooperative_perception::convert_covariances	(	carma_cooperative_perception_interfaces::msg::Detection &	detection,
		const carma_v2x_msgs::msg::DetectedObjectCommonData &	common_data,
		const std::optional< SdsmToDetectionListConfig > &	conversion_adjustment
	)

Definition at line 382 of file msg_conversion.cpp.

{
  // Variables to store original covariance values for debugging
  double original_pose_covariance_x = 0.0;
  double original_pose_covariance_y = 0.0;
  double original_pose_covariance_z = 0.0;
  double original_pose_covariance_yaw = 0.0;
  double original_twist_covariance_x = 0.0;
  double original_twist_covariance_z = 0.0;
  double original_twist_covariance_yaw = 0.0;
 
  // Calculate and log original pose covariance values
  try {
    original_pose_covariance_x =
      0.5 * std::pow(j2735_v2x_msgs::to_double(common_data.pos_confidence.pos).value(), 2);
    original_pose_covariance_y = original_pose_covariance_x;
 
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sdsm_to_detection_list_node"),
      "Original pose covariance X/Y: " << original_pose_covariance_x);
  } catch (const std::bad_optional_access &) {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sdsm_to_detection_list_node"),
      "Missing position confidence");
  }
 
  try {
    original_pose_covariance_z =
      0.5 * std::pow(j2735_v2x_msgs::to_double(common_data.pos_confidence.elevation).value(), 2);
 
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sdsm_to_detection_list_node"),
      "Original pose covariance Z: " << original_pose_covariance_z);
  } catch (const std::bad_optional_access &) {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sdsm_to_detection_list_node"),
      "Missing elevation confidence");
  }
 
  try {
    // Get original heading/yaw covariance
    original_pose_covariance_yaw =
      0.5 * std::pow(j2735_v2x_msgs::to_double(common_data.heading_conf).value(), 2);
 
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sdsm_to_detection_list_node"),
      "Original pose covariance yaw: " << original_pose_covariance_yaw);
  } catch (const std::bad_optional_access &) {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sdsm_to_detection_list_node"),
      "Missing heading confidence");
  }
 
  try {
    // Get original linear x velocity covariance
    original_twist_covariance_x =
      0.5 * std::pow(j2735_v2x_msgs::to_double(common_data.speed_confidence).value(), 2);
 
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sdsm_to_detection_list_node"),
      "Original twist covariance X: " << original_twist_covariance_x);
  } catch (const std::bad_optional_access &) {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sdsm_to_detection_list_node"),
      "Missing speed confidence");
  }
 
  if (!common_data.speed_z.unavailable){
    try {
      // Get original linear z velocity covariance
      original_twist_covariance_z =
        0.5 * std::pow(j2735_v2x_msgs::to_double(common_data.speed_confidence_z).value(), 2);
 
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sdsm_to_detection_list_node"),
        "Original twist covariance Z: " << original_twist_covariance_z);
    } catch (const std::bad_optional_access &) {
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sdsm_to_detection_list_node"),
        "Missing z-speed confidence");
    }
  }
  else{
    original_twist_covariance_z = 0.0;
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sdsm_to_detection_list_node"),
      "Original twist covariance Z: 0.0 (speed_z not provided)");
  }
 
  // Having non-zero value means available
  if(static_cast<bool>(common_data.accel_4_way.yaw_rate)){
    try {
      // Get original angular z velocity (yaw rate) covariance
      original_twist_covariance_yaw =
        0.5 * std::pow(j2735_v2x_msgs::to_double(common_data.acc_cfd_yaw).value(), 2);
 
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sdsm_to_detection_list_node"),
        "Original twist covariance yaw: " << original_twist_covariance_yaw);
    } catch (const std::bad_optional_access &) {
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sdsm_to_detection_list_node"),
        "Missing yaw-rate confidence");
    }
  }
  else{
    original_twist_covariance_yaw = 0.0;
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sdsm_to_detection_list_node"),
      "Original twist covariance yaw: 0.0 (yaw_rate not provided)");
  }
 
  if (conversion_adjustment && conversion_adjustment.value().overwrite_covariance)
  {
    // Hardcoded pose covariance
    detection.pose.covariance[0] = conversion_adjustment.value().pose_covariance_x;
    detection.pose.covariance[7] = conversion_adjustment.value().pose_covariance_y;
    detection.pose.covariance[14] = conversion_adjustment.value().pose_covariance_z;
    detection.pose.covariance[35] = conversion_adjustment.value().pose_covariance_yaw;
 
    // Hardcoded twist covariance
    detection.twist.covariance[0] = conversion_adjustment.value().twist_covariance_x;
    detection.twist.covariance[14] = conversion_adjustment.value().twist_covariance_z;
    detection.twist.covariance[35] = conversion_adjustment.value().twist_covariance_yaw;
 
    // Print comparison between original and hardcoded values
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sdsm_to_detection_list_node"),
      "POSE COVARIANCE COMPARISON - Original vs Hardcoded: " <<
      "X: " << original_pose_covariance_x << " -> " << detection.pose.covariance[0] << ", " <<
      "Y: " << original_pose_covariance_y << " -> " << detection.pose.covariance[7] << ", " <<
      "Z: " << original_pose_covariance_z << " -> " << detection.pose.covariance[14] << ", " <<
      "Yaw: " << original_pose_covariance_yaw << " -> " << detection.pose.covariance[35]);
 
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sdsm_to_detection_list_node"),
      "TWIST COVARIANCE COMPARISON - Original vs Hardcoded: " <<
      "X: " << original_twist_covariance_x << " -> , " << detection.twist.covariance[0] << ", " <<
      "Z: " << original_twist_covariance_z << " -> , " << detection.twist.covariance[14] << ", " <<
      "Yaw: " << original_twist_covariance_yaw << " -> " << detection.twist.covariance[35]);
  }
  else
  {
    // Original pose covariance
    detection.pose.covariance[0] = original_pose_covariance_x;
    detection.pose.covariance[7] = original_pose_covariance_y;
    detection.pose.covariance[14] = original_pose_covariance_z;
    detection.pose.covariance[35] = original_pose_covariance_yaw;
 
    // Original twist covariance
    detection.twist.covariance[0] = original_twist_covariance_x;
    detection.twist.covariance[14] = original_twist_covariance_z;
    detection.twist.covariance[35] = original_twist_covariance_yaw;
  }
 
  // Fill zeros for all other twist covariance values
  for (size_t i = 0; i < 36; ++i) {
    if (i != 0 && i != 14 && i != 35) {
      detection.twist.covariance[i] = 0.0;
    }
  }
 
  // Fill zeros for all other pose covariance values
  for (size_t i = 0; i < 36; ++i) {
    if (i != 0 && i != 7 && i != 14 && i != 35) {
      detection.pose.covariance[i] = 0.0;
    }
  }
}

References process_bag::i.

Referenced by to_detection_list_msg().

Here is the caller graph for this function:

◆ convert_object_type()

void carma_cooperative_perception::convert_object_type	(	carma_cooperative_perception_interfaces::msg::Detection &	detection,
		const j3224_v2x_msgs::msg::ObjectType &	j3224_obj_type
	)

Definition at line 359 of file msg_conversion.cpp.

{
  switch (j3224_obj_type.object_type) {
    case j3224_obj_type.ANIMAL:
      detection.motion_model = detection.MOTION_MODEL_CTRV;
      // We don't have a good semantic class mapping for animals
      detection.semantic_class = detection.SEMANTIC_CLASS_UNKNOWN;
      break;
    case j3224_obj_type.VRU:
      detection.motion_model = detection.MOTION_MODEL_CTRV;
      detection.semantic_class = detection.SEMANTIC_CLASS_PEDESTRIAN;
      break;
    case j3224_obj_type.VEHICLE:
      detection.motion_model = detection.MOTION_MODEL_CTRV;
      detection.semantic_class = detection.SEMANTIC_CLASS_SMALL_VEHICLE;
      break;
    default:
      detection.motion_model = detection.MOTION_MODEL_CTRV;
      detection.semantic_class = detection.SEMANTIC_CLASS_UNKNOWN;
  }
}

Referenced by to_detection_list_msg().

Here is the caller graph for this function:

◆ enu_orientation_to_true_heading()

auto carma_cooperative_perception::enu_orientation_to_true_heading	(	double	yaw,
		const lanelet::BasicPoint3d &	obj_pose,
		const std::shared_ptr< lanelet::projection::LocalFrameProjector > &	map_projection
	)		-> units::angle::degree_t

Definition at line 278 of file msg_conversion.cpp.

{
  // Get object geodetic position
  lanelet::GPSPoint wgs_obj_pose = map_projection->reverse(obj_pose);
 
  // Get WGS84 Heading
  gsl::owner<PJ_CONTEXT *> context = proj_context_create();
  gsl::owner<PJ *> transform = proj_create(context, map_projection->ECEF_PROJ_STR);
  units::angle::degree_t grid_heading{std::fmod(90 - yaw + 360, 360)};
 
  const auto factors = proj_factors(
    transform, proj_coord(proj_torad(wgs_obj_pose.lon), proj_torad(wgs_obj_pose.lat), 0, 0));
  units::angle::degree_t grid_convergence{proj_todeg(factors.meridian_convergence)};
 
  auto wgs_heading = grid_convergence + grid_heading;
 
  proj_destroy(transform);
  proj_context_destroy(context);
 
  return wgs_heading;
}

Referenced by to_detected_object_data_msg().

Here is the caller graph for this function:

◆ euclidean_distance_squared()

auto carma_cooperative_perception::euclidean_distance_squared	(	const geometry_msgs::msg::Pose &	a,
		const geometry_msgs::msg::Pose &	b
	)		-> double

Definition at line 189 of file host_vehicle_filter_component.cpp.

{
  return std::pow(a.position.x - b.position.x, 2) + std::pow(a.position.y - b.position.y, 2) +
         std::pow(a.position.z - b.position.z, 2);
}

Referenced by carma_cooperative_perception::HostVehicleFilterNode::attempt_filter_and_republish().

Here is the caller graph for this function:

◆ heading_to_enu_yaw()

auto carma_cooperative_perception::heading_to_enu_yaw ( const units::angle::degree_t & heading ) -> units::angle::degree_t

Definition at line 273 of file msg_conversion.cpp.

{
  return units::angle::degree_t{std::fmod(-(remove_units(heading) - 90.0) + 360.0, 360.0)};
}

References remove_units().

Referenced by to_detection_list_msg().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ make_ctra_detection()

auto carma_cooperative_perception::make_ctra_detection ( const carma_cooperative_perception_interfaces::msg::Detection & msg ) -> Detection

Definition at line 115 of file multiple_object_tracker_component.cpp.

{
  const auto timestamp{
    units::time::second_t{static_cast<double>(msg.header.stamp.sec)} +
    units::time::nanosecond_t{static_cast<double>(msg.header.stamp.nanosec)}};
 
  tf2::Quaternion orientation;
  orientation.setX(msg.pose.pose.orientation.x);
  orientation.setY(msg.pose.pose.orientation.y);
  orientation.setZ(msg.pose.pose.orientation.z);
  orientation.setW(msg.pose.pose.orientation.w);
 
  double roll{0.0};
  double pitch{0.0};
  double yaw{0.0};
 
  tf2::Matrix3x3 matrix{orientation};
  matrix.getRPY(roll, pitch, yaw);
 
  const mot::CtraState state{
    units::length::meter_t{msg.pose.pose.position.x},
    units::length::meter_t{msg.pose.pose.position.y},
    units::velocity::meters_per_second_t{msg.twist.twist.linear.x},
    mot::Angle{units::angle::radian_t{yaw}},
    units::angular_velocity::radians_per_second_t{msg.twist.twist.angular.z},
    units::acceleration::meters_per_second_squared_t{msg.accel.accel.linear.x}};
 
  mot::CtraStateCovariance covariance = mot::CtraStateCovariance::Zero();
  covariance(0, 0) = msg.pose.covariance.at(0);
  covariance(1, 1) = msg.pose.covariance.at(7);
  covariance(2, 2) = msg.twist.covariance.at(0);
  covariance(3, 3) = msg.pose.covariance.at(35);
  covariance(4, 4) = msg.twist.covariance.at(35);
  covariance(5, 5) = msg.accel.covariance.at(0);
 
  return mot::CtraDetection{
    timestamp, state, covariance, mot::Uuid{msg.id}, make_semantic_class(msg.semantic_class)};
}

References make_semantic_class().

Referenced by make_detection().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ make_ctrv_detection()

auto carma_cooperative_perception::make_ctrv_detection ( const carma_cooperative_perception_interfaces::msg::Detection & msg ) -> Detection

Definition at line 77 of file multiple_object_tracker_component.cpp.

{
  const auto timestamp{
    units::time::second_t{static_cast<double>(msg.header.stamp.sec)} +
    units::time::nanosecond_t{static_cast<double>(msg.header.stamp.nanosec)}};
 
  tf2::Quaternion orientation;
  orientation.setX(msg.pose.pose.orientation.x);
  orientation.setY(msg.pose.pose.orientation.y);
  orientation.setZ(msg.pose.pose.orientation.z);
  orientation.setW(msg.pose.pose.orientation.w);
 
  double roll{0.0};
  double pitch{0.0};
  double yaw{0.0};
 
  tf2::Matrix3x3 matrix{orientation};
  matrix.getRPY(roll, pitch, yaw);
 
  const mot::CtrvState state{
    units::length::meter_t{msg.pose.pose.position.x},
    units::length::meter_t{msg.pose.pose.position.y},
    units::velocity::meters_per_second_t{msg.twist.twist.linear.x},
    mot::Angle{units::angle::radian_t{yaw}},
    units::angular_velocity::radians_per_second_t{msg.twist.twist.angular.z}};
 
  mot::CtrvStateCovariance covariance = mot::CtrvStateCovariance::Zero();
  covariance(0, 0) = msg.pose.covariance.at(0);
  covariance(1, 1) = msg.pose.covariance.at(7);
  covariance(2, 2) = msg.twist.covariance.at(0);
  covariance(3, 3) = msg.pose.covariance.at(35);
  covariance(4, 4) = msg.twist.covariance.at(35);
 
  return mot::CtrvDetection{
    timestamp, state, covariance, mot::Uuid{msg.id}, make_semantic_class(msg.semantic_class)};
}

References make_semantic_class().

Referenced by make_detection().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ make_detection()

auto carma_cooperative_perception::make_detection ( const carma_cooperative_perception_interfaces::msg::Detection & msg ) -> Detection

Definition at line 155 of file multiple_object_tracker_component.cpp.

{
  switch (msg.motion_model) {
    case msg.MOTION_MODEL_CTRV:
      return make_ctrv_detection(msg);
 
    case msg.MOTION_MODEL_CTRA:
      return make_ctra_detection(msg);
 
    case msg.MOTION_MODEL_CV:
      throw std::runtime_error("unsupported motion model type '3: constant velocity (CV)'");
  }
 
  throw std::runtime_error("unkown motion model type '" + std::to_string(msg.motion_model) + "'");
}

References make_ctra_detection(), make_ctrv_detection(), and to_string().

Referenced by carma_cooperative_perception::MultipleObjectTrackerNode::store_new_detections().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ make_semantic_class()

auto carma_cooperative_perception::make_semantic_class ( std::size_t numeric_value )

Definition at line 41 of file multiple_object_tracker_component.cpp.

{
  switch (numeric_value) {
    case 0:
      return mot::SemanticClass::kUnknown;
    case 1:
      return mot::SemanticClass::kSmallVehicle;
    case 2:
      return mot::SemanticClass::kLargeVehicle;
    case 3:
      return mot::SemanticClass::kPedestrian;
    case 4:
      return mot::SemanticClass::kMotorcycle;
  }
 
  return mot::SemanticClass::kUnknown;
}

Referenced by make_ctra_detection(), and make_ctrv_detection().

Here is the caller graph for this function:

◆ ned_to_enu()

auto carma_cooperative_perception::ned_to_enu ( const PositionOffsetXYZ & offset_ned )

noexcept

Definition at line 178 of file msg_conversion.cpp.

{
  auto offset_enu{offset_ned};
 
  // NED to ENU: swap x and y axis and negate z axis
  offset_enu.offset_x = offset_ned.offset_y;
  offset_enu.offset_y = offset_ned.offset_x;
 
  if (offset_enu.offset_z) {
    offset_enu.offset_z.value() *= -1;
  }
 
  return offset_enu;
}

Referenced by to_detection_list_msg().

Here is the caller graph for this function:

◆ operator!=()

constexpr auto carma_cooperative_perception::operator!=	(	const UtmZone &	lhs,
		const UtmZone &	rhs
	)		-> bool

inlineconstexpr

Definition at line 40 of file utm_zone.hpp.

{
  return !(lhs == rhs);
}

◆ operator+() [1/2]

constexpr auto carma_cooperative_perception::operator+	(	const UtmDisplacement &	displacement,
		UtmCoordinate	coordinate
	)		-> UtmCoordinate

inlineconstexpr

Addition operator overload.

Parameters

[in]	displacement	Displacement from coordinate
[in]	coordinate	Position represented in UTM coordinates

Returns: A new UtmCoordinate representing the new position

Definition at line 107 of file geodetic.hpp.

{
  return coordinate += displacement;
}

◆ operator+() [2/2]

constexpr auto carma_cooperative_perception::operator+	(	UtmCoordinate	coordinate,
		const UtmDisplacement &	displacement
	)		-> UtmCoordinate

inlineconstexpr

Addition operator overload.

Parameters

[in]	coordinate	Position represented in UTM coordinates
[in]	displacement	Displacement form coordinate

Returns: A new UtmCoordinate representing the new position

Definition at line 93 of file geodetic.hpp.

{
  return coordinate += displacement;
}

◆ operator+=()

constexpr auto carma_cooperative_perception::operator+=	(	UtmCoordinate &	coordinate,
		const UtmDisplacement &	displacement
	)		-> UtmCoordinate &

inlineconstexpr

Addition-assignment operator overload.

Parameters

[in]	coordinate	Position represented in UTM coordinates
[in]	displacement	Displacement from coordinate

Returns: Reference to the coordinate's updated position

Definition at line 75 of file geodetic.hpp.

{
  coordinate.easting += displacement.easting;
  coordinate.northing += displacement.northing;
  coordinate.elevation += displacement.elevation;
 
  return coordinate;
}

References carma_cooperative_perception::UtmCoordinate::easting.

◆ operator-() [1/2]

constexpr auto carma_cooperative_perception::operator-	(	const UtmDisplacement &	displacement,
		UtmCoordinate	coordinate
	)		-> UtmCoordinate

inlineconstexpr

Subtraction operator overload.

Parameters

[in]	displacement	Displacement from coordinate
[in]	coordinate	Position represented in UTM coordinates

Returns: A new UtmCoordinate representing the new position

Definition at line 153 of file geodetic.hpp.

{
  return coordinate -= displacement;
}

◆ operator-() [2/2]

constexpr auto carma_cooperative_perception::operator-	(	UtmCoordinate	coordinate,
		const UtmDisplacement &	displacement
	)		-> UtmCoordinate

inlineconstexpr

Subtraction operator overload.

Parameters

[in]	coordinate	Position represented in UTM coordinates
[in]	displacement	Displacement form coordinate

Returns: A new UtmCoordinate representing the new position

Definition at line 139 of file geodetic.hpp.

{
  return coordinate -= displacement;
}

◆ operator-=()

constexpr auto carma_cooperative_perception::operator-=	(	UtmCoordinate &	coordinate,
		const UtmDisplacement &	displacement
	)		-> UtmCoordinate &

inlineconstexpr

Subtraction-assignment operator overload.

Parameters

[in]	coordinate	Position represented in UTM coordinates
[in]	displacement	Displacement from coordinate

Returns: Reference to the coordinate's updated position

Definition at line 121 of file geodetic.hpp.

{
  coordinate.easting += displacement.easting;
  coordinate.northing += displacement.northing;
  coordinate.elevation += displacement.elevation;
 
  return coordinate;
}

References carma_cooperative_perception::UtmCoordinate::easting.

◆ operator==()

constexpr auto carma_cooperative_perception::operator==	(	const UtmZone &	lhs,
		const UtmZone &	rhs
	)		-> bool

inlineconstexpr

Definition at line 35 of file utm_zone.hpp.

{
  return lhs.number == rhs.number && lhs.hemisphere == rhs.hemisphere;
}

◆ predict_track_states()

static auto carma_cooperative_perception::predict_track_states	(	std::vector< Track >	tracks,
		units::time::second_t	end_time
	)

static

Definition at line 454 of file multiple_object_tracker_component.cpp.

{
  for (auto & track : tracks) {
    mot::propagate_to_time(track, end_time, mot::default_unscented_transform);
  }
 
  return tracks;
}

Referenced by carma_cooperative_perception::MultipleObjectTrackerNode::execute_pipeline().

Here is the caller graph for this function:

◆ project_to_carma_map()

auto carma_cooperative_perception::project_to_carma_map	(	const Wgs84Coordinate &	coordinate,
		std::string_view	proj_string
	)		-> MapCoordinate

Definition at line 55 of file geodetic.cpp.

{
  gsl::owner<PJ_CONTEXT *> context = proj_context_create();
  proj_log_level(context, PJ_LOG_NONE);
 
  if (!context) {
    const std::string error_string{proj_errno_string(proj_context_errno(context))};
    throw std::invalid_argument("Could not create PROJ context: " + error_string + '.');
  }
 
  gsl ::owner<PJ *> transformation =
    proj_create_crs_to_crs(context, "EPSG:4326", proj_string.data(), nullptr);
 
  if (!transformation) {
    const std::string error_string{proj_errno_string(proj_context_errno(context))};
    throw std::invalid_argument("Could not create PROJ transform: " + error_string + '.');
  }
 
  const auto coord_wgs84 = proj_coord(
    carma_cooperative_perception::remove_units(coordinate.latitude),
    carma_cooperative_perception::remove_units(coordinate.longitude), 0, 0);
  const auto coord_projected = proj_trans(transformation, PJ_FWD, coord_wgs84);
 
  proj_destroy(transformation);
  proj_context_destroy(context);
 
  return {
    units::length::meter_t{coord_projected.enu.e}, units::length::meter_t{coord_projected.enu.n},
    units::length::meter_t{coordinate.elevation}};
}

References remove_units().

Referenced by to_detection_list_msg().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ project_to_utm()

auto carma_cooperative_perception::project_to_utm ( const Wgs84Coordinate & coordinate ) -> UtmCoordinate

Projects a Wgs84Coordinate to its corresponding UTM zone.

Parameters

[in] coordinate Position represented in WGS-84 coordinates

Returns: Coordinate's position represented in UTM coordinates

Definition at line 87 of file geodetic.cpp.

{
  gsl::owner<PJ_CONTEXT *> context = proj_context_create();
  proj_log_level(context, PJ_LOG_NONE);
 
  if (context == nullptr) {
    const std::string error_string{proj_errno_string(proj_context_errno(context))};
    throw std::invalid_argument("Could not create PROJ context: " + error_string + '.');
  }
 
  const auto utm_zone{calculate_utm_zone(coordinate)};
  std::string proj_string{"+proj=utm +zone=" + std::to_string(utm_zone.number) + " +datum=WGS84"};
 
  if (utm_zone.hemisphere == Hemisphere::kSouth) {
    proj_string += " +south";
  }
 
  gsl ::owner<PJ *> utm_transformation =
    proj_create_crs_to_crs(context, "EPSG:4326", proj_string.c_str(), nullptr);
 
  if (utm_transformation == nullptr) {
    const std::string error_string{proj_errno_string(proj_context_errno(context))};
    throw std::invalid_argument("Could not create PROJ transform: " + error_string + '.');
  }
 
  auto coord_wgs84 = proj_coord(
    carma_cooperative_perception::remove_units(coordinate.latitude),
    carma_cooperative_perception::remove_units(coordinate.longitude), 0, 0);
  auto coord_utm = proj_trans(utm_transformation, PJ_FWD, coord_wgs84);
 
  proj_destroy(utm_transformation);
  proj_context_destroy(context);
 
  return {
    utm_zone, units::length::meter_t{coord_utm.enu.e}, units::length::meter_t{coord_utm.enu.n},
    units::length::meter_t{coordinate.elevation}};
}

References calculate_utm_zone(), kSouth, remove_units(), and to_string().

Referenced by transform_from_map_to_utm().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ remove_units()

template<typename T >

constexpr auto carma_cooperative_perception::remove_units ( const T & value )

constexpr

Definition at line 25 of file units_extensions.hpp.

{
  return units::unit_cast<typename T::underlying_type>(value);
}

Referenced by calculate_grid_convergence(), calculate_utm_zone(), carma_cooperative_perception::MultipleObjectTrackerNode::handle_on_activate(), heading_to_enu_yaw(), carma_cooperative_perception::MetricSe2::operator()(), project_to_carma_map(), project_to_utm(), to_detected_object_data_msg(), to_detection_list_msg(), to_position_msg(), to_ros_msg(), to_time_msg(), transform_from_map_to_utm(), and carma_cooperative_perception::SemanticDistance2dScore::two_dimensional_distance().

Here is the caller graph for this function:

◆ semantic_class_to_numeric_value()

auto carma_cooperative_perception::semantic_class_to_numeric_value ( mot::SemanticClass semantic_class )

Definition at line 59 of file multiple_object_tracker_component.cpp.

{
  switch (semantic_class) {
    case mot::SemanticClass::kUnknown:
      return 0;
    case mot::SemanticClass::kSmallVehicle:
      return 1;
    case mot::SemanticClass::kLargeVehicle:
      return 2;
    case mot::SemanticClass::kPedestrian:
      return 3;
    case mot::SemanticClass::kMotorcycle:
      return 4;
  }
 
  return 0;
}

Referenced by to_ros_msg().

Here is the caller graph for this function:

◆ temporally_align_detections()

static auto carma_cooperative_perception::temporally_align_detections	(	std::vector< Detection > &	detections,
		units::time::second_t	end_time
	)		-> void

static

Definition at line 446 of file multiple_object_tracker_component.cpp.

{
  for (auto & detection : detections) {
    mot::propagate_to_time(detection, end_time, mot::default_unscented_transform);
  }
}

Referenced by carma_cooperative_perception::MultipleObjectTrackerNode::execute_pipeline().

Here is the caller graph for this function:

◆ to_ddate_time_msg()

auto carma_cooperative_perception::to_ddate_time_msg ( const builtin_interfaces::msg::Time & builtin_time ) -> j2735_v2x_msgs::msg::DDateTime

Definition at line 193 of file msg_conversion.cpp.

{
  j2735_v2x_msgs::msg::DDateTime ddate_time_output;
 
  // Add the time components from epoch seconds
  boost::posix_time::ptime posix_time = boost::posix_time::from_time_t(builtin_time.sec) +
                                        boost::posix_time::nanosec(builtin_time.nanosec);
 
  const auto time_stamp_year = posix_time.date().year();
  const auto time_stamp_month = posix_time.date().month();
  const auto time_stamp_day = posix_time.date().day();
 
  const auto hours_of_day = posix_time.time_of_day().hours();
  const auto minutes_of_hour = posix_time.time_of_day().minutes();
  const auto seconds_of_minute = posix_time.time_of_day().seconds();
 
  ddate_time_output.presence_vector = 0;
 
  ddate_time_output.presence_vector |= j2735_v2x_msgs::msg::DDateTime::YEAR;
  ddate_time_output.year.year = time_stamp_year;
  ddate_time_output.presence_vector |= j2735_v2x_msgs::msg::DDateTime::MONTH;
  ddate_time_output.month.month = time_stamp_month;
  ddate_time_output.presence_vector |= j2735_v2x_msgs::msg::DDateTime::DAY;
  ddate_time_output.day.day = time_stamp_day;
  ddate_time_output.presence_vector |= j2735_v2x_msgs::msg::DDateTime::HOUR;
  ddate_time_output.hour.hour = hours_of_day;
  ddate_time_output.presence_vector |= j2735_v2x_msgs::msg::DDateTime::MINUTE;
  ddate_time_output.minute.minute = minutes_of_hour;
  ddate_time_output.presence_vector |= j2735_v2x_msgs::msg::DDateTime::SECOND;
  ddate_time_output.second.millisecond = seconds_of_minute;
 
  return ddate_time_output;
}

Referenced by to_sdsm_msg().

Here is the caller graph for this function:

◆ to_detected_object_data_msg()

auto carma_cooperative_perception::to_detected_object_data_msg	(	const carma_perception_msgs::msg::ExternalObject &	external_object,
		const std::shared_ptr< lanelet::projection::LocalFrameProjector > &	map_projection
	)		-> carma_v2x_msgs::msg::DetectedObjectData

Definition at line 879 of file msg_conversion.cpp.

{
  carma_v2x_msgs::msg::DetectedObjectData detected_object_data;
  detected_object_data.presence_vector = 0;
 
  carma_v2x_msgs::msg::DetectedObjectCommonData detected_object_common_data;
  detected_object_common_data.presence_vector = 0;
 
  // common data //////////
 
  // obj_type_conf - convert from percentile, cast to proper uint type
  if (external_object.presence_vector & external_object.OBJECT_TYPE_PRESENCE_VECTOR) {
    detected_object_common_data.obj_type_cfd.classification_confidence =
      static_cast<std::uint8_t>(external_object.confidence * 100);
  }
 
  // detected_id - cast proper type
  if (external_object.presence_vector & external_object.ID_PRESENCE_VECTOR) {
    detected_object_common_data.detected_id.object_id =
      static_cast<std::uint16_t>(external_object.id);
  }
 
  // pos - Add offset to ref_pos to get object position
  // in map frame -> convert to WGS84 coordinates for sdsm
 
  // To get offset: Subtract the external object pose from
  // the current vehicle location given by the current_pose topic
  if (external_object.presence_vector & external_object.POSE_PRESENCE_VECTOR) {
    detected_object_common_data.pos.offset_x.object_distance =
      static_cast<float>(external_object.pose.pose.position.x);
    detected_object_common_data.pos.offset_y.object_distance =
      static_cast<float>(external_object.pose.pose.position.y);
    detected_object_common_data.pos.offset_z.object_distance =
      static_cast<float>(external_object.pose.pose.position.z);
  }
 
  // speed/speed_z - convert vector velocity to scalar speed val given x/y components
  if (external_object.presence_vector & external_object.VELOCITY_PRESENCE_VECTOR) {
    detected_object_common_data.speed.speed =
      std::hypot(external_object.velocity.twist.linear.x, external_object.velocity.twist.linear.y);
 
    detected_object_common_data.presence_vector |=
      carma_v2x_msgs::msg::DetectedObjectCommonData::HAS_SPEED_Z;
    detected_object_common_data.speed_z.speed = external_object.velocity.twist.linear.z;
 
    // heading - convert ang vel to scale heading
    lanelet::BasicPoint3d external_object_position{
      external_object.pose.pose.position.x, external_object.pose.pose.position.y,
      external_object.pose.pose.position.z};
    // Get yaw from orientation
    auto obj_orientation = external_object.pose.pose.orientation;
    tf2::Quaternion q(obj_orientation.x, obj_orientation.y, obj_orientation.z, obj_orientation.w);
    tf2::Matrix3x3 m(q);
    double roll, pitch, yaw;
    m.getRPY(roll, pitch, yaw);
 
    detected_object_common_data.heading.heading =
      remove_units(enu_orientation_to_true_heading(yaw, external_object_position, map_projection));
  }
 
  // optional data (determine based on object type)
  // use object type struct for better control
  carma_v2x_msgs::msg::DetectedObjectOptionalData detected_object_optional_data;
 
  switch (external_object.object_type) {
    case external_object.SMALL_VEHICLE:
      detected_object_common_data.obj_type.object_type = j3224_v2x_msgs::msg::ObjectType::VEHICLE;
 
      if (external_object.presence_vector & external_object.SIZE_PRESENCE_VECTOR) {
        detected_object_optional_data.det_veh.presence_vector =
          carma_v2x_msgs::msg::DetectedVehicleData::HAS_SIZE;
        detected_object_optional_data.det_veh.presence_vector |=
          carma_v2x_msgs::msg::DetectedVehicleData::HAS_HEIGHT;
 
        detected_object_optional_data.det_veh.size.vehicle_width = external_object.size.y;
        detected_object_optional_data.det_veh.size.vehicle_length = external_object.size.x;
        detected_object_optional_data.det_veh.height.vehicle_height = external_object.size.z;
      }
      break;
    case external_object.LARGE_VEHICLE:
      detected_object_common_data.obj_type.object_type = j3224_v2x_msgs::msg::ObjectType::VEHICLE;
 
      if (external_object.presence_vector & external_object.SIZE_PRESENCE_VECTOR) {
        detected_object_optional_data.det_veh.presence_vector =
          carma_v2x_msgs::msg::DetectedVehicleData::HAS_SIZE;
        detected_object_optional_data.det_veh.presence_vector |=
          carma_v2x_msgs::msg::DetectedVehicleData::HAS_HEIGHT;
 
        detected_object_optional_data.det_veh.size.vehicle_width = external_object.size.y;
        detected_object_optional_data.det_veh.size.vehicle_length = external_object.size.x;
        detected_object_optional_data.det_veh.height.vehicle_height = external_object.size.z;
      }
      break;
    case external_object.MOTORCYCLE:
      detected_object_common_data.obj_type.object_type = j3224_v2x_msgs::msg::ObjectType::VEHICLE;
 
      if (external_object.presence_vector & external_object.SIZE_PRESENCE_VECTOR) {
        detected_object_optional_data.det_veh.presence_vector =
          carma_v2x_msgs::msg::DetectedVehicleData::HAS_SIZE;
        detected_object_optional_data.det_veh.presence_vector |=
          carma_v2x_msgs::msg::DetectedVehicleData::HAS_HEIGHT;
 
        detected_object_optional_data.det_veh.size.vehicle_width = external_object.size.y;
        detected_object_optional_data.det_veh.size.vehicle_length = external_object.size.x;
        detected_object_optional_data.det_veh.height.vehicle_height = external_object.size.z;
      }
      break;
    case external_object.PEDESTRIAN:
      detected_object_common_data.obj_type.object_type = j3224_v2x_msgs::msg::ObjectType::VRU;
 
      detected_object_optional_data.det_vru.presence_vector =
        carma_v2x_msgs::msg::DetectedVRUData::HAS_BASIC_TYPE;
      detected_object_optional_data.det_vru.basic_type.type |=
        j2735_v2x_msgs::msg::PersonalDeviceUserType::A_PEDESTRIAN;
 
      break;
    case external_object.UNKNOWN:
    default:
      detected_object_common_data.obj_type.object_type = j3224_v2x_msgs::msg::ObjectType::UNKNOWN;
 
      if (external_object.presence_vector & external_object.SIZE_PRESENCE_VECTOR) {
        detected_object_optional_data.det_obst.obst_size.width.size_value = external_object.size.y;
        detected_object_optional_data.det_obst.obst_size.length.size_value = external_object.size.x;
 
        detected_object_optional_data.det_obst.obst_size.presence_vector =
          carma_v2x_msgs::msg::ObstacleSize::HAS_HEIGHT;
        detected_object_optional_data.det_obst.obst_size.height.size_value = external_object.size.z;
      }
  }
 
  detected_object_data.detected_object_common_data = std::move(detected_object_common_data);
  detected_object_data.detected_object_optional_data = std::move(detected_object_optional_data);
 
  return detected_object_data;
}

References enu_orientation_to_true_heading(), and remove_units().

Referenced by to_sdsm_msg().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ to_detection_list_msg() [1/2]

auto carma_cooperative_perception::to_detection_list_msg	(	const carma_perception_msgs::msg::ExternalObjectList &	object_list,
		const MotionModelMapping &	motion_model_mapping
	)		-> carma_cooperative_perception_interfaces::msg::DetectionList

Definition at line 738 of file msg_conversion.cpp.

{
  carma_cooperative_perception_interfaces::msg::DetectionList detection_list;
 
  std::transform(
    std::cbegin(object_list.objects), std::cend(object_list.objects),
    std::back_inserter(detection_list.detections),
    [&motion_model_mapping = std::as_const(motion_model_mapping)](const auto & object) {
      return to_detection_msg(object, motion_model_mapping);
    });
 
  return detection_list;
}

◆ to_detection_list_msg() [2/2]

auto carma_cooperative_perception::to_detection_list_msg	(	const carma_v2x_msgs::msg::SensorDataSharingMessage &	sdsm,
		std::string_view	georeference,
		bool	is_simulation,
		const std::optional< SdsmToDetectionListConfig > &	conversion_adjustment
	)		-> carma_cooperative_perception_interfaces::msg::DetectionList

Converts a carma_v2x_msgs::msg::SensorDataSharingMessage (SDSM) to carma_cooperative_perception_interfaces::msg::DetectionList format.

This function transforms data from the V2X SDSM format into the CARMA cooperative perception DetectionList format, handling the necessary coordinate transformations.

Important coordinate system transformations:

SDSM uses NED (North-East-Down) coordinate system for position offsets
SDSM heading is measured clockwise from true north (0° at north, 90° at east)
Output DetectionList uses ENU (East-North-Up) coordinate system
Output heading is converted to ENU yaw (0° at east, 90° at north)

The function performs the following key operations:

Projects reference position from WGS84 to the local map frame
Converts NED position offsets to ENU
Handles heading conversion from true north to map grid
Transforms detection confidence values to covariance values
Maps object types to appropriate semantic classes

Parameters

sdsm	The input J3224 SDSM message containing detected objects
georeference	String containing the georeference information for coordinate projection
is_simulation	Boolean flag indicating if running in simulation mode (affects timestamps)
conversion_adjustment	Optional configuration for position and covariance adjustments

Returns: carma_cooperative_perception_interfaces::msg::DetectionList message containing the transformed detections in CARMA Platform format

Definition at line 566 of file msg_conversion.cpp.

{
  carma_cooperative_perception_interfaces::msg::DetectionList detection_list;
  try{
  
    const auto ref_pos_3d{Position3D::from_msg(sdsm.ref_pos)};
 
    units::length::meter_t elevation(0.0);
    if(ref_pos_3d.elevation){
      elevation = ref_pos_3d.elevation.value();
    }
    const Wgs84Coordinate ref_pos_wgs84{
      ref_pos_3d.latitude, ref_pos_3d.longitude, elevation};
 
    const auto ref_pos_map{project_to_carma_map(ref_pos_wgs84, georeference)};
 
    for (const auto & object_data : sdsm.objects.detected_object_data) {
      const auto common_data{object_data.detected_object_common_data};
 
      carma_cooperative_perception_interfaces::msg::Detection detection;
      detection.header.frame_id = "map";
 
      const auto detection_time{calc_detection_time_stamp(
        DDateTime::from_msg(sdsm.sdsm_time_stamp),
        MeasurementTimeOffset::from_msg(common_data.measurement_time))};
 
      detection.header.stamp = to_time_msg(detection_time, is_simulation);
 
      detection.id = fmt::format("{}-{}",
        carma_cooperative_perception::to_string(sdsm.source_id.id),
        common_data.detected_id.object_id);
 
      const auto pos_offset_enu{ned_to_enu(PositionOffsetXYZ::from_msg(common_data.pos))};
      detection.pose.pose.position = to_position_msg(MapCoordinate{
        ref_pos_map.easting + pos_offset_enu.offset_x, ref_pos_map.northing + pos_offset_enu.offset_y,
        ref_pos_map.elevation + pos_offset_enu.offset_z.value_or(units::length::meter_t{0.0})});
 
      // Adjust object's position to match vector map coordinates as sensor calibrations are not
      // always reliable
      if (conversion_adjustment && conversion_adjustment.value().adjust_pose)
      {
        detection.pose.pose.position.x += conversion_adjustment.value().x_offset;
        detection.pose.pose.position.y += conversion_adjustment.value().y_offset;
      }
 
      const auto true_heading{units::angle::degree_t{Heading::from_msg(common_data.heading).heading}};
 
      // Note: This should really use the detection's WGS-84 position, so the
      // convergence will be off slightly. TODO
      const units::angle::degree_t grid_convergence{
        calculate_grid_convergence(ref_pos_wgs84, georeference)};
 
      const auto grid_heading{true_heading - grid_convergence};
      const auto enu_yaw{heading_to_enu_yaw(grid_heading)};
 
      tf2::Quaternion quat_tf;
 
      if (conversion_adjustment && conversion_adjustment.value().adjust_pose)
      {
        // Adjust object's heading to match vector map coordinates as sensor calibrations are not
        // always reliable
        auto yaw_with_offset = units::angle::radian_t{enu_yaw} +
          units::angle::radian_t{units::angle::degree_t{conversion_adjustment.value().yaw_offset}};
        auto new_yaw = std::fmod(remove_units(yaw_with_offset) + 2 * M_PI, 2 * M_PI);
        quat_tf.setRPY(0, 0, new_yaw);
      }
      else
      {
        // No adjustment needed
        quat_tf.setRPY(0, 0, remove_units(units::angle::radian_t{enu_yaw}));
      }
 
      detection.pose.pose.orientation = tf2::toMsg(quat_tf);
 
      const auto speed{Speed::from_msg(common_data.speed)};
      detection.twist.twist.linear.x =
        remove_units(units::velocity::meters_per_second_t{speed.speed});
 
      if (!common_data.speed_z.unavailable){
        const auto speed_z{Speed::from_msg(common_data.speed_z)};
        detection.twist.twist.linear.z =
          remove_units(units::velocity::meters_per_second_t{speed_z.speed});
      }
      else{
        detection.twist.twist.linear.z = remove_units(units::velocity::meters_per_second_t{0.0});
      }
 
      // NOTE: common_data.accel_4_way.longitudinal, lateral, vert not supported
      // and not needed at the moment for multiple object tracking algorithm
      // Having non-zero yaw_rate value means available
      if(static_cast<bool>(common_data.accel_4_way.yaw_rate)){
        const auto accel_set{AccelerationSet4Way::from_msg(common_data.accel_4_way)};
        detection.twist.twist.angular.z =
          remove_units(units::angular_velocity::degrees_per_second_t{accel_set.yaw_rate});
      }
      else{
        detection.twist.twist.angular.z = 0.0;
      }
 
      convert_covariances(detection, common_data, conversion_adjustment);
 
      convert_object_type(detection, common_data.obj_type);
 
      detection_list.detections.push_back(std::move(detection));
    }
  }
  catch (...) {
    RCLCPP_ERROR_STREAM(rclcpp::get_logger("sdsm_to_detection_list_node"), "Error converting SDSM to object, ignoring sdsm message.");
  }
  
  return detection_list;
}

Referenced by carma_cooperative_perception::ExternalObjectListToDetectionListNode::publish_as_detection_list(), and carma_cooperative_perception::SdsmToDetectionListNode::sdsm_msg_callback().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ to_detection_msg()

auto carma_cooperative_perception::to_detection_msg	(	const carma_perception_msgs::msg::ExternalObject &	object,
		const MotionModelMapping &	motion_model_mapping
	)		-> carma_cooperative_perception_interfaces::msg::Detection

Definition at line 682 of file msg_conversion.cpp.

{
  carma_cooperative_perception_interfaces::msg::Detection detection;
 
  detection.header = object.header;
 
  if (object.presence_vector & object.BSM_ID_PRESENCE_VECTOR) {
    detection.id = "";
    std::transform(
      std::cbegin(object.bsm_id), std::cend(object.bsm_id), std::back_inserter(detection.id),
      [](const auto & i) { return i + '0'; });
  }
 
  if (object.presence_vector & object.ID_PRESENCE_VECTOR) {
    detection.id += '-' + std::to_string(object.id);
  }
 
  if (object.presence_vector & object.POSE_PRESENCE_VECTOR) {
    detection.pose = object.pose;
  }
 
  if (object.presence_vector & object.VELOCITY_PRESENCE_VECTOR) {
    detection.twist = object.velocity;
  }
 
  if (object.presence_vector & object.OBJECT_TYPE_PRESENCE_VECTOR) {
    switch (object.object_type) {
      case object.SMALL_VEHICLE:
        detection.motion_model = motion_model_mapping.small_vehicle_model;
        detection.semantic_class = detection.SEMANTIC_CLASS_SMALL_VEHICLE;
        break;
      case object.LARGE_VEHICLE:
        detection.motion_model = motion_model_mapping.large_vehicle_model;
        detection.semantic_class = detection.SEMANTIC_CLASS_LARGE_VEHICLE;
        break;
      case object.MOTORCYCLE:
        detection.motion_model = motion_model_mapping.motorcycle_model;
        detection.semantic_class = detection.SEMANTIC_CLASS_MOTORCYCLE;
        break;
      case object.PEDESTRIAN:
        detection.motion_model = motion_model_mapping.pedestrian_model;
        detection.semantic_class = detection.SEMANTIC_CLASS_PEDESTRIAN;
        break;
      case object.UNKNOWN:
      default:
        detection.motion_model = motion_model_mapping.unknown_model;
        detection.semantic_class = detection.SEMANTIC_CLASS_UNKNOWN;
    }
  }
 
  return detection;
}

References process_bag::i, and to_string().

Here is the call graph for this function:

◆ to_external_object_list_msg()

auto carma_cooperative_perception::to_external_object_list_msg ( const carma_cooperative_perception_interfaces::msg::TrackList & track_list ) -> carma_perception_msgs::msg::ExternalObjectList

Definition at line 826 of file msg_conversion.cpp.

{
  carma_perception_msgs::msg::ExternalObjectList external_object_list;
 
  for (const auto & track : track_list.tracks) {
    external_object_list.objects.push_back(to_external_object_msg(track));
  }
 
  return external_object_list;
}

References to_external_object_msg().

Referenced by carma_cooperative_perception::TrackListToExternalObjectListNode::publish_as_external_object_list().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ to_external_object_msg()

auto carma_cooperative_perception::to_external_object_msg ( const carma_cooperative_perception_interfaces::msg::Track & track ) -> carma_perception_msgs::msg::ExternalObject

Definition at line 755 of file msg_conversion.cpp.

{
  carma_perception_msgs::msg::ExternalObject external_object;
  external_object.header = track.header;
  external_object.presence_vector = 0;
 
  const auto to_numeric_id = [](std::string string_id) -> std::optional<uint32_t> {
    auto non_digit_start = std::remove_if(
      std::begin(string_id), std::end(string_id),
      [](const auto & ch) { return !std::isdigit(ch); });
 
    std::uint32_t numeric_id;
    const auto digit_substr_size{std::distance(std::begin(string_id), non_digit_start)};
    if (
      std::from_chars(string_id.c_str(), string_id.c_str() + digit_substr_size, numeric_id).ec ==
      std::errc{}) {
      return numeric_id;
    }
 
    return std::nullopt;
  };
 
  if (const auto numeric_id{to_numeric_id(track.id)}) {
    external_object.presence_vector |= external_object.ID_PRESENCE_VECTOR;
    external_object.id = numeric_id.value();
  } else {
    external_object.presence_vector &= ~external_object.ID_PRESENCE_VECTOR;
  }
 
  external_object.presence_vector |= external_object.POSE_PRESENCE_VECTOR;
  external_object.pose = track.pose;
 
  external_object.presence_vector |= external_object.VELOCITY_PRESENCE_VECTOR;
 
  const auto track_longitudinal_velocity{track.twist.twist.linear.x};
  const auto track_orientation = track.pose.pose.orientation;
 
  tf2::Quaternion q(
    track_orientation.x, track_orientation.y, track_orientation.z, track_orientation.w);
  tf2::Matrix3x3 m(q);
  double roll, pitch, yaw;
  m.getRPY(roll, pitch, yaw);
 
  external_object.velocity.twist.linear.x = track_longitudinal_velocity * std::cos(yaw);
  external_object.velocity.twist.linear.y = track_longitudinal_velocity * std::sin(yaw);
 
  external_object.object_type = track.semantic_class;
 
  external_object.presence_vector |= external_object.OBJECT_TYPE_PRESENCE_VECTOR;
  switch (track.semantic_class) {
    case track.SEMANTIC_CLASS_SMALL_VEHICLE:
      external_object.object_type = external_object.SMALL_VEHICLE;
      break;
    case track.SEMANTIC_CLASS_LARGE_VEHICLE:
      external_object.object_type = external_object.LARGE_VEHICLE;
      break;
    case track.SEMANTIC_CLASS_MOTORCYCLE:
      external_object.object_type = external_object.MOTORCYCLE;
      break;
    case track.SEMANTIC_CLASS_PEDESTRIAN:
      external_object.object_type = external_object.PEDESTRIAN;
      break;
    case track.SEMANTIC_CLASS_UNKNOWN:
    default:
      external_object.object_type = external_object.UNKNOWN;
  }
 
  return external_object;
}

Referenced by to_external_object_list_msg().

Here is the caller graph for this function:

◆ to_position_msg() [1/2]

auto carma_cooperative_perception::to_position_msg ( const MapCoordinate & position_map ) -> geometry_msgs::msg::Point

Definition at line 262 of file msg_conversion.cpp.

{
  geometry_msgs::msg::Point msg;
 
  msg.x = remove_units(position_map.easting);
  msg.y = remove_units(position_map.northing);
  msg.z = remove_units(position_map.elevation);
 
  return msg;
}

References remove_units().

Here is the call graph for this function:

◆ to_position_msg() [2/2]

auto carma_cooperative_perception::to_position_msg ( const UtmCoordinate & position_utm ) -> geometry_msgs::msg::Point

Definition at line 251 of file msg_conversion.cpp.

{
  geometry_msgs::msg::Point msg;
 
  msg.x = remove_units(position_utm.easting);
  msg.y = remove_units(position_utm.northing);
  msg.z = remove_units(position_utm.elevation);
 
  return msg;
}

References remove_units().

Referenced by to_detection_list_msg().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ to_ros_msg() [1/3]

static auto carma_cooperative_perception::to_ros_msg ( const mot::CtraTrack & track )

static

Definition at line 172 of file multiple_object_tracker_component.cpp.

{
  carma_cooperative_perception_interfaces::msg::Track msg;
 
  msg.header.stamp.sec = mot::remove_units(units::math::floor(track.timestamp));
  msg.header.stamp.nanosec = mot::remove_units(
    units::time::nanosecond_t{units::math::fmod(track.timestamp, units::time::second_t{1.0})});
  msg.header.frame_id = "map";
 
  msg.id = track.uuid.value();
  msg.motion_model = msg.MOTION_MODEL_CTRA;
  msg.pose.pose.position.x = mot::remove_units(track.state.position_x);
  msg.pose.pose.position.y = mot::remove_units(track.state.position_y);
 
  tf2::Quaternion orientation;
  orientation.setRPY(0, 0, mot::remove_units(track.state.yaw.get_angle()));
  msg.pose.pose.orientation.x = orientation.getX();
  msg.pose.pose.orientation.y = orientation.getY();
  msg.pose.pose.orientation.z = orientation.getZ();
  msg.pose.pose.orientation.w = orientation.getW();
 
  msg.twist.twist.linear.x = mot::remove_units(track.state.velocity);
  msg.twist.twist.angular.z = mot::remove_units(track.state.yaw_rate);
 
  msg.accel.accel.linear.x = mot::remove_units(track.state.acceleration);
 
  msg.semantic_class = semantic_class_to_numeric_value(mot::get_semantic_class(track));
 
  return msg;
}

References remove_units(), and semantic_class_to_numeric_value().

Referenced by to_ros_msg().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ to_ros_msg() [2/3]

static auto carma_cooperative_perception::to_ros_msg ( const mot::CtrvTrack & track )

static

Definition at line 203 of file multiple_object_tracker_component.cpp.

{
  carma_cooperative_perception_interfaces::msg::Track msg;
 
  msg.header.stamp.sec = mot::remove_units(units::math::floor(track.timestamp));
  msg.header.stamp.nanosec = mot::remove_units(
    units::time::nanosecond_t{units::math::fmod(track.timestamp, units::time::second_t{1.0})});
  msg.header.frame_id = "map";
 
  msg.id = track.uuid.value();
  msg.motion_model = msg.MOTION_MODEL_CTRV;
  msg.pose.pose.position.x = mot::remove_units(track.state.position_x);
  msg.pose.pose.position.y = mot::remove_units(track.state.position_y);
 
  tf2::Quaternion orientation;
  orientation.setRPY(0, 0, mot::remove_units(track.state.yaw.get_angle()));
  msg.pose.pose.orientation.x = orientation.getX();
  msg.pose.pose.orientation.y = orientation.getY();
  msg.pose.pose.orientation.z = orientation.getZ();
  msg.pose.pose.orientation.w = orientation.getW();
 
  msg.twist.twist.linear.x = mot::remove_units(track.state.velocity);
  msg.twist.twist.angular.z = mot::remove_units(track.state.yaw_rate);
 
  msg.semantic_class = semantic_class_to_numeric_value(mot::get_semantic_class(track));
 
  return msg;
}

References remove_units(), and semantic_class_to_numeric_value().

Here is the call graph for this function:

◆ to_ros_msg() [3/3]

static auto carma_cooperative_perception::to_ros_msg ( const Track & track )

static

Definition at line 232 of file multiple_object_tracker_component.cpp.

{
  static constexpr mot::Visitor visitor{
    [](const mot::CtrvTrack & t) { return to_ros_msg(t); },
    [](const mot::CtraTrack & t) { return to_ros_msg(t); },
    [](const auto &) {
      // Currently on support CTRV and CTRA
      throw std::runtime_error{"cannot make ROS 2 message from track type"};
    }};
 
  return std::visit(visitor, track);
}

References to_ros_msg().

Referenced by carma_cooperative_perception::MultipleObjectTrackerNode::execute_pipeline().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ to_sdsm_msg()

auto carma_cooperative_perception::to_sdsm_msg	(	const carma_perception_msgs::msg::ExternalObjectList &	external_object_list,
		const geometry_msgs::msg::PoseStamped &	current_pose,
		const std::shared_ptr< lanelet::projection::LocalFrameProjector > &	map_projection
	)		-> carma_v2x_msgs::msg::SensorDataSharingMessage

Definition at line 839 of file msg_conversion.cpp.

{
  carma_v2x_msgs::msg::SensorDataSharingMessage sdsm;
  carma_v2x_msgs::msg::DetectedObjectList detected_object_list;
 
  sdsm.sdsm_time_stamp = to_ddate_time_msg(external_object_list.header.stamp);
 
  sdsm.ref_pos = transform_pose_from_map_to_wgs84(current_pose, map_projection);
 
  // Convert external objects within the list to detected_object_data
  for (const auto & external_object : external_object_list.objects) {
    auto sdsm_detected_object = to_detected_object_data_msg(external_object, map_projection);
 
    // Calculate the time offset between individual objects and the respective SDSM container msg
    sdsm_detected_object.detected_object_common_data.measurement_time =
      calc_sdsm_time_offset(external_object.header.stamp, external_object.header.stamp);
 
    // Calculate the position offset from the current reference pose (in m)
    sdsm_detected_object.detected_object_common_data.pos =
      calc_relative_position(current_pose, sdsm_detected_object.detected_object_common_data.pos);
 
    detected_object_list.detected_object_data.push_back(sdsm_detected_object);
  }
 
  std::vector<uint8_t> id = {0x00, 0x00, 0x00, 0x01};
  sdsm.source_id.id = id;
  sdsm.equipment_type.equipment_type = j3224_v2x_msgs::msg::EquipmentType::OBU;
  sdsm.ref_pos_xy_conf.semi_major = j2735_v2x_msgs::msg::PositionalAccuracy::ACCURACY_UNAVAILABLE;
  sdsm.ref_pos_xy_conf.semi_minor = j2735_v2x_msgs::msg::PositionalAccuracy::ACCURACY_UNAVAILABLE;
  sdsm.ref_pos_xy_conf.orientation =
    j2735_v2x_msgs::msg::PositionalAccuracy::ACCURACY_ORIENTATION_UNAVAILABLE;
  sdsm.objects = detected_object_list;
 
  return sdsm;
}

References calc_relative_position(), calc_sdsm_time_offset(), to_ddate_time_msg(), to_detected_object_data_msg(), and transform_pose_from_map_to_wgs84().

Referenced by carma_cooperative_perception::ExternalObjectListToSdsmNode::publish_as_sdsm().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ to_string() [1/2]

std::string carma_cooperative_perception::to_string ( const std::vector< std::uint8_t > & temporary_id )

Definition at line 344 of file msg_conversion.cpp.

                                                                {
  std::string str;
  str.reserve(2 * std::size(temporary_id));  // Two hex characters per octet string
 
  std::array<char, 2> buffer;
  for (const auto & octet_string : temporary_id) {
    std::to_chars(std::begin(buffer), std::end(buffer), octet_string, 16);
    str.push_back(std::toupper(std::get<0>(buffer)));
    str.push_back(std::toupper(std::get<1>(buffer)));
  }
 
  return str;
};

References create_two_lane_map::str.

Referenced by to_detection_msg().

Here is the caller graph for this function:

◆ to_string() [2/2]

auto carma_cooperative_perception::to_string ( const UtmZone & zone ) -> std::string

Definition at line 21 of file utm_zone.cpp.

{
  if (zone.hemisphere == Hemisphere::kNorth) {
    return std::to_string(zone.number) + "N";
  }
 
  return std::to_string(zone.number) + "S";
}

References kNorth, and to_string().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ to_time_msg()

auto carma_cooperative_perception::to_time_msg	(	const DDateTime &	d_date_time,
		bool	is_simulation
	)		-> builtin_interfaces::msg::Time

Definition at line 61 of file msg_conversion.cpp.

{
  // Convert DDateTime to builtin_interfaces::msg::Time
  builtin_interfaces::msg::Time msg;
  if (!is_simulation) {
    // Create a tm structure to hold the date and time components
    std::tm timeinfo = {};
 
    // Year
    if (d_date_time.year){
      if(remove_units(d_date_time.year.value()) >= 1970){
        // std::tm is counted since 1900
        timeinfo.tm_year = static_cast<int>(remove_units(d_date_time.year.value())) - 1900;
      }
      else{
        throw std::invalid_argument(
          "Year must be greater than 1970 for live date/time conversion");
      }
    }
 
    // Month
    if (d_date_time.month && static_cast<int>(d_date_time.month.value().get_value()) != 0)
    {
      // std::tm is counted from 0 to 11, J2735 is counted from 1 to 12
      timeinfo.tm_mon = static_cast<int>(d_date_time.month.value().get_value()) - 1;
    }
 
    // Day
    if (d_date_time.day && static_cast<int>(d_date_time.day.value()) != 0)
    {
      // Day is counted from 1 to 31 in both std::tm and J2735
      timeinfo.tm_mday = static_cast<int>(d_date_time.day.value());
    }
    else{
      timeinfo.tm_mday = 1; // Default to 1 if day is not provided as C++ initializes to 0
    }
 
    // Hour
    if (d_date_time.hour && static_cast<int>(d_date_time.hour.value()) != 31)
    {
      // Hour is counted from 0 to 23 in both std::tm and J2735
      timeinfo.tm_hour = static_cast<int>(d_date_time.hour.value());
    }
 
    // Minute
    if (d_date_time.minute && static_cast<int>(d_date_time.minute.value()) != 60)
    {
      // Minute is counted from 0 to 59 in both std::tm and J2735
      timeinfo.tm_min = static_cast<int>(d_date_time.minute.value());
    }
    // Set seconds field (which actually uses ms in j2735) to 0
    // for now and add milliseconds later
    timeinfo.tm_sec = 0;
 
    std::time_t timeT; // Integer number of seconds since the Unix epoch
 
    // Treat the time as UTC to adjust the timezone offset later
    // which also works if no timezone offset is provided because SAE J3224 SDSM is in UTC
    timeinfo.tm_gmtoff = 0; // Set to 0 for UTC
    timeinfo.tm_isdst = 0; // UTC doesn't observe DST
    std::time_t utc_time = timegm(&timeinfo); // func is available on GNU-based systems (ubuntu etc)
 
    if (d_date_time.time_zone_offset) {
      // Offset is in minutes, convert to seconds
      // If offset is negative (e.g., -5 for EST), we add 5 hours to get UTC
      // If offset is positive (e.g., +9 for JST), we subtract 9 hours to get UTC
      int offset_seconds = static_cast<int>(d_date_time.time_zone_offset.value()) * 60;
      timeT = utc_time - offset_seconds; // Subtract offset to get UTC
    } else {
      // No timezone offset provided, assume input is already UTC
      timeT = utc_time;
    }
 
    // Convert time_t to system_clock::time_point
    auto timePoint = std::chrono::system_clock::from_time_t(timeT);
 
    // Add milliseconds
    int milliseconds = 0;
    if (d_date_time.second)
    {
      milliseconds = static_cast<int>(d_date_time.second.value());
    }
    timePoint += std::chrono::milliseconds(milliseconds);
 
    // Extract seconds and nanoseconds since epoch
    auto duration = timePoint.time_since_epoch();
    auto seconds = std::chrono::duration_cast<std::chrono::seconds>(duration);
    auto nanoseconds = std::chrono::duration_cast<std::chrono::nanoseconds>(duration - seconds);
 
    msg.sec = static_cast<int32_t>(seconds.count());
    msg.nanosec = static_cast<uint32_t>(nanoseconds.count());
 
    return msg;
  }
 
  // if simulation, we ignore the date, month, year etc because the simulation won't be that long
  double seconds;
  const auto fractional_secs{std::modf(
    remove_units(units::time::second_t{d_date_time.hour.value_or(units::time::second_t{0.0})}) +
      remove_units(units::time::second_t{d_date_time.minute.value_or(units::time::second_t{0.0})}) +
      remove_units(units::time::second_t{d_date_time.second.value_or(units::time::second_t{0.0})}),
    &seconds)};
 
  msg.sec = static_cast<std::int32_t>(seconds);
  msg.nanosec = static_cast<std::int32_t>(fractional_secs * 1e9);
 
  return msg;
}

References remove_units().

Referenced by to_detection_list_msg().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ transform_from_map_to_utm()

auto carma_cooperative_perception::transform_from_map_to_utm	(	carma_cooperative_perception_interfaces::msg::DetectionList	detection_list,
		const std::string &	map_origin
	)		-> carma_cooperative_perception_interfaces::msg::DetectionList

Definition at line 27 of file external_object_list_to_detection_list_component.cpp.

{
  gsl::owner<PJ_CONTEXT *> context = proj_context_create();
  proj_log_level(context, PJ_LOG_NONE);
 
  if (context == nullptr) {
    const std::string error_string{proj_errno_string(proj_context_errno(context))};
    throw std::invalid_argument("Could not create PROJ context: " + error_string + '.');
  }
 
  gsl::owner<PJ *> map_transformation = proj_create(context, map_origin.c_str());
 
  if (map_transformation == nullptr) {
    const std::string error_string{proj_errno_string(proj_context_errno(context))};
    throw std::invalid_argument(
      "Could not create PROJ transform to origin '" + map_origin + "': " + error_string + '.');
  }
 
  std::vector<carma_cooperative_perception_interfaces::msg::Detection> new_detections;
  for (auto detection : detection_list.detections) {
    // Coordinate order is easting (meters), northing (meters)
    const auto position_planar{
      proj_coord(detection.pose.pose.position.x, detection.pose.pose.position.y, 0, 0)};
    const auto proj_inverse{proj_trans(map_transformation, PJ_DIRECTION::PJ_INV, position_planar)};
    const Wgs84Coordinate position_wgs84{
      units::angle::radian_t{proj_inverse.lp.phi}, units::angle::radian_t{proj_inverse.lp.lam},
      units::length::meter_t{detection.pose.pose.position.z}};
 
    const auto utm_zone{calculate_utm_zone(position_wgs84)};
    const auto position_utm{project_to_utm(position_wgs84)};
 
    detection.header.frame_id = to_string(utm_zone);
    detection.pose.pose.position.x = remove_units(position_utm.easting);
    detection.pose.pose.position.y = remove_units(position_utm.northing);
 
    new_detections.push_back(std::move(detection));
  }
 
  std::swap(detection_list.detections, new_detections);
 
  proj_destroy(map_transformation);
  proj_context_destroy(context);
 
  return detection_list;
}

References calculate_utm_zone(), project_to_utm(), remove_units(), and to_string().

Here is the call graph for this function:

◆ transform_pose_from_map_to_wgs84()

auto carma_cooperative_perception::transform_pose_from_map_to_wgs84	(	const geometry_msgs::msg::PoseStamped &	source_pose,
		const std::shared_ptr< lanelet::projection::LocalFrameProjector > &	map_projection
	)		-> carma_v2x_msgs::msg::Position3D

Definition at line 323 of file msg_conversion.cpp.

{
  carma_v2x_msgs::msg::Position3D ref_pos;
  lanelet::BasicPoint3d source_pose_basicpoint{
    source_pose.pose.position.x, source_pose.pose.position.y, 0.0};
 
  lanelet::GPSPoint wgs84_ref_pose = map_projection->reverse(source_pose_basicpoint);
 
  ref_pos.longitude = wgs84_ref_pose.lon;
  ref_pos.latitude = wgs84_ref_pose.lat;
  ref_pos.elevation = wgs84_ref_pose.ele;
  ref_pos.elevation_exists = true;
 
  return ref_pos;
}

Referenced by to_sdsm_msg().

Here is the caller graph for this function:

Variable Documentation

◆ April

constexpr Month carma_cooperative_perception::April {4}

inlineconstexpr

Definition at line 234 of file month.hpp.

◆ August

constexpr Month carma_cooperative_perception::August {8}

inlineconstexpr

Definition at line 238 of file month.hpp.

◆ December

constexpr Month carma_cooperative_perception::December {12}

inlineconstexpr

Definition at line 242 of file month.hpp.

◆ February

constexpr Month carma_cooperative_perception::February {2}

inlineconstexpr

Definition at line 232 of file month.hpp.

◆ January

constexpr Month carma_cooperative_perception::January {1}

inlineconstexpr

Definition at line 231 of file month.hpp.

◆ July

constexpr Month carma_cooperative_perception::July {7}

inlineconstexpr

Definition at line 237 of file month.hpp.

◆ June

constexpr Month carma_cooperative_perception::June {6}

inlineconstexpr

Definition at line 236 of file month.hpp.

◆ March

constexpr Month carma_cooperative_perception::March {3}

inlineconstexpr

Definition at line 233 of file month.hpp.

◆ May

constexpr Month carma_cooperative_perception::May {5}

inlineconstexpr

Definition at line 235 of file month.hpp.

◆ November

constexpr Month carma_cooperative_perception::November {11}

inlineconstexpr

Definition at line 241 of file month.hpp.

◆ October

constexpr Month carma_cooperative_perception::October {10}

inlineconstexpr

Definition at line 240 of file month.hpp.

◆ September

constexpr Month carma_cooperative_perception::September {9}

inlineconstexpr

Definition at line 239 of file month.hpp.

Classes

Typedefs

Enumerations

Functions

Variables

Detailed Description

Typedef Documentation

◆ Detection

◆ Track

Enumeration Type Documentation

◆ Hemisphere

Function Documentation

◆ calc_detection_time_stamp()

◆ calc_relative_position()

◆ calc_sdsm_time_offset()

◆ calculate_grid_convergence() [1/2]

◆ calculate_grid_convergence() [2/2]

◆ calculate_utm_zone()

◆ convert_covariances()

◆ convert_object_type()

◆ enu_orientation_to_true_heading()

◆ euclidean_distance_squared()

◆ heading_to_enu_yaw()

◆ make_ctra_detection()

◆ make_ctrv_detection()

◆ make_detection()

◆ make_semantic_class()

◆ ned_to_enu()

◆ operator!=()

◆ operator+() [1/2]

◆ operator+() [2/2]

◆ operator+=()

◆ operator-() [1/2]

◆ operator-() [2/2]

◆ operator-=()

◆ operator==()

◆ predict_track_states()

◆ project_to_carma_map()

◆ project_to_utm()

◆ remove_units()

◆ semantic_class_to_numeric_value()

◆ temporally_align_detections()

◆ to_ddate_time_msg()

◆ to_detected_object_data_msg()

◆ to_detection_list_msg() [1/2]

◆ to_detection_list_msg() [2/2]

◆ to_detection_msg()

◆ to_external_object_list_msg()

◆ to_external_object_msg()

◆ to_position_msg() [1/2]

◆ to_position_msg() [2/2]

◆ to_ros_msg() [1/3]

◆ to_ros_msg() [2/3]

◆ to_ros_msg() [3/3]

◆ to_sdsm_msg()

◆ to_string() [1/2]

◆ to_string() [2/2]

◆ to_time_msg()

◆ transform_from_map_to_utm()

◆ transform_pose_from_map_to_wgs84()

Variable Documentation

◆ April

◆ August

◆ December

◆ February

◆ January

◆ July

◆ June

◆ March

◆ May

◆ November

◆ October

◆ September