carma_cooperative_perception Namespace Reference

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	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) -> 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) -> carma_cooperative_perception_interfaces::msg::DetectionList
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
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 74 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 209 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 132 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:

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 182 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 177 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 82 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 451 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 443 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 97 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 586 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 446 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
	)		-> carma_cooperative_perception_interfaces::msg::DetectionList

Definition at line 246 of file msg_conversion.cpp.

{
  carma_cooperative_perception_interfaces::msg::DetectionList detection_list;
 
  const auto ref_pos_3d{Position3D::from_msg(sdsm.ref_pos)};
  const Wgs84Coordinate ref_pos_wgs84{
    ref_pos_3d.latitude, ref_pos_3d.longitude, ref_pos_3d.elevation.value()};
  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);
 
    // TemporaryID and octet string terms come from the SAE J2735 message definitions
    static constexpr auto to_string = [](const std::vector<std::uint8_t> & temporary_id) {
      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;
    };
 
    detection.id =
      to_string(sdsm.source_id.id) + "-" + std::to_string(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})});
 
    // Pose covariance is flattened 6x6 matrix with rows/columns of x, y, z, roll, pitch, yaw
    try {
      detection.pose.covariance.at(0) =
        0.5 * std::pow(j2735_v2x_msgs::to_double(common_data.pos_confidence.pos).value(), 2);
      detection.pose.covariance.at(7) =
        0.5 * std::pow(j2735_v2x_msgs::to_double(common_data.pos_confidence.pos).value(), 2);
    } catch (const std::bad_optional_access &) {
      throw std::runtime_error("missing position confidence");
    }
 
    try {
      detection.pose.covariance.at(14) =
        0.5 * std::pow(j2735_v2x_msgs::to_double(common_data.pos_confidence.elevation).value(), 2);
    } catch (const std::bad_optional_access &) {
      throw std::runtime_error("missing elevation confidence");
    }
 
    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;
    quat_tf.setRPY(0, 0, remove_units(units::angle::radian_t{enu_yaw}));
    detection.pose.pose.orientation = tf2::toMsg(quat_tf);
 
    try {
      // Pose covariance is flattened 6x6 matrix with rows/columns of x, y, z, roll, pitch, yaw
      detection.pose.covariance.at(35) =
        0.5 * std::pow(j2735_v2x_msgs::to_double(common_data.heading_conf).value(), 2);
    } catch (const std::bad_optional_access &) {
      throw std::runtime_error("missing heading confidence");
    }
 
    const auto speed{Speed::from_msg(common_data.speed)};
    detection.twist.twist.linear.x =
      remove_units(units::velocity::meters_per_second_t{speed.speed});
 
    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});
 
    try {
      // Twist covariance is flattened 6x6 matrix with rows/columns of x, y, z, roll, pitch, yaw
      detection.twist.covariance.at(0) =
        0.5 * std::pow(j2735_v2x_msgs::to_double(common_data.speed_confidence).value(), 2);
    } catch (const std::bad_optional_access &) {
      throw std::runtime_error("missing speed confidence");
    }
 
    try {
      detection.twist.covariance.at(14) =
        0.5 * std::pow(j2735_v2x_msgs::to_double(common_data.speed_confidence_z).value(), 2);
    } catch (const std::bad_optional_access &) {
      throw std::runtime_error("missing z-speed confidence");
    }
 
    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});
 
    try {
      detection.twist.covariance.at(35) =
        0.5 * std::pow(j2735_v2x_msgs::to_double(common_data.acc_cfd_yaw).value(), 2);
    } catch (const std::bad_optional_access &) {
      throw std::runtime_error("missing yaw-rate confidence");
    }
 
    switch (common_data.obj_type.object_type) {
      case common_data.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 common_data.obj_type.VRU:
        detection.motion_model = detection.MOTION_MODEL_CTRV;
        detection.semantic_class = detection.SEMANTIC_CLASS_PEDESTRIAN;
        break;
      case common_data.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;
    }
 
    detection_list.detections.push_back(std::move(detection));
  }
 
  return detection_list;
}

References calc_detection_time_stamp(), calculate_grid_convergence(), carma_cooperative_perception::MapCoordinate::easting, carma_cooperative_perception::AccelerationSet4Way::from_msg(), carma_cooperative_perception::DDateTime::from_msg(), carma_cooperative_perception::Heading::from_msg(), carma_cooperative_perception::Position3D::from_msg(), carma_cooperative_perception::Speed::from_msg(), carma_cooperative_perception::MeasurementTimeOffset::from_msg(), carma_cooperative_perception::PositionOffsetXYZ::from_msg(), heading_to_enu_yaw(), carma_cooperative_perception::Wgs84Coordinate::latitude, ned_to_enu(), project_to_carma_map(), remove_units(), to_position_msg(), to_string(), and to_time_msg().

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 390 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 534 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 463 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 166 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 155 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 547 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()

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().

Referenced by cooperative_lanechange::CooperativeLaneChangePlugin::add_trajectory_to_response(), carma_wm_ctrl::WMBroadcaster::addScheduleFromMsg(), route_following_plugin::RouteFollowingPlugin::addStopAndWaitAtRouteEnd(), lci_strategic_plugin::LCIStrategicPlugin::boundary_accel_nocruise_maxspeed_decel(), lci_strategic_plugin::LCIStrategicPlugin::boundary_accel_or_decel_complete_upper(), lci_strategic_plugin::LCIStrategicPlugin::boundary_decel_nocruise_minspeed_accel_complete(), cooperative_lanechange::CooperativeLaneChangePlugin::bsmIDtoString(), yield_plugin::YieldPlugin::bsmIDtoString(), calculate_grid_convergence(), mobilitypath_visualizer::MobilityPathVisualizer::callbackMobilityPath(), lci_strategic_plugin::LCIStrategicPlugin::canArriveAtGreenWithCertainty(), platoon_strategic_ihp::PlatoonManager::changeFromFollowerToLeader(), mobilitypath_publisher::MobilityPathPublication::compose_mobility_header(), lci_strategic_plugin::LCIStrategicPlugin::composeIntersectionTransitMessage(), sci_strategic_plugin::SCIStrategicPlugin::composeIntersectionTransitMessage(), mobilitypath_visualizer::MobilityPathVisualizer::composeLabelMarker(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::composeLaneChangeManeuverMessage(), route_following_plugin::RouteFollowingPlugin::composeLaneChangeManeuverMessage(), approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::composeLaneChangeManeuverMessage(), route_following_plugin::RouteFollowingPlugin::composeLaneFollowingManeuverMessage(), approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::composeLaneFollowingManeuverMessage(), stop_and_dwell_strategic_plugin::StopAndDwellStrategicPlugin::composeLaneFollowingManeuverMessage(), sci_strategic_plugin::SCIStrategicPlugin::composeLaneFollowingManeuverMessage(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::composeManeuverMessage(), plan_delegator::PlanDelegator::composePlanTrajectoryRequest(), sci_strategic_plugin::SCIStrategicPlugin::composeStopAndWaitManeuverMessage(), stop_and_dwell_strategic_plugin::StopAndDwellStrategicPlugin::composeStopAndWaitManeuverMessage(), lci_strategic_plugin::LCIStrategicPlugin::composeStopAndWaitManeuverMessage(), route_following_plugin::RouteFollowingPlugin::composeStopAndWaitManeuverMessage(), approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::composeStopAndWaitManeuverMessage(), carma_wm_ctrl::WMBroadcaster::composeTCRStatus(), lci_strategic_plugin::LCIStrategicPlugin::composeTrajectorySmoothingManeuverMessage(), carma_wm_ctrl::WMBroadcaster::controlRequestFromRoute(), trajectory_follower_wrapper::TrajectoryFollowerWrapperNode::convert_cmd(), pure_pursuit_wrapper::PurePursuitWrapperNode::convert_cmd(), carma_wm::SignalizedIntersectionManager::convertLaneToLaneletId(), basic_autonomy::waypoint_generation::create_lanechange_geometry(), cooperative_lanechange::CooperativeLaneChangePlugin::create_mobility_request(), carma_cooperative_perception::MultipleObjectTrackerNode::execute_pipeline(), object_visualizer::Node::external_objects_callback(), lci_strategic_plugin::LCIStrategicPlugin::extractInitialState(), sci_strategic_plugin::SCIStrategicPlugin::extractInitialState(), stop_and_dwell_strategic_plugin::StopAndDwellStrategicPlugin::extractInitialState(), yield_plugin::YieldPlugin::generate_JMT_trajectory(), approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin::generateApproachingErvStatusMessage(), lci_strategic_plugin::LCIStrategicPlugin::generateMobilityOperation(), sci_strategic_plugin::SCIStrategicPlugin::generateMobilityOperation(), carma_wm_ctrl::WMBroadcaster::geofenceCallback(), yield_plugin::YieldPlugin::get_collision(), yield_plugin::YieldPlugin::get_collision_time(), lci_strategic_plugin::LCIStrategicPlugin::get_eet_or_tbd(), lci_strategic_plugin::LCIStrategicPlugin::get_final_entry_time_and_conditions(), lci_strategic_plugin::LCIStrategicPlugin::get_nearest_green_entry_time(), yield_plugin::YieldPlugin::get_predicted_velocity_at_time(), lci_strategic_plugin::LCIStrategicPlugin::get_ts_case(), carma_wm::IndexedDistanceMap::getElementIndexByDistance(), plan_delegator::PlanDelegator::getLaneChangeInformation(), lci_strategic_plugin::LCIStrategicPlugin::getLaneletsBetweenWithException(), sci_strategic_plugin::SCIStrategicPlugin::getLaneletsBetweenWithException(), stop_and_dwell_strategic_plugin::StopAndDwellStrategicPlugin::getLaneletsBetweenWithException(), lci_strategic_plugin::LCIStrategicPlugin::handleFailureCaseHelper(), lci_strategic_plugin::LCIStrategicPlugin::handleGreenSignalScenario(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::is_lanechange_possible(), yield_plugin::YieldPlugin::is_object_behind_vehicle(), plan_delegator::PlanDelegator::isManeuverExpired(), make_detection(), carma_wm::CARMAWorldModel::min_end_time_converter_minute_of_year(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_op_cb_leader(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_op_cb_preparetojoin(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_req_cb_leadwithoperation(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_resp_cb_preparetojoin(), carma_wm::query::nonConnectedAdjacentLeft(), route::RouteStateWorker::onRouteEvent(), intersection_transit_maneuvering::operator<<(), route_following_plugin::RouteFollowingPlugin::plan_maneuvers_callback(), inlanecruising_plugin::InLaneCruisingPlugin::plan_trajectory_callback(), yield_plugin::YieldPlugin::plan_trajectory_callback(), stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPlugin::plan_trajectory_callback(), platooning_tactical_plugin::PlatooningTacticalPlugin::plan_trajectory_cb(), stop_and_wait_plugin::StopandWait::plan_trajectory_cb(), light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::planTrajectoryCB(), lci_strategic_plugin::LCIStrategicPlugin::planWhenAPPROACHING(), lci_strategic_plugin::LCIStrategicPlugin::planWhenWAITING(), carma_wm::CARMAWorldModel::pointFromRouteTrackPos(), lci_strategic_plugin::LCIStrategicPlugin::print_params(), carma_wm::CARMAWorldModel::processSpatFromMsg(), project_to_utm(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::run_candidate_follower(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::run_leader_aborting(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::run_prepare_to_join(), boost::serialization::save(), plan_delegator::anonymous_namespace{plan_delegator.cpp}::setManeuverEndingLaneletId(), route_following_plugin::anonymous_namespace{route_following_plugin.cpp}::setManeuverLaneletIds(), plan_delegator::anonymous_namespace{plan_delegator.cpp}::setManeuverStartingLaneletId(), to_detection_list_msg(), to_detection_msg(), to_string(), transform_from_map_to_utm(), lci_strategic_plugin::LCIStrategicPlugin::ts_case1(), lci_strategic_plugin::LCIStrategicPlugin::ts_case2(), lci_strategic_plugin::LCIStrategicPlugin::ts_case3(), lci_strategic_plugin::LCIStrategicPlugin::ts_case4(), lci_strategic_plugin::LCIStrategicPlugin::ts_case6(), lci_strategic_plugin::LCIStrategicPlugin::ts_case7(), plan_delegator::PlanDelegator::updateManeuverParameters(), and lci_strategic_plugin::LCIStrategicPlugin::validLightState().

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

)

-> builtin_interfaces::msg::Time

Definition at line 58 of file msg_conversion.cpp.

{
  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)};
 
  builtin_interfaces::msg::Time msg;
  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 227 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 232 of file month.hpp.

August

constexpr Month carma_cooperative_perception::August {8}

inlineconstexpr

Definition at line 236 of file month.hpp.

December

constexpr Month carma_cooperative_perception::December {12}

inlineconstexpr

Definition at line 240 of file month.hpp.

February

constexpr Month carma_cooperative_perception::February {2}

inlineconstexpr

Definition at line 230 of file month.hpp.

January

constexpr Month carma_cooperative_perception::January {1}

inlineconstexpr

Definition at line 229 of file month.hpp.

July

constexpr Month carma_cooperative_perception::July {7}

inlineconstexpr

Definition at line 235 of file month.hpp.

June

constexpr Month carma_cooperative_perception::June {6}

inlineconstexpr

Definition at line 234 of file month.hpp.

March

constexpr Month carma_cooperative_perception::March {3}

inlineconstexpr

Definition at line 231 of file month.hpp.

May

constexpr Month carma_cooperative_perception::May {5}

inlineconstexpr

Definition at line 233 of file month.hpp.

November

constexpr Month carma_cooperative_perception::November {11}

inlineconstexpr

Definition at line 239 of file month.hpp.

October

constexpr Month carma_cooperative_perception::October {10}

inlineconstexpr

Definition at line 238 of file month.hpp.

September

constexpr Month carma_cooperative_perception::September {9}

inlineconstexpr

Definition at line 237 of file month.hpp.