feat(continental_ars548): combined continental ars548 ros wrappers into a single node

nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp

@@ -1,4 +1,4 @@
-// Copyright 2024 Tier IV, Inc.
+// Copyright 2024 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -22,7 +22,6 @@
 #include <diagnostic_msgs/msg/diagnostic_array.hpp>
 #include <nebula_msgs/msg/nebula_packet.hpp>
 #include <nebula_msgs/msg/nebula_packets.hpp>
-#include <std_msgs/msg/header.hpp>
 
 #include <array>
 #include <memory>
@@ -41,30 +40,16 @@ class ContinentalARS548Decoder : public ContinentalPacketsDecoder
   /// @brief Constructor
   /// @param sensor_configuration SensorConfiguration for this decoder
   explicit ContinentalARS548Decoder(
-    const std::shared_ptr<ContinentalARS548SensorConfiguration> & sensor_configuration);
+    const std::shared_ptr<const ContinentalARS548SensorConfiguration> & sensor_configuration);
+
+  /// @brief Get current status of this driver
+  /// @return Current status
+  Status GetStatus() override;
 
   /// @brief Function for parsing NebulaPackets
   /// @param nebula_packets
   /// @return Resulting flag
-  bool ProcessPackets(const nebula_msgs::msg::NebulaPackets & nebula_packets) override;
-
-  /// @brief Function for parsing detection lists
-  /// @param data
-  /// @return Resulting flag
-  bool ParseDetectionsListPacket(
-    const std::vector<uint8_t> & data, const std_msgs::msg::Header & header);
-
-  /// @brief Function for parsing object lists
-  /// @param data
-  /// @return Resulting flag
-  bool ParseObjectsListPacket(
-    const std::vector<uint8_t> & data, const std_msgs::msg::Header & header);
-
-  /// @brief Function for parsing sensor status messages
-  /// @param data
-  /// @return Resulting flag
-  bool ParseSensorStatusPacket(
-    const std::vector<uint8_t> & data, const std_msgs::msg::Header & header);
+  bool ProcessPacket(std::unique_ptr<nebula_msgs::msg::NebulaPacket> packet_msg) override;
 
   /// @brief Register function to call when a new detection list is processed
   /// @param detection_list_callback
@@ -86,17 +71,40 @@ class ContinentalARS548Decoder : public ContinentalPacketsDecoder
   Status RegisterSensorStatusCallback(
     std::function<void(const ContinentalARS548Status & status)> sensor_status_callback);
 
+  /// @brief Register function to call when a new sensor status message is processed
+  /// @param object_list_callback
+  /// @return Resulting status
+  Status RegisterPacketsCallback(
+    std::function<void(std::unique_ptr<nebula_msgs::msg::NebulaPackets>)> packets_callback);
+
 private:
+  /// @brief Function for parsing detection lists
+  /// @param data
+  /// @return Resulting flag
+  bool ParseDetectionsListPacket(const nebula_msgs::msg::NebulaPacket & packet_msg);
+
+  /// @brief Function for parsing object lists
+  /// @param data
+  /// @return Resulting flag
+  bool ParseObjectsListPacket(const nebula_msgs::msg::NebulaPacket & packet_msg);
+
+  /// @brief Function for parsing sensor status messages
+  /// @param data
+  /// @return Resulting flag
+  bool ParseSensorStatusPacket(const nebula_msgs::msg::NebulaPacket & packet_msg);
+
   std::function<void(std::unique_ptr<continental_msgs::msg::ContinentalArs548DetectionList> msg)>
-    detection_list_callback_;
+    detection_list_callback_{};
   std::function<void(std::unique_ptr<continental_msgs::msg::ContinentalArs548ObjectList> msg)>
-    object_list_callback_;
-  std::function<void(const ContinentalARS548Status & status)> sensor_status_callback_;
+    object_list_callback_{};
+  std::function<void(const ContinentalARS548Status & status)> sensor_status_callback_{};
+  std::function<void(std::unique_ptr<nebula_msgs::msg::NebulaPackets> msg)>
+    nebula_packets_callback_{};
 
   ContinentalARS548Status radar_status_{};
 
   /// @brief SensorConfiguration for this decoder
-  std::shared_ptr<continental_ars548::ContinentalARS548SensorConfiguration> sensor_configuration_;
+  std::shared_ptr<const continental_ars548::ContinentalARS548SensorConfiguration> config_ptr_{};
 };
 
 }  // namespace continental_ars548

nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp

@@ -1,4 +1,4 @@
-// Copyright 2024 Tier IV, Inc.
+// Copyright 2024 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -15,10 +15,9 @@
 #ifndef NEBULA_WS_CONTINENTAL_PACKETS_DECODER_HPP
 #define NEBULA_WS_CONTINENTAL_PACKETS_DECODER_HPP
 
-#include "nebula_common/point_types.hpp"
+#include <nebula_common/point_types.hpp>
 
-#include "nebula_msgs/msg/nebula_packet.hpp"
-#include "nebula_msgs/msg/nebula_packets.hpp"
+#include <nebula_msgs/msg/nebula_packet.hpp>
 
 #include <memory>
 #include <tuple>
@@ -39,10 +38,14 @@ class ContinentalPacketsDecoder
   virtual ~ContinentalPacketsDecoder() = default;
   ContinentalPacketsDecoder() = default;
 
-  /// @brief Virtual function for parsing NebulaPackets based on packet structure
-  /// @param nebula_packets
+  /// @brief Get current status of this driver
+  /// @return Current status
+  virtual Status GetStatus() = 0;
+
+  /// @brief Virtual function for parsing a NebulaPacket
+  /// @param packet_msg
   /// @return Resulting flag
-  virtual bool ProcessPackets(const nebula_msgs::msg::NebulaPackets & nebula_packets) = 0;
+  virtual bool ProcessPacket(std::unique_ptr<nebula_msgs::msg::NebulaPacket> packet_msg) = 0;
 };
 }  // namespace drivers
 }  // namespace nebula

nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp

@@ -1,4 +1,4 @@
-// Copyright 2024 Tier IV, Inc.
+// Copyright 2024 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -12,9 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp"
-
-#include "nebula_common/continental/continental_ars548.hpp"
+#include <nebula_common/continental/continental_ars548.hpp>
+#include <nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp>
 
 #include <cmath>
 #include <utility>
@@ -26,10 +25,15 @@ namespace drivers
 namespace continental_ars548
 {
 ContinentalARS548Decoder::ContinentalARS548Decoder(
-  const std::shared_ptr<continental_ars548::ContinentalARS548SensorConfiguration> &
+  const std::shared_ptr<const continental_ars548::ContinentalARS548SensorConfiguration> &
     sensor_configuration)
 {
-  sensor_configuration_ = sensor_configuration;
+  config_ptr_ = sensor_configuration;
+}
+
+Status ContinentalARS548Decoder::GetStatus()
+{
+  return Status::OK;
 }
 
 Status ContinentalARS548Decoder::RegisterDetectionListCallback(
@@ -55,14 +59,17 @@ Status ContinentalARS548Decoder::RegisterSensorStatusCallback(
   return Status::OK;
 }
 
-bool ContinentalARS548Decoder::ProcessPackets(
-  const nebula_msgs::msg::NebulaPackets & nebula_packets)
+Status ContinentalARS548Decoder::RegisterPacketsCallback(
+  std::function<void(std::unique_ptr<nebula_msgs::msg::NebulaPackets>)> nebula_packets_callback)
 {
-  if (nebula_packets.packets.size() != 1) {
-    return false;
-  }
+  nebula_packets_callback_ = std::move(nebula_packets_callback);
+  return Status::OK;
+}
 
-  const auto & data = nebula_packets.packets[0].data;
+bool ContinentalARS548Decoder::ProcessPacket(
+  std::unique_ptr<nebula_msgs::msg::NebulaPacket> packet_msg)
+{
+  const auto & data = packet_msg->data;
 
   if (data.size() < sizeof(HeaderPacket)) {
     return false;
@@ -82,44 +89,53 @@ bool ContinentalARS548Decoder::ProcessPackets(
       return false;
     }
 
-    return ParseDetectionsListPacket(data, nebula_packets.header);
+    ParseDetectionsListPacket(*packet_msg);
   } else if (header.method_id.value() == OBJECT_LIST_METHOD_ID) {
     if (data.size() != OBJECT_LIST_UDP_PAYLOAD || header.length.value() != OBJECT_LIST_PDU_LENGTH) {
       return false;
     }
 
-    return ParseObjectsListPacket(data, nebula_packets.header);
+    ParseObjectsListPacket(*packet_msg);
   } else if (header.method_id.value() == SENSOR_STATUS_METHOD_ID) {
     if (
       data.size() != SENSOR_STATUS_UDP_PAYLOAD ||
       header.length.value() != SENSOR_STATUS_PDU_LENGTH) {
       return false;
     }
 
-    return ParseSensorStatusPacket(data, nebula_packets.header);
+    ParseSensorStatusPacket(*packet_msg);
+  }
+
+  // Some messages are not parsed but are still sent to the user (e.g., filters)
+  if (nebula_packets_callback_) {
+    auto packets_msg = std::make_unique<nebula_msgs::msg::NebulaPackets>();
+    packets_msg->packets.emplace_back(std::move(*packet_msg));
+    packets_msg->header.stamp = packet_msg->stamp;
+    packets_msg->header.frame_id = config_ptr_->frame_id;
+    nebula_packets_callback_(std::move(packets_msg));
   }
 
   return true;
 }
 
 bool ContinentalARS548Decoder::ParseDetectionsListPacket(
-  const std::vector<uint8_t> & data, const std_msgs::msg::Header & header)
+  const nebula_msgs::msg::NebulaPacket & packet_msg)
 {
   auto msg_ptr = std::make_unique<continental_msgs::msg::ContinentalArs548DetectionList>();
   auto & msg = *msg_ptr;
 
   DetectionListPacket detection_list;
-  assert(sizeof(DetectionListPacket) == data.size());
+  assert(sizeof(DetectionListPacket) == packet_msg.data.size());
 
-  std::memcpy(&detection_list, data.data(), sizeof(DetectionListPacket));
+  std::memcpy(&detection_list, packet_msg.data.data(), sizeof(DetectionListPacket));
 
-  msg.header.frame_id = sensor_configuration_->frame_id;
+  msg.header.frame_id = config_ptr_->frame_id;
 
-  if (sensor_configuration_->use_sensor_time) {
+  if (config_ptr_->use_sensor_time) {
     msg.header.stamp.nanosec = detection_list.stamp.timestamp_nanoseconds.value();
     msg.header.stamp.sec = detection_list.stamp.timestamp_seconds.value();
   } else {
-    msg.header.stamp = header.stamp;
+    msg.header.stamp = packet_msg.stamp;
   }
 
   msg.stamp_sync_status = detection_list.stamp.timestamp_sync_status;
@@ -222,29 +238,31 @@ bool ContinentalARS548Decoder::ParseDetectionsListPacket(
     detection_msg.range_rate_std = detection.range_rate_std.value();
   }
 
-  detection_list_callback_(std::move(msg_ptr));
+  if (detection_list_callback_) {
+    detection_list_callback_(std::move(msg_ptr));
+  }
 
   return true;
 }
 
 bool ContinentalARS548Decoder::ParseObjectsListPacket(
-  const std::vector<uint8_t> & data, const std_msgs::msg::Header & header)
+  const nebula_msgs::msg::NebulaPacket & packet_msg)
 {
   auto msg_ptr = std::make_unique<continental_msgs::msg::ContinentalArs548ObjectList>();
   auto & msg = *msg_ptr;
 
   ObjectListPacket object_list;
-  assert(sizeof(ObjectListPacket) == data.size());
+  assert(sizeof(ObjectListPacket) == packet_msg.data.size());
 
-  std::memcpy(&object_list, data.data(), sizeof(object_list));
+  std::memcpy(&object_list, packet_msg.data.data(), sizeof(object_list));
 
-  msg.header.frame_id = sensor_configuration_->object_frame;
+  msg.header.frame_id = config_ptr_->object_frame;
 
-  if (sensor_configuration_->use_sensor_time) {
+  if (config_ptr_->use_sensor_time) {
     msg.header.stamp.nanosec = object_list.stamp.timestamp_nanoseconds.value();
     msg.header.stamp.sec = object_list.stamp.timestamp_seconds.value();
   } else {
-    msg.header.stamp = header.stamp;
+    msg.header.stamp = packet_msg.stamp;
   }
 
   msg.stamp_sync_status = object_list.stamp.timestamp_sync_status;
@@ -374,16 +392,18 @@ bool ContinentalARS548Decoder::ParseObjectsListPacket(
     object_msg.shape_width_edge_mean = object.shape_width_edge_mean.value();
   }
 
-  object_list_callback_(std::move(msg_ptr));
+  if (object_list_callback_) {
+    object_list_callback_(std::move(msg_ptr));
+  }
 
   return true;
 }
 
 bool ContinentalARS548Decoder::ParseSensorStatusPacket(
-  const std::vector<uint8_t> & data, [[maybe_unused]] const std_msgs::msg::Header & header)
+  const nebula_msgs::msg::NebulaPacket & packet_msg)
 {
   SensorStatusPacket sensor_status_packet;
-  std::memcpy(&sensor_status_packet, data.data(), sizeof(SensorStatusPacket));
+  std::memcpy(&sensor_status_packet, packet_msg.data.data(), sizeof(SensorStatusPacket));
 
   radar_status_.timestamp_nanoseconds = sensor_status_packet.stamp.timestamp_nanoseconds.value();
   radar_status_.timestamp_seconds = sensor_status_packet.stamp.timestamp_seconds.value();
@@ -613,7 +633,9 @@ bool ContinentalARS548Decoder::ParseSensorStatusPacket(
   radar_status_.status_total_count++;
   radar_status_.radar_invalid_count += sensor_status_packet.radar_status == 2 ? 1 : 0;
 
-  sensor_status_callback_(radar_status_);
+  if (sensor_status_callback_) {
+    sensor_status_callback_(radar_status_);
+  }
 
   return true;
 }

nebula_hw_interfaces/CMakeLists.txt

@@ -40,7 +40,6 @@ ament_auto_add_library(nebula_hw_interfaces_robosense SHARED
 
 ament_auto_add_library(nebula_hw_interfaces_continental SHARED
         src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp
-        src/nebula_continental_hw_interfaces/multi_continental_ars548_hw_interface.cpp
         )
 
 

nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp

@@ -1,10 +1,11 @@
 #ifndef NEBULA_HW_INTERFACE_BASE_H
 #define NEBULA_HW_INTERFACE_BASE_H
 
+#include "boost_udp_driver/udp_driver.hpp"
 #include "nebula_common/nebula_common.hpp"
 #include "nebula_common/nebula_status.hpp"
-#include "boost_udp_driver/udp_driver.hpp"
 
+#include <memory>
 #include <stdexcept>
 #include <string>
 #include <vector>
@@ -22,7 +23,7 @@ class NebulaHwInterfaceBase
    * @param buffer Buffer containing the data received from the UDP socket
    * @return Status::OK if no error occurred.
    */
-  virtual void ReceiveSensorPacketCallback([[maybe_unused]] const std::vector<uint8_t> & buffer) {}
+  virtual void ReceiveSensorPacketCallback([[maybe_unused]] std::vector<uint8_t> & buffer) {}
   //  virtual Status RegisterScanCallback(
   //    std::function<void(std::unique_ptr<std::vector<std::vector<uint8_t>>>)> scan_callback) = 0;