feat: missing features for proper vehicle integration. fixed fixes an…

…d added visualization markers

Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]>

nebula_common/include/nebula_common/continental/continental_ars548.hpp

@@ -375,6 +375,8 @@ struct PointARS548Detection
   float elevation_std;
   float range;
   float range_std;
+  float range_rate;
+  float range_rate_std;
   int8_t rcs;
   uint16_t measurement_id;
   uint8_t positive_predictive_value;

nebula_common/include/nebula_common/continental/continental_common.hpp

@@ -34,6 +34,7 @@ struct ContinentalARS548SensorConfiguration : EthernetSensorConfigurationBase
 {
   std::string multicast_ip{};
   std::string new_sensor_ip{};
+  std::string base_frame{};
   uint16_t configuration_host_port{};
   uint16_t configuration_sensor_port{};
   bool use_sensor_time{};
@@ -59,7 +60,7 @@ inline std::ostream & operator<<(
   std::ostream & os, ContinentalARS548SensorConfiguration const & arg)
 {
   os << (EthernetSensorConfigurationBase)(arg) << ", MulticastIP: " << arg.multicast_ip
-     << ", NewSensorIP: " << arg.new_sensor_ip
+     << ", NewSensorIP: " << arg.new_sensor_ip << ", BaseFrame: " << arg.base_frame
      << ", ConfigurationHostPort: " << arg.configuration_host_port
      << ", ConfigurationSensorPort: " << arg.configuration_sensor_port
      << ", UseSensorTime: " << arg.use_sensor_time

nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp

@@ -204,8 +204,7 @@ bool ContinentalARS548Decoder::ParseObjectsListPacket(
 
   std::memcpy(&object_list, data.data(), sizeof(object_list));
 
-  // msg.header.frame_id = sensor_configuration_->frame_id;
-  msg.header.frame_id = "base_link";
+  msg.header.frame_id = sensor_configuration_->base_frame;
 
   if (sensor_configuration_->use_sensor_time) {
     msg.header.stamp.nanosec = object_list.stamp.timestamp_nanoseconds.value();
@@ -268,7 +267,7 @@ bool ContinentalARS548Decoder::ParseObjectsListPacket(
 
     object_msg.position.x = static_cast<double>(object.position_x.value());
     object_msg.position.y = static_cast<double>(object.position_y.value());
-    object_msg.position.y = static_cast<double>(object.position_z.value());
+    object_msg.position.z = static_cast<double>(object.position_z.value());
 
     object_msg.position_std.x = static_cast<double>(object.position_x_std.value());
     object_msg.position_std.y = static_cast<double>(object.position_y_std.value());

nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_ros_wrapper.hpp

@@ -35,9 +35,11 @@
 #include <nebula_msgs/msg/nebula_packets.hpp>
 #include <radar_msgs/msg/radar_scan.hpp>
 #include <radar_msgs/msg/radar_tracks.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
 
 #include <chrono>
 #include <memory>
+#include <unordered_set>
 
 namespace nebula
 {
@@ -58,8 +60,23 @@ class ContinentalARS548DriverRosWrapper final : public rclcpp::Node, NebulaDrive
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr detection_pointcloud_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_;
 
-  std::shared_ptr<drivers::SensorConfigurationBase> sensor_cfg_ptr_;
+  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_ = {
+    {{{-1.0, -1.0}},
+     {{-1.0, 0.0}},
+     {{-1.0, 1.0}},
+     {{0.0, 1.0}},
+     {{1.0, 1.0}},
+     {{1.0, 0.0}},
+     {{1.0, -1.0}},
+     {{0.0, -1.0}},
+     {{0.0, 0.0}}}};
+
+  std::shared_ptr<drivers::ContinentalARS548SensorConfiguration> sensor_cfg_ptr_;
 
   drivers::continental_ars548::ContinentalARS548HwInterface hw_interface_;
 
@@ -128,6 +145,12 @@ class ContinentalARS548DriverRosWrapper final : public rclcpp::Node, NebulaDrive
   /// @return Resulting RadarTracks msg
   radar_msgs::msg::RadarTracks ConvertToRadarTracks(
     const continental_msgs::msg::ContinentalArs548ObjectList & msg);
+
+  /// @brief Convert ARS548 objects to a standard MarkerArray msg
+  /// @param msg The ARS548 object list msg
+  /// @return Resulting MarkerArray msg
+  visualization_msgs::msg::MarkerArray ConvertToMarkers(
+    const continental_msgs::msg::ContinentalArs548ObjectList & msg);
 };
 
 }  // namespace ros

nebula_ros/launch/continental_launch_all_hw.xml

@@ -2,6 +2,7 @@
 <launch>
     <arg name="sensor_model" default="ARS548"/>
     <arg name="frame_id"  default="continental"/>
+    <arg name="base_frame"  default="base_link"/>
 
     <arg name="odometry_topic" default="odometry_input"/>
     <arg name="acceleration_topic"  default="acceleration_input"/>
@@ -31,6 +32,7 @@
     <node pkg="nebula_ros" exec="continental_ars548_driver_ros_wrapper_node" name="continental_ars548_decoder_driver" output="screen">
         <param name="sensor_model" value="$(var sensor_model)"/>
         <param name="frame_id" value="$(var frame_id)"/>
+        <param name="base_frame" value="$(var base_frame)"/>
         <param name="host_ip" value="$(var host_ip)"/>
         <param name="sensor_ip" value="$(var sensor_ip)"/>
         <param name="new_sensor_ip" value="$(var new_sensor_ip)"/>
@@ -59,6 +61,7 @@
 
         <param name="sensor_model" value="$(var sensor_model)"/>
         <param name="frame_id" value="$(var frame_id)"/>
+        <param name="base_frame" value="$(var base_frame)"/>
         <param name="host_ip" value="$(var host_ip)"/>
         <param name="multicast_ip" value="$(var multicast_ip)"/>
         <param name="data_port" value="$(var data_port)"/>

nebula_ros/package.xml

@@ -29,6 +29,7 @@
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
   <depend>velodyne_msgs</depend>
+  <depend>visualization_msgs</depend>
   <depend>yaml-cpp</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>