17#include <carma_cooperative_perception_interfaces/msg/track.hpp>
18#include <carma_cooperative_perception_interfaces/msg/track_list.hpp>
19#include <carma_perception_msgs/msg/external_object.hpp>
20#include <carma_perception_msgs/msg/external_object_list.hpp>
21#include <j2735_v2x_msgs/msg/d_date_time.hpp>
22#include <j2735_v2x_msgs/msg/personal_device_user_type.hpp>
23#include <j2735_v2x_msgs/msg/positional_accuracy.hpp>
24#include <j2735_v2x_msgs/to_floating_point.hpp>
25#include <j3224_v2x_msgs/msg/detected_object_data.hpp>
26#include <j3224_v2x_msgs/msg/equipment_type.hpp>
27#include <j3224_v2x_msgs/msg/measurement_time_offset.hpp>
28#include <j3224_v2x_msgs/msg/object_type.hpp>
29#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
42#include <fmt/format.h>
45#include <gsl/pointers>
53#include <lanelet2_core/geometry/Lanelet.h>
54#include <lanelet2_extension/projection/local_frame_projector.h>
55#include <boost/date_time/posix_time/conversion.hpp>
56#include <boost/date_time/posix_time/posix_time_io.hpp>
57#include "boost/date_time/posix_time/posix_time.hpp"
64 builtin_interfaces::msg::Time msg;
67 std::tm timeinfo = {};
70 if (d_date_time.year){
73 timeinfo.tm_year =
static_cast<int>(
remove_units(d_date_time.year.value())) - 1900;
76 throw std::invalid_argument(
77 "Year must be greater than 1970 for live date/time conversion");
82 if (d_date_time.month &&
static_cast<int>(d_date_time.month.value().get_value()) != 0)
85 timeinfo.tm_mon =
static_cast<int>(d_date_time.month.value().get_value()) - 1;
89 if (d_date_time.day &&
static_cast<int>(d_date_time.day.value()) != 0)
92 timeinfo.tm_mday =
static_cast<int>(d_date_time.day.value());
99 if (d_date_time.hour &&
static_cast<int>(d_date_time.hour.value()) != 31)
102 timeinfo.tm_hour =
static_cast<int>(d_date_time.hour.value());
106 if (d_date_time.minute &&
static_cast<int>(d_date_time.minute.value()) != 60)
109 timeinfo.tm_min =
static_cast<int>(d_date_time.minute.value());
119 timeinfo.tm_gmtoff = 0;
120 timeinfo.tm_isdst = 0;
121 std::time_t utc_time = timegm(&timeinfo);
123 if (d_date_time.time_zone_offset) {
127 int offset_seconds =
static_cast<int>(d_date_time.time_zone_offset.value()) * 60;
128 timeT = utc_time - offset_seconds;
135 auto timePoint = std::chrono::system_clock::from_time_t(timeT);
138 int milliseconds = 0;
139 if (d_date_time.second)
141 milliseconds =
static_cast<int>(d_date_time.second.value());
143 timePoint += std::chrono::milliseconds(milliseconds);
146 auto duration = timePoint.time_since_epoch();
147 auto seconds = std::chrono::duration_cast<std::chrono::seconds>(duration);
148 auto nanoseconds = std::chrono::duration_cast<std::chrono::nanoseconds>(duration - seconds);
150 msg.sec =
static_cast<int32_t
>(seconds.count());
151 msg.nanosec =
static_cast<uint32_t
>(nanoseconds.count());
158 const auto fractional_secs{std::modf(
159 remove_units(units::time::second_t{d_date_time.hour.value_or(units::time::second_t{0.0})}) +
160 remove_units(units::time::second_t{d_date_time.minute.value_or(units::time::second_t{0.0})}) +
161 remove_units(units::time::second_t{d_date_time.second.value_or(units::time::second_t{0.0})}),
164 msg.sec =
static_cast<std::int32_t
>(seconds);
165 msg.nanosec =
static_cast<std::int32_t
>(fractional_secs * 1e9);
173 sdsm_time.
second.value() += offset.measurement_time_offset;
180 auto offset_enu{offset_ned};
183 offset_enu.offset_x = offset_ned.offset_y;
184 offset_enu.offset_y = offset_ned.offset_x;
186 if (offset_enu.offset_z) {
187 offset_enu.offset_z.value() *= -1;
194 -> j2735_v2x_msgs::msg::DDateTime
196 j2735_v2x_msgs::msg::DDateTime ddate_time_output;
199 boost::posix_time::ptime posix_time = boost::posix_time::from_time_t(builtin_time.sec) +
200 boost::posix_time::nanosec(builtin_time.nanosec);
202 const auto time_stamp_year = posix_time.date().year();
203 const auto time_stamp_month = posix_time.date().month();
204 const auto time_stamp_day = posix_time.date().day();
206 const auto hours_of_day = posix_time.time_of_day().hours();
207 const auto minutes_of_hour = posix_time.time_of_day().minutes();
208 const auto seconds_of_minute = posix_time.time_of_day().seconds();
210 ddate_time_output.presence_vector = 0;
212 ddate_time_output.presence_vector |= j2735_v2x_msgs::msg::DDateTime::YEAR;
213 ddate_time_output.year.year = time_stamp_year;
214 ddate_time_output.presence_vector |= j2735_v2x_msgs::msg::DDateTime::MONTH;
215 ddate_time_output.month.month = time_stamp_month;
216 ddate_time_output.presence_vector |= j2735_v2x_msgs::msg::DDateTime::DAY;
217 ddate_time_output.day.day = time_stamp_day;
218 ddate_time_output.presence_vector |= j2735_v2x_msgs::msg::DDateTime::HOUR;
219 ddate_time_output.hour.hour = hours_of_day;
220 ddate_time_output.presence_vector |= j2735_v2x_msgs::msg::DDateTime::MINUTE;
221 ddate_time_output.minute.minute = minutes_of_hour;
222 ddate_time_output.presence_vector |= j2735_v2x_msgs::msg::DDateTime::SECOND;
223 ddate_time_output.second.millisecond = seconds_of_minute;
225 return ddate_time_output;
229 const builtin_interfaces::msg::Time & external_object_list_stamp,
230 const builtin_interfaces::msg::Time & external_object_stamp)
231 -> carma_v2x_msgs::msg::MeasurementTimeOffset
233 carma_v2x_msgs::msg::MeasurementTimeOffset time_offset;
235 boost::posix_time::ptime external_object_list_time =
236 boost::posix_time::from_time_t(external_object_list_stamp.sec) +
237 boost::posix_time::nanosec(external_object_list_stamp.nanosec);
239 boost::posix_time::ptime external_object_time =
240 boost::posix_time::from_time_t(external_object_stamp.sec) +
241 boost::posix_time::nanosec(external_object_stamp.nanosec);
243 boost::posix_time::time_duration offset_duration =
244 (external_object_list_time - external_object_time);
246 time_offset.measurement_time_offset = offset_duration.total_seconds();
253 geometry_msgs::msg::Point msg;
264 geometry_msgs::msg::Point msg;
275 return units::angle::degree_t{std::fmod(-(
remove_units(heading) - 90.0) + 360.0, 360.0)};
279 double yaw,
const lanelet::BasicPoint3d & obj_pose,
280 const std::shared_ptr<lanelet::projection::LocalFrameProjector> & map_projection)
281 -> units::angle::degree_t
284 lanelet::GPSPoint wgs_obj_pose = map_projection->reverse(obj_pose);
287 gsl::owner<PJ_CONTEXT *> context = proj_context_create();
288 gsl::owner<PJ *> transform = proj_create(context, map_projection->ECEF_PROJ_STR);
289 units::angle::degree_t grid_heading{std::fmod(90 - yaw + 360, 360)};
291 const auto factors = proj_factors(
292 transform, proj_coord(proj_torad(wgs_obj_pose.lon), proj_torad(wgs_obj_pose.lat), 0, 0));
293 units::angle::degree_t grid_convergence{proj_todeg(factors.meridian_convergence)};
295 auto wgs_heading = grid_convergence + grid_heading;
297 proj_destroy(transform);
298 proj_context_destroy(context);
306 const geometry_msgs::msg::PoseStamped & source_pose,
307 const carma_v2x_msgs::msg::PositionOffsetXYZ & position_offset)
308 -> carma_v2x_msgs::msg::PositionOffsetXYZ
310 carma_v2x_msgs::msg::PositionOffsetXYZ adjusted_offset;
312 adjusted_offset.offset_x.object_distance =
313 position_offset.offset_x.object_distance - source_pose.pose.position.x;
314 adjusted_offset.offset_y.object_distance =
315 position_offset.offset_y.object_distance - source_pose.pose.position.y;
316 adjusted_offset.offset_z.object_distance =
317 position_offset.offset_z.object_distance - source_pose.pose.position.z;
318 adjusted_offset.presence_vector = carma_v2x_msgs::msg::PositionOffsetXYZ::HAS_OFFSET_Z;
320 return adjusted_offset;
324 const geometry_msgs::msg::PoseStamped & source_pose,
325 const std::shared_ptr<lanelet::projection::LocalFrameProjector> & map_projection)
326 -> carma_v2x_msgs::msg::Position3D
328 carma_v2x_msgs::msg::Position3D ref_pos;
329 lanelet::BasicPoint3d source_pose_basicpoint{
330 source_pose.pose.position.x, source_pose.pose.position.y, 0.0};
332 lanelet::GPSPoint wgs84_ref_pose = map_projection->reverse(source_pose_basicpoint);
334 ref_pos.longitude = wgs84_ref_pose.lon;
335 ref_pos.latitude = wgs84_ref_pose.lat;
336 ref_pos.elevation = wgs84_ref_pose.ele;
337 ref_pos.elevation_exists =
true;
344std::string
to_string(
const std::vector<std::uint8_t> & temporary_id) {
346 str.reserve(2 * std::size(temporary_id));
348 std::array<char, 2> buffer;
349 for (
const auto & octet_string : temporary_id) {
350 std::to_chars(std::begin(buffer), std::end(buffer), octet_string, 16);
351 str.push_back(std::toupper(std::get<0>(buffer)));
352 str.push_back(std::toupper(std::get<1>(buffer)));
360 const j3224_v2x_msgs::msg::ObjectType& j3224_obj_type)
362 switch (j3224_obj_type.object_type) {
363 case j3224_obj_type.ANIMAL:
364 detection.motion_model = detection.MOTION_MODEL_CTRV;
366 detection.semantic_class = detection.SEMANTIC_CLASS_UNKNOWN;
368 case j3224_obj_type.VRU:
369 detection.motion_model = detection.MOTION_MODEL_CTRV;
370 detection.semantic_class = detection.SEMANTIC_CLASS_PEDESTRIAN;
372 case j3224_obj_type.VEHICLE:
373 detection.motion_model = detection.MOTION_MODEL_CTRV;
374 detection.semantic_class = detection.SEMANTIC_CLASS_SMALL_VEHICLE;
377 detection.motion_model = detection.MOTION_MODEL_CTRV;
378 detection.semantic_class = detection.SEMANTIC_CLASS_UNKNOWN;
383 const carma_v2x_msgs::msg::DetectedObjectCommonData& common_data,
384 const std::optional<SdsmToDetectionListConfig>& conversion_adjustment)
387 double original_pose_covariance_x = 0.0;
388 double original_pose_covariance_y = 0.0;
389 double original_pose_covariance_z = 0.0;
390 double original_pose_covariance_yaw = 0.0;
391 double original_twist_covariance_x = 0.0;
392 double original_twist_covariance_z = 0.0;
393 double original_twist_covariance_yaw = 0.0;
397 original_pose_covariance_x =
398 0.5 * std::pow(j2735_v2x_msgs::to_double(common_data.pos_confidence.pos).value(), 2);
399 original_pose_covariance_y = original_pose_covariance_x;
401 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"sdsm_to_detection_list_node"),
402 "Original pose covariance X/Y: " << original_pose_covariance_x);
403 }
catch (
const std::bad_optional_access &) {
404 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"sdsm_to_detection_list_node"),
405 "Missing position confidence");
409 original_pose_covariance_z =
410 0.5 * std::pow(j2735_v2x_msgs::to_double(common_data.pos_confidence.elevation).value(), 2);
412 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"sdsm_to_detection_list_node"),
413 "Original pose covariance Z: " << original_pose_covariance_z);
414 }
catch (
const std::bad_optional_access &) {
415 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"sdsm_to_detection_list_node"),
416 "Missing elevation confidence");
421 original_pose_covariance_yaw =
422 0.5 * std::pow(j2735_v2x_msgs::to_double(common_data.heading_conf).value(), 2);
424 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"sdsm_to_detection_list_node"),
425 "Original pose covariance yaw: " << original_pose_covariance_yaw);
426 }
catch (
const std::bad_optional_access &) {
427 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"sdsm_to_detection_list_node"),
428 "Missing heading confidence");
433 original_twist_covariance_x =
434 0.5 * std::pow(j2735_v2x_msgs::to_double(common_data.speed_confidence).value(), 2);
436 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"sdsm_to_detection_list_node"),
437 "Original twist covariance X: " << original_twist_covariance_x);
438 }
catch (
const std::bad_optional_access &) {
439 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"sdsm_to_detection_list_node"),
440 "Missing speed confidence");
443 if (!common_data.speed_z.unavailable){
446 original_twist_covariance_z =
447 0.5 * std::pow(j2735_v2x_msgs::to_double(common_data.speed_confidence_z).value(), 2);
449 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"sdsm_to_detection_list_node"),
450 "Original twist covariance Z: " << original_twist_covariance_z);
451 }
catch (
const std::bad_optional_access &) {
452 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"sdsm_to_detection_list_node"),
453 "Missing z-speed confidence");
457 original_twist_covariance_z = 0.0;
458 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"sdsm_to_detection_list_node"),
459 "Original twist covariance Z: 0.0 (speed_z not provided)");
463 if(
static_cast<bool>(common_data.accel_4_way.yaw_rate)){
466 original_twist_covariance_yaw =
467 0.5 * std::pow(j2735_v2x_msgs::to_double(common_data.acc_cfd_yaw).value(), 2);
469 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"sdsm_to_detection_list_node"),
470 "Original twist covariance yaw: " << original_twist_covariance_yaw);
471 }
catch (
const std::bad_optional_access &) {
472 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"sdsm_to_detection_list_node"),
473 "Missing yaw-rate confidence");
477 original_twist_covariance_yaw = 0.0;
478 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"sdsm_to_detection_list_node"),
479 "Original twist covariance yaw: 0.0 (yaw_rate not provided)");
482 if (conversion_adjustment && conversion_adjustment.value().overwrite_covariance)
485 detection.pose.covariance[0] = conversion_adjustment.value().pose_covariance_x;
486 detection.pose.covariance[7] = conversion_adjustment.value().pose_covariance_y;
487 detection.pose.covariance[14] = conversion_adjustment.value().pose_covariance_z;
488 detection.pose.covariance[35] = conversion_adjustment.value().pose_covariance_yaw;
491 detection.twist.covariance[0] = conversion_adjustment.value().twist_covariance_x;
492 detection.twist.covariance[14] = conversion_adjustment.value().twist_covariance_z;
493 detection.twist.covariance[35] = conversion_adjustment.value().twist_covariance_yaw;
496 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"sdsm_to_detection_list_node"),
497 "POSE COVARIANCE COMPARISON - Original vs Hardcoded: " <<
498 "X: " << original_pose_covariance_x <<
" -> " << detection.pose.covariance[0] <<
", " <<
499 "Y: " << original_pose_covariance_y <<
" -> " << detection.pose.covariance[7] <<
", " <<
500 "Z: " << original_pose_covariance_z <<
" -> " << detection.pose.covariance[14] <<
", " <<
501 "Yaw: " << original_pose_covariance_yaw <<
" -> " << detection.pose.covariance[35]);
503 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"sdsm_to_detection_list_node"),
504 "TWIST COVARIANCE COMPARISON - Original vs Hardcoded: " <<
505 "X: " << original_twist_covariance_x <<
" -> , " << detection.twist.covariance[0] <<
", " <<
506 "Z: " << original_twist_covariance_z <<
" -> , " << detection.twist.covariance[14] <<
", " <<
507 "Yaw: " << original_twist_covariance_yaw <<
" -> " << detection.twist.covariance[35]);
512 detection.pose.covariance[0] = original_pose_covariance_x;
513 detection.pose.covariance[7] = original_pose_covariance_y;
514 detection.pose.covariance[14] = original_pose_covariance_z;
515 detection.pose.covariance[35] = original_pose_covariance_yaw;
518 detection.twist.covariance[0] = original_twist_covariance_x;
519 detection.twist.covariance[14] = original_twist_covariance_z;
520 detection.twist.covariance[35] = original_twist_covariance_yaw;
524 for (
size_t i = 0;
i < 36; ++
i) {
525 if (
i != 0 &&
i != 14 &&
i != 35) {
526 detection.twist.covariance[
i] = 0.0;
531 for (
size_t i = 0;
i < 36; ++
i) {
532 if (
i != 0 &&
i != 7 &&
i != 14 &&
i != 35) {
533 detection.pose.covariance[
i] = 0.0;
567 const carma_v2x_msgs::msg::SensorDataSharingMessage & sdsm, std::string_view georeference,
568 bool is_simulation,
const std::optional<SdsmToDetectionListConfig>& conversion_adjustment)
569 -> carma_cooperative_perception_interfaces::msg::DetectionList
571 carma_cooperative_perception_interfaces::msg::DetectionList detection_list;
574 units::length::meter_t elevation(0.0);
575 if(ref_pos_3d.elevation){
576 elevation = ref_pos_3d.elevation.value();
579 ref_pos_3d.
latitude, ref_pos_3d.longitude, elevation};
583 for (
const auto & object_data : sdsm.objects.detected_object_data) {
584 const auto common_data{object_data.detected_object_common_data};
587 detection.header.frame_id =
"map";
593 detection.header.stamp =
to_time_msg(detection_time, is_simulation);
595 detection.id = fmt::format(
"{}-{}",
597 common_data.detected_id.object_id);
601 ref_pos_map.
easting + pos_offset_enu.offset_x, ref_pos_map.northing + pos_offset_enu.offset_y,
602 ref_pos_map.elevation + pos_offset_enu.offset_z.value_or(units::length::meter_t{0.0})});
606 if (conversion_adjustment && conversion_adjustment.value().adjust_pose)
608 detection.pose.pose.position.x += conversion_adjustment.value().x_offset;
609 detection.pose.pose.position.y += conversion_adjustment.value().y_offset;
612 const auto true_heading{units::angle::degree_t{
Heading::from_msg(common_data.heading).heading}};
616 const units::angle::degree_t grid_convergence{
619 const auto grid_heading{true_heading - grid_convergence};
622 tf2::Quaternion quat_tf;
624 if (conversion_adjustment && conversion_adjustment.value().adjust_pose)
628 auto yaw_with_offset = units::angle::radian_t{enu_yaw} +
629 units::angle::radian_t{units::angle::degree_t{conversion_adjustment.value().yaw_offset}};
630 auto new_yaw = std::fmod(
remove_units(yaw_with_offset) + 2 * M_PI, 2 * M_PI);
631 quat_tf.setRPY(0, 0, new_yaw);
636 quat_tf.setRPY(0, 0,
remove_units(units::angle::radian_t{enu_yaw}));
639 detection.pose.pose.orientation = tf2::toMsg(quat_tf);
642 detection.twist.twist.linear.x =
643 remove_units(units::velocity::meters_per_second_t{speed.speed});
645 if (!common_data.speed_z.unavailable){
647 detection.twist.twist.linear.z =
648 remove_units(units::velocity::meters_per_second_t{speed_z.speed});
651 detection.twist.twist.linear.z =
remove_units(units::velocity::meters_per_second_t{0.0});
657 if(
static_cast<bool>(common_data.accel_4_way.yaw_rate)){
659 detection.twist.twist.angular.z =
660 remove_units(units::angular_velocity::degrees_per_second_t{accel_set.yaw_rate});
663 detection.twist.twist.angular.z = 0.0;
670 detection_list.detections.push_back(std::move(detection));
673 return detection_list;
677 const carma_perception_msgs::msg::ExternalObject &
object,
683 detection.header =
object.header;
685 if (
object.presence_vector &
object.BSM_ID_PRESENCE_VECTOR) {
688 std::cbegin(
object.bsm_id), std::cend(
object.bsm_id), std::back_inserter(detection.id),
689 [](
const auto &
i) { return i +
'0'; });
692 if (
object.presence_vector &
object.ID_PRESENCE_VECTOR) {
696 if (
object.presence_vector &
object.POSE_PRESENCE_VECTOR) {
697 detection.pose =
object.pose;
700 if (
object.presence_vector &
object.VELOCITY_PRESENCE_VECTOR) {
701 detection.twist =
object.velocity;
704 if (
object.presence_vector &
object.OBJECT_TYPE_PRESENCE_VECTOR) {
705 switch (
object.object_type) {
706 case object.SMALL_VEHICLE:
707 detection.motion_model = motion_model_mapping.small_vehicle_model;
708 detection.semantic_class = detection.SEMANTIC_CLASS_SMALL_VEHICLE;
710 case object.LARGE_VEHICLE:
711 detection.motion_model = motion_model_mapping.large_vehicle_model;
712 detection.semantic_class = detection.SEMANTIC_CLASS_LARGE_VEHICLE;
714 case object.MOTORCYCLE:
715 detection.motion_model = motion_model_mapping.motorcycle_model;
716 detection.semantic_class = detection.SEMANTIC_CLASS_MOTORCYCLE;
718 case object.PEDESTRIAN:
719 detection.motion_model = motion_model_mapping.pedestrian_model;
720 detection.semantic_class = detection.SEMANTIC_CLASS_PEDESTRIAN;
724 detection.motion_model = motion_model_mapping.unknown_model;
725 detection.semantic_class = detection.SEMANTIC_CLASS_UNKNOWN;
733 const carma_perception_msgs::msg::ExternalObjectList & object_list,
735 -> carma_cooperative_perception_interfaces::msg::DetectionList
737 carma_cooperative_perception_interfaces::msg::DetectionList detection_list;
740 std::cbegin(object_list.objects), std::cend(object_list.objects),
741 std::back_inserter(detection_list.detections),
742 [&motion_model_mapping = std::as_const(motion_model_mapping)](
const auto &
object) {
743 return to_detection_msg(object, motion_model_mapping);
746 return detection_list;
750 -> carma_perception_msgs::msg::ExternalObject
756 const auto to_numeric_id = [](std::string string_id) -> std::optional<uint32_t> {
757 auto non_digit_start = std::remove_if(
758 std::begin(string_id), std::end(string_id),
759 [](
const auto & ch) {
return !std::isdigit(ch); });
761 std::uint32_t numeric_id;
762 const auto digit_substr_size{std::distance(std::begin(string_id), non_digit_start)};
764 std::from_chars(string_id.c_str(), string_id.c_str() + digit_substr_size, numeric_id).ec ==
772 if (
const auto numeric_id{to_numeric_id(track.id)}) {
784 const auto track_longitudinal_velocity{track.twist.twist.linear.x};
785 const auto track_orientation = track.pose.pose.orientation;
788 track_orientation.x, track_orientation.y, track_orientation.z, track_orientation.w);
790 double roll, pitch, yaw;
791 m.getRPY(roll, pitch, yaw);
793 external_object.velocity.twist.linear.x = track_longitudinal_velocity * std::cos(yaw);
794 external_object.velocity.twist.linear.y = track_longitudinal_velocity * std::sin(yaw);
799 switch (track.semantic_class) {
800 case track.SEMANTIC_CLASS_SMALL_VEHICLE:
803 case track.SEMANTIC_CLASS_LARGE_VEHICLE:
806 case track.SEMANTIC_CLASS_MOTORCYCLE:
809 case track.SEMANTIC_CLASS_PEDESTRIAN:
812 case track.SEMANTIC_CLASS_UNKNOWN:
821 const carma_cooperative_perception_interfaces::msg::TrackList & track_list)
822 -> carma_perception_msgs::msg::ExternalObjectList
824 carma_perception_msgs::msg::ExternalObjectList external_object_list;
826 for (
const auto & track : track_list.tracks) {
830 return external_object_list;
834 const carma_perception_msgs::msg::ExternalObjectList & external_object_list,
835 const geometry_msgs::msg::PoseStamped & current_pose,
836 const std::shared_ptr<lanelet::projection::LocalFrameProjector> & map_projection)
837 -> carma_v2x_msgs::msg::SensorDataSharingMessage
839 carma_v2x_msgs::msg::SensorDataSharingMessage sdsm;
840 carma_v2x_msgs::msg::DetectedObjectList detected_object_list;
851 sdsm_detected_object.detected_object_common_data.measurement_time =
855 sdsm_detected_object.detected_object_common_data.pos =
858 detected_object_list.detected_object_data.push_back(sdsm_detected_object);
861 std::vector<uint8_t>
id = {0x00, 0x00, 0x00, 0x01};
862 sdsm.source_id.id = id;
863 sdsm.equipment_type.equipment_type = j3224_v2x_msgs::msg::EquipmentType::OBU;
864 sdsm.ref_pos_xy_conf.semi_major = j2735_v2x_msgs::msg::PositionalAccuracy::ACCURACY_UNAVAILABLE;
865 sdsm.ref_pos_xy_conf.semi_minor = j2735_v2x_msgs::msg::PositionalAccuracy::ACCURACY_UNAVAILABLE;
866 sdsm.ref_pos_xy_conf.orientation =
867 j2735_v2x_msgs::msg::PositionalAccuracy::ACCURACY_ORIENTATION_UNAVAILABLE;
868 sdsm.objects = detected_object_list;
875 const std::shared_ptr<lanelet::projection::LocalFrameProjector> & map_projection)
876 -> carma_v2x_msgs::msg::DetectedObjectData
878 carma_v2x_msgs::msg::DetectedObjectData detected_object_data;
879 detected_object_data.presence_vector = 0;
881 carma_v2x_msgs::msg::DetectedObjectCommonData detected_object_common_data;
882 detected_object_common_data.presence_vector = 0;
888 detected_object_common_data.obj_type_cfd.classification_confidence =
894 detected_object_common_data.detected_id.object_id =
904 detected_object_common_data.pos.offset_x.object_distance =
906 detected_object_common_data.pos.offset_y.object_distance =
908 detected_object_common_data.pos.offset_z.object_distance =
914 detected_object_common_data.speed.speed =
917 detected_object_common_data.presence_vector |=
918 carma_v2x_msgs::msg::DetectedObjectCommonData::HAS_SPEED_Z;
919 detected_object_common_data.speed_z.speed =
external_object.velocity.twist.linear.z;
922 lanelet::BasicPoint3d external_object_position{
927 tf2::Quaternion q(obj_orientation.x, obj_orientation.y, obj_orientation.z, obj_orientation.w);
929 double roll, pitch, yaw;
930 m.getRPY(roll, pitch, yaw);
932 detected_object_common_data.heading.heading =
938 carma_v2x_msgs::msg::DetectedObjectOptionalData detected_object_optional_data;
942 detected_object_common_data.obj_type.object_type = j3224_v2x_msgs::msg::ObjectType::VEHICLE;
945 detected_object_optional_data.det_veh.presence_vector =
946 carma_v2x_msgs::msg::DetectedVehicleData::HAS_SIZE;
947 detected_object_optional_data.det_veh.presence_vector |=
948 carma_v2x_msgs::msg::DetectedVehicleData::HAS_HEIGHT;
950 detected_object_optional_data.det_veh.size.vehicle_width =
external_object.size.y;
951 detected_object_optional_data.det_veh.size.vehicle_length =
external_object.size.x;
952 detected_object_optional_data.det_veh.height.vehicle_height =
external_object.size.z;
956 detected_object_common_data.obj_type.object_type = j3224_v2x_msgs::msg::ObjectType::VEHICLE;
959 detected_object_optional_data.det_veh.presence_vector =
960 carma_v2x_msgs::msg::DetectedVehicleData::HAS_SIZE;
961 detected_object_optional_data.det_veh.presence_vector |=
962 carma_v2x_msgs::msg::DetectedVehicleData::HAS_HEIGHT;
964 detected_object_optional_data.det_veh.size.vehicle_width =
external_object.size.y;
965 detected_object_optional_data.det_veh.size.vehicle_length =
external_object.size.x;
966 detected_object_optional_data.det_veh.height.vehicle_height =
external_object.size.z;
970 detected_object_common_data.obj_type.object_type = j3224_v2x_msgs::msg::ObjectType::VEHICLE;
973 detected_object_optional_data.det_veh.presence_vector =
974 carma_v2x_msgs::msg::DetectedVehicleData::HAS_SIZE;
975 detected_object_optional_data.det_veh.presence_vector |=
976 carma_v2x_msgs::msg::DetectedVehicleData::HAS_HEIGHT;
978 detected_object_optional_data.det_veh.size.vehicle_width =
external_object.size.y;
979 detected_object_optional_data.det_veh.size.vehicle_length =
external_object.size.x;
980 detected_object_optional_data.det_veh.height.vehicle_height =
external_object.size.z;
984 detected_object_common_data.obj_type.object_type = j3224_v2x_msgs::msg::ObjectType::VRU;
986 detected_object_optional_data.det_vru.presence_vector =
987 carma_v2x_msgs::msg::DetectedVRUData::HAS_BASIC_TYPE;
988 detected_object_optional_data.det_vru.basic_type.type |=
989 j2735_v2x_msgs::msg::PersonalDeviceUserType::A_PEDESTRIAN;
994 detected_object_common_data.obj_type.object_type = j3224_v2x_msgs::msg::ObjectType::UNKNOWN;
997 detected_object_optional_data.det_obst.obst_size.width.size_value =
external_object.size.y;
998 detected_object_optional_data.det_obst.obst_size.length.size_value =
external_object.size.x;
1000 detected_object_optional_data.det_obst.obst_size.presence_vector =
1001 carma_v2x_msgs::msg::ObstacleSize::HAS_HEIGHT;
1002 detected_object_optional_data.det_obst.obst_size.height.size_value =
external_object.size.z;
1006 detected_object_data.detected_object_common_data = std::move(detected_object_common_data);
1007 detected_object_data.detected_object_optional_data = std::move(detected_object_optional_data);
1009 return detected_object_data;
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
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 to_external_object_list_msg(const carma_cooperative_perception_interfaces::msg::TrackList &track_list) -> carma_perception_msgs::msg::ExternalObjectList
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
std::string to_string(const std::vector< std::uint8_t > &temporary_id)
auto calculate_grid_convergence(const Wgs84Coordinate &position, const UtmZone &zone) -> units::angle::degree_t
Calculate grid convergence at a given position.
auto to_ddate_time_msg(const builtin_interfaces::msg::Time &builtin_time) -> j2735_v2x_msgs::msg::DDateTime
auto calc_detection_time_stamp(DDateTime d_date_time, const MeasurementTimeOffset &offset) -> DDateTime
std::variant< multiple_object_tracking::CtrvDetection, multiple_object_tracking::CtraDetection > Detection
auto project_to_carma_map(const Wgs84Coordinate &coordinate, std::string_view proj_string) -> MapCoordinate
auto to_string(const UtmZone &zone) -> std::string
auto to_detection_msg(const carma_perception_msgs::msg::ExternalObject &object, const MotionModelMapping &motion_model_mapping) -> carma_cooperative_perception_interfaces::msg::Detection
auto to_time_msg(const DDateTime &d_date_time, bool is_simulation) -> builtin_interfaces::msg::Time
auto ned_to_enu(const PositionOffsetXYZ &offset_ned) noexcept
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
void convert_object_type(carma_cooperative_perception_interfaces::msg::Detection &detection, const j3224_v2x_msgs::msg::ObjectType &j3224_obj_type)
auto to_external_object_msg(const carma_cooperative_perception_interfaces::msg::Track &track) -> carma_perception_msgs::msg::ExternalObject
auto heading_to_enu_yaw(const units::angle::degree_t &heading) -> units::angle::degree_t
auto calc_relative_position(const geometry_msgs::msg::PoseStamped ¤t_pose, const carma_v2x_msgs::msg::PositionOffsetXYZ &detected_object_data) -> carma_v2x_msgs::msg::PositionOffsetXYZ
auto to_sdsm_msg(const carma_perception_msgs::msg::ExternalObjectList &external_object_list, const geometry_msgs::msg::PoseStamped ¤t_pose, const std::shared_ptr< lanelet::projection::LocalFrameProjector > &map_projection) -> carma_v2x_msgs::msg::SensorDataSharingMessage
constexpr auto remove_units(const T &value)
auto to_position_msg(const UtmCoordinate &position_utm) -> geometry_msgs::msg::Point
std::variant< multiple_object_tracking::CtrvTrack, multiple_object_tracking::CtraTrack > Track
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_inter...
static auto from_msg(const j2735_v2x_msgs::msg::AccelerationSet4Way &msg) -> AccelerationSet4Way
std::optional< units::time::millisecond_t > second
static auto from_msg(const j2735_v2x_msgs::msg::DDateTime &msg) -> DDateTime
static auto from_msg(const j2735_v2x_msgs::msg::Heading &heading) -> Heading
units::length::meter_t easting
static auto from_msg(const j3224_v2x_msgs::msg::MeasurementTimeOffset &msg) -> MeasurementTimeOffset
static auto from_msg(const j2735_v2x_msgs::msg::Position3D &msg) -> Position3D
static auto from_msg(const j3224_v2x_msgs::msg::PositionOffsetXYZ &msg) -> PositionOffsetXYZ
static auto from_msg(const j2735_v2x_msgs::msg::Speed &speed) -> Speed
Represents a position using UTM coordinates.
Represents a position using WGS-84 coordinates.
units::angle::degree_t latitude