feat(ars548): implemented universal radar messages proposal

build_depends.repos

@@ -9,3 +9,8 @@ repositories:
     type: git
     url: https://github.com/autowarefoundation/ros2_socketcan
     version: main
+  # Autoware messages for the universal radar messages
+  autoware_msgs:
+    type: git
+    url: https://github.com/autowarefoundation/autoware_msgs
+    version: main

nebula_common/include/nebula_common/continental/continental_ars548.hpp

@@ -46,6 +46,7 @@ struct ContinentalARS548SensorConfiguration : EthernetSensorConfigurationBase
   uint16_t configuration_host_port{};
   uint16_t configuration_sensor_port{};
   bool use_sensor_time{};
+  int radar_info_rate_subsample{};
   float configuration_vehicle_length{};
   float configuration_vehicle_width{};
   float configuration_vehicle_height{};
@@ -68,6 +69,7 @@ inline std::ostream & operator<<(
   os << "Host Port: " << arg.configuration_host_port << '\n';
   os << "Sensor Port: " << arg.configuration_sensor_port << '\n';
   os << "UseSensor Time: " << arg.use_sensor_time << '\n';
+  os << "RadarInfoRateSubsample: " << arg.radar_info_rate_subsample << '\n';
   os << "Vehicle Length: " << arg.configuration_vehicle_length << '\n';
   os << "Vehicle Width: " << arg.configuration_vehicle_width << '\n';
   os << "Vehicle Height: " << arg.configuration_vehicle_height << '\n';
@@ -292,6 +294,74 @@ constexpr int blockage_test_ongoing = 2;
 constexpr int min_odometry_hz = 10;
 constexpr int max_odometry_hz = 50;
 
+struct FieldInfo
+{
+  FieldInfo(
+    bool min_value_available, bool max_value_available, bool resolution_available, float min_value,
+    float max_value, float resolution)
+  : min_value_available(min_value_available),
+    max_value_available(max_value_available),
+    resolution_available(resolution_available),
+    min_value(min_value),
+    max_value(max_value),
+    resolution(resolution)
+  {
+  }
+  bool min_value_available;
+  bool max_value_available;
+  bool resolution_available;
+  float min_value;
+  float max_value;
+  float resolution;
+};
+
+// Detection field infos
+const FieldInfo azimuth_info{true, true, false, -M_PI, M_PI, 0.f};
+const FieldInfo azimuth_std_info{true, true, false, 0.f, 1.f, 0.f};
+const FieldInfo elevation_info{true, true, false, -M_PI, M_PI, 0.f};
+const FieldInfo elevation_std_info{true, true, false, 0.f, 1.f, 0.f};
+
+const FieldInfo range_info{true, true, false, 0.f, 301.f, 0.f};
+const FieldInfo range_std_info{true, true, false, 0.f, 1.f, 0.f};
+const FieldInfo range_rate_info{true, true, false, -100.f, 100.f, 0.f};
+const FieldInfo range_rate_std_info{true, true, false, 0.f, 1.f, 0.f};
+
+const FieldInfo rcs_info{true, true, true, -128.f, 127.f, 1.f};
+const FieldInfo measurement_id_info{true, true, true, 0.f, 65535.f, 1.f};
+const FieldInfo positive_predictive_value_info{true, true, true, 0.f, 100.f, 1.f};
+const FieldInfo classification_info{true, true, true, 0.f, 255.f, 1.f};
+const FieldInfo multi_target_probability_info{true, true, true, 0.f, 100.f, 1.f};
+const FieldInfo object_id_info{true, true, false, 0.f, 65535.f, 1.f};
+const FieldInfo ambiguity_flag_info{true, true, true, 0.f, 100.f, 1.f};
+
+// Object field infos
+const FieldInfo age_info{true, true, true, 0.f, 65535.f, 1.f};
+const FieldInfo measurement_status_info{true, true, true, 0.f, 255.f, 1.f};
+const FieldInfo movement_status_info{true, true, true, 0.f, 255.f, 1.f};
+
+const FieldInfo position_x_info{true, true, false, -1600.f, 1600.f, 0.f};
+const FieldInfo position_y_info{true, true, false, -1600.f, 1600.f, 0.f};
+const FieldInfo position_z_info{true, true, false, -1600.f, 1600.f, 0.f};
+
+const FieldInfo velocity_x_info{false, false, false, 0.f, 0.f, 0.f};
+const FieldInfo velocity_y_info{false, false, false, 0.f, 0.f, 0.f};
+const FieldInfo velocity_z_info{false, false, false, 0.f, 0.f, 0.f};
+
+const FieldInfo acceleration_x_info{false, false, false, 0.f, 0.f, 0.f};
+const FieldInfo acceleration_y_info{false, false, false, 0.f, 0.f, 0.f};
+const FieldInfo acceleration_z_info{false, false, false, 0.f, 0.f, 0.f};
+
+const FieldInfo shape_x_info{false, false, false, 0.f, 0.f, 0.f};
+const FieldInfo shape_y_info{false, false, false, 0.f, 0.f, 0.f};
+const FieldInfo shape_z_info{false, false, false, 0.f, 0.f, 0.f};
+
+const FieldInfo orientation_info{true, true, false, -M_PI, M_PI, 0.f};
+const FieldInfo orientation_std_info{false, false, false, 0.f, 0.f, 0.f};
+const FieldInfo orientation_rate_info{false, false, false, -M_PI, M_PI, 0.f};
+const FieldInfo orientation_rate_std_info{false, false, false, 0.f, 0.f, 0.f};
+
+const FieldInfo existence_probability_info{true, true, true, 0.f, 100.f, 1.f};
+
 #pragma pack(push, 1)
 
 struct HeaderPacket

nebula_ros/CMakeLists.txt

@@ -12,6 +12,7 @@ endif ()
 
 find_package(ament_cmake_auto REQUIRED)
 find_package(PCL REQUIRED components common)
+find_package(autoware_sensing_msgs REQUIRED)
 find_package(continental_msgs REQUIRED)
 find_package(continental_srvs REQUIRED)
 find_package(diagnostic_msgs REQUIRED)
@@ -144,6 +145,7 @@ add_library(continental_ars548_ros_wrapper SHARED
 )
 
 target_include_directories(continental_ars548_ros_wrapper PUBLIC
+    ${autoware_sensing_msgs_INCLUDE_DIRS}
     ${continental_msgs_INCLUDE_DIRS}
     ${continental_srvs_INCLUDE_DIRS}
     ${diagnostic_msgs_INCLUDE_DIRS}
@@ -158,6 +160,7 @@ target_include_directories(continental_ars548_ros_wrapper PUBLIC
 )
 
 target_link_libraries(continental_ars548_ros_wrapper PUBLIC
+    ${autoware_sensing_msgs_TARGETS}
     ${continental_msgs_TARGETS}
     ${continental_srvs_TARGETS}
     ${diagnostic_msgs_TARGETS}

nebula_ros/config/radar/continental/ARS548.param.yaml

@@ -12,6 +12,7 @@
     configuration_host_port: 42401
     configuration_sensor_port: 42101
     use_sensor_time: false
+    radar_info_rate_subsample: 10
     configuration_vehicle_length: 4.89
     configuration_vehicle_width: 1.896
     configuration_vehicle_height: 2.5

nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp

@@ -23,11 +23,15 @@
 #include <nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include <autoware_sensing_msgs/msg/radar_classification.hpp>
+#include <autoware_sensing_msgs/msg/radar_info.hpp>
+#include <autoware_sensing_msgs/msg/radar_objects.hpp>
 #include <continental_msgs/msg/continental_ars548_detection.hpp>
 #include <continental_msgs/msg/continental_ars548_detection_list.hpp>
 #include <continental_msgs/msg/continental_ars548_object.hpp>
 #include <continental_msgs/msg/continental_ars548_object_list.hpp>
 #include <diagnostic_msgs/msg/diagnostic_array.hpp>
+#include <geometry_msgs/msg/vector3.hpp>
 #include <nebula_msgs/msg/nebula_packet.hpp>
 #include <nebula_msgs/msg/nebula_packets.hpp>
 #include <radar_msgs/msg/radar_scan.hpp>
@@ -87,6 +91,28 @@ class ContinentalARS548DecoderWrapper
     const std::shared_ptr<
       const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration> & config);
 
+  // @brief Create a RadarInfo message for the ARS548 radar
+  // @return RadarInfo message
+  void create_radar_info();
+
+  // @brief Convert an ARS548 reference point to the center of an object
+  // @return Thee center of the object
+  geometry_msgs::msg::Point reference_point_to_center(
+    const geometry_msgs::msg::Point & reference_point, double yaw, double length, double width,
+    int reference_index);
+
+  /// @brief Convert ARS548 detections to a autoware's radar PointCloud2 msg
+  /// @param msg The ARS548 objects list msg
+  /// @return Resulting PointCloud2 msg
+  sensor_msgs::msg::PointCloud2 convert_to_autoware_radar_detections(
+    const continental_msgs::msg::ContinentalArs548DetectionList & msg);
+
+  /// @brief Convert ARS548 objects to a autoware's PointCloud2 msg
+  /// @param msg The ARS548 objects list msg
+  /// @return Resulting RadarObjects msg
+  autoware_sensing_msgs::msg::RadarObjects convert_to_autoware_radar_objects(
+    const continental_msgs::msg::ContinentalArs548ObjectList & msg);
+
   /// @brief Convert ARS548 detections to a pointcloud
   /// @param msg The ARS548 detection list msg
   /// @return Resulting detection pointcloud
@@ -143,15 +169,20 @@ class ContinentalARS548DecoderWrapper
     object_list_pub_{};
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr object_pointcloud_pub_{};
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr detection_pointcloud_pub_{};
+  rclcpp::Publisher<autoware_sensing_msgs::msg::RadarObjects>::SharedPtr autoware_objects_pub_{};
   rclcpp::Publisher<radar_msgs::msg::RadarScan>::SharedPtr scan_raw_pub_{};
   rclcpp::Publisher<radar_msgs::msg::RadarTracks>::SharedPtr objects_raw_pub_{};
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr objects_markers_pub_{};
   rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diagnostics_pub_{};
+  rclcpp::Publisher<autoware_sensing_msgs::msg::RadarInfo>::SharedPtr radar_info_pub_{};
+
+  autoware_sensing_msgs::msg::RadarInfo radar_info_msg_{};
+  std::size_t detection_msgs_counter_{0};
 
   std::unordered_set<int> previous_ids_{};
 
   constexpr static int reference_points_num = 9;
-  constexpr static std::array<std::array<double, 2>, reference_points_num> reference_to_center = {
+  constexpr static std::array<std::array<double, 2>, reference_points_num> reference_to_center_ = {
     {{{-1.0, -1.0}},
      {{-1.0, 0.0}},
      {{-1.0, 1.0}},

nebula_ros/package.xml

@@ -15,6 +15,7 @@
   <build_depend>libpcl-all-dev</build_depend>
   <build_export_depend>libpcl-all-dev</build_export_depend>
 
+  <depend>autoware_sensing_msgs</depend>
   <depend>continental_msgs</depend>
   <depend>continental_srvs</depend>
   <depend>diagnostic_msgs</depend>

nebula_ros/schema/ARS548.schema.json

@@ -39,6 +39,9 @@
         "use_sensor_time": {
           "$ref": "sub/topic.json#/definitions/use_sensor_time"
         },
+        "radar_info_rate_subsample": {
+          "$ref": "sub/topic.json#/definitions/radar_info_rate_subsample"
+        },
         "configuration_vehicle_length": {
           "$ref": "sub/misc.json#/definitions/configuration_vehicle_length"
         },

nebula_ros/schema/sub/topic.json

@@ -45,6 +45,12 @@
       "default": "false",
       "readOnly": true,
       "description": "Use sensor time for published sensor data."
+    },
+    "radar_info_rate_subsample": {
+      "type": "integer",
+      "default": "10",
+      "readOnly": true,
+      "description": "Subsample factor to publish radar info messages with respect to the radar detections."
     }
   }
 }