From a191060027556ddc69620e7b647cd5338fa55256 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Tue, 21 May 2024 15:16:51 +0900 Subject: [PATCH 01/47] feat: refactored the ars548 into the new single node scheme Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental/continental_ars548.hpp | 24 +- .../decoders/continental_ars548_decoder.hpp | 27 +- .../decoders/continental_packets_decoder.hpp | 6 +- .../decoders/continental_ars548_decoder.cpp | 48 +- nebula_hw_interfaces/CMakeLists.txt | 1 - .../nebula_hw_interface_base.hpp | 5 +- .../continental_ars548_hw_interface.hpp | 34 +- .../multi_continental_ars548_hw_interface.hpp | 179 ------ .../continental_ars548_hw_interface.cpp | 105 ++-- .../multi_continental_ars548_hw_interface.cpp | 453 -------------- nebula_ros/CMakeLists.txt | 30 +- ...=> continental_ars548_decoder_wrapper.hpp} | 157 +++-- ...ntinental_ars548_hw_interface_wrapper.hpp} | 138 ++--- .../continental_ars548_ros_wrapper.hpp | 106 ++++ ...nental_ars548_hw_interface_ros_wrapper.hpp | 146 ----- .../nebula_ros/hesai/hesai_ros_wrapper.hpp | 2 +- .../velodyne/velodyne_ros_wrapper.hpp | 2 +- .../launch/continental_launch_all_hw.xml | 19 +- ...=> continental_ars548_decoder_wrapper.cpp} | 307 ++++------ ...nental_ars548_hw_interface_ros_wrapper.cpp | 561 ------------------ ...ontinental_ars548_hw_interface_wrapper.cpp | 286 +++++++++ .../continental_ars548_ros_wrapper.cpp | 360 +++++++++++ ...nental_ars548_hw_interface_ros_wrapper.cpp | 356 ----------- .../continental_ros_decoder_test_ars548.cpp | 19 +- .../continental_ros_decoder_test_ars548.hpp | 8 +- 25 files changed, 1123 insertions(+), 2256 deletions(-) delete mode 100644 nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/multi_continental_ars548_hw_interface.hpp delete mode 100644 nebula_hw_interfaces/src/nebula_continental_hw_interfaces/multi_continental_ars548_hw_interface.cpp rename nebula_ros/include/nebula_ros/continental/{continental_ars548_decoder_ros_wrapper.hpp => continental_ars548_decoder_wrapper.hpp} (64%) rename nebula_ros/include/nebula_ros/continental/{continental_ars548_hw_interface_ros_wrapper.hpp => continental_ars548_hw_interface_wrapper.hpp} (55%) create mode 100644 nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp delete mode 100644 nebula_ros/include/nebula_ros/continental/multi_continental_ars548_hw_interface_ros_wrapper.hpp rename nebula_ros/src/continental/{continental_ars548_decoder_ros_wrapper.cpp => continental_ars548_decoder_wrapper.cpp} (70%) delete mode 100644 nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp create mode 100644 nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp create mode 100644 nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp delete mode 100644 nebula_ros/src/continental/multi_continental_ars548_hw_interface_ros_wrapper.cpp diff --git a/nebula_common/include/nebula_common/continental/continental_ars548.hpp b/nebula_common/include/nebula_common/continental/continental_ars548.hpp index b56fd03c5..3fde9e84b 100644 --- a/nebula_common/include/nebula_common/continental/continental_ars548.hpp +++ b/nebula_common/include/nebula_common/continental/continental_ars548.hpp @@ -43,7 +43,7 @@ namespace continental_ars548 { /// @brief struct for ARS548 sensor configuration -struct ContinentalARS548SensorConfiguration : EthernetSensorConfigurationBase +struct ContinentalArs548SensorConfiguration : EthernetSensorConfigurationBase { std::string multicast_ip{}; std::string base_frame{}; @@ -58,7 +58,7 @@ struct ContinentalARS548SensorConfiguration : EthernetSensorConfigurationBase }; /// @brief struct for Multiple ARS548 sensor configuration -struct MultiContinentalARS548SensorConfiguration : ContinentalARS548SensorConfiguration +struct MultiContinentalArs548SensorConfiguration : ContinentalArs548SensorConfiguration { std::vector sensor_ips{}; std::vector frame_ids{}; @@ -70,7 +70,7 @@ struct MultiContinentalARS548SensorConfiguration : ContinentalARS548SensorConfig /// @param arg /// @return stream inline std::ostream & operator<<( - std::ostream & os, ContinentalARS548SensorConfiguration const & arg) + std::ostream & os, ContinentalArs548SensorConfiguration const & arg) { os << (EthernetSensorConfigurationBase)(arg) << ", MulticastIP: " << arg.multicast_ip << ", BaseFrame: " << arg.base_frame << ", ObjectFrame: " << arg.object_frame @@ -84,13 +84,13 @@ inline std::ostream & operator<<( return os; } -/// @brief Convert MultiContinentalARS548SensorConfiguration to string (Overloading the << +/// @brief Convert MultiContinentalArs548SensorConfiguration to string (Overloading the << /// operator) /// @param os /// @param arg /// @return stream inline std::ostream & operator<<( - std::ostream & os, MultiContinentalARS548SensorConfiguration const & arg) + std::ostream & os, MultiContinentalArs548SensorConfiguration const & arg) { std::stringstream sensor_ips_ss; sensor_ips_ss << "["; @@ -106,13 +106,13 @@ inline std::ostream & operator<<( } frame_ids_ss << "]"; - os << (ContinentalARS548SensorConfiguration)(arg) << ", MulticastIP: " << arg.multicast_ip + os << (ContinentalArs548SensorConfiguration)(arg) << ", MulticastIP: " << arg.multicast_ip << ", SensorIPs: " << sensor_ips_ss.str() << ", FrameIds: " << frame_ids_ss.str(); return os; } /// @brief semantic struct of ARS548 Status -struct ContinentalARS548Status +struct ContinentalArs548Status { // Filled with raw sensor data uint32_t timestamp_nanoseconds{}; @@ -174,7 +174,7 @@ struct ContinentalARS548Status /// @param os /// @param arg /// @return stream - friend std::ostream & operator<<(std::ostream & os, ContinentalARS548Status const & arg) + friend std::ostream & operator<<(std::ostream & os, ContinentalArs548Status const & arg) { os << "timestamp_nanoseconds: " << arg.timestamp_nanoseconds; os << ", "; @@ -620,7 +620,7 @@ struct FilterStatusPacket #pragma pack(pop) -struct PointARS548Detection +struct PointArs548Detection { PCL_ADD_POINT4D; float azimuth; @@ -643,7 +643,7 @@ struct PointARS548Detection // Note we only use a subset of the data since POINT_CLOUD_REGISTER_POINT_STRUCT has a limit in the // number of fields -struct PointARS548Object +struct PointArs548Object { PCL_ADD_POINT4D; uint32_t id; @@ -671,7 +671,7 @@ struct PointARS548Object } // namespace nebula POINT_CLOUD_REGISTER_POINT_STRUCT( - nebula::drivers::continental_ars548::PointARS548Detection, + nebula::drivers::continental_ars548::PointArs548Detection, (float, x, x)(float, y, y)(float, z, z)(float, azimuth, azimuth)(float, azimuth_std, azimuth_std)( float, elevation, elevation)(float, elevation_std, elevation_std)(float, range, range)( float, range_std, range_std)(int8_t, rcs, rcs)(uint16_t, measurement_id, measurement_id)( @@ -682,7 +682,7 @@ POINT_CLOUD_REGISTER_POINT_STRUCT( // Note: we can only use up to 20 fields POINT_CLOUD_REGISTER_POINT_STRUCT( - nebula::drivers::continental_ars548::PointARS548Object, + nebula::drivers::continental_ars548::PointArs548Object, (float, x, x)(float, y, y)(float, z, z)(uint32_t, id, id)(uint16_t, age, age)( uint8_t, status_measurement, status_measurement)(uint8_t, status_movement, status_movement)( uint8_t, position_reference, diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp index 139ea6d5e..d5579a077 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp @@ -35,36 +35,40 @@ namespace drivers namespace continental_ars548 { /// @brief Continental Radar decoder (ARS548) -class ContinentalARS548Decoder : public ContinentalPacketsDecoder +class ContinentalArs548Decoder : public ContinentalPacketsDecoder { public: /// @brief Constructor /// @param sensor_configuration SensorConfiguration for this decoder - explicit ContinentalARS548Decoder( - const std::shared_ptr & sensor_configuration); + explicit ContinentalArs548Decoder( + const std::shared_ptr & 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; + bool ProcessPacket(const nebula_msgs::msg::NebulaPacket & nebula_packet) override; /// @brief Function for parsing detection lists /// @param data /// @return Resulting flag bool ParseDetectionsListPacket( - const std::vector & data, const std_msgs::msg::Header & header); + const std::vector & data, const builtin_interfaces::msg::Time & stamp); /// @brief Function for parsing object lists /// @param data /// @return Resulting flag bool ParseObjectsListPacket( - const std::vector & data, const std_msgs::msg::Header & header); + const std::vector & data, const builtin_interfaces::msg::Time & stamp); /// @brief Function for parsing sensor status messages /// @param data /// @return Resulting flag bool ParseSensorStatusPacket( - const std::vector & data, const std_msgs::msg::Header & header); + const std::vector & data, const builtin_interfaces::msg::Time & stamp); /// @brief Register function to call when a new detection list is processed /// @param detection_list_callback @@ -84,19 +88,20 @@ class ContinentalARS548Decoder : public ContinentalPacketsDecoder /// @param object_list_callback /// @return Resulting status Status RegisterSensorStatusCallback( - std::function sensor_status_callback); + std::function sensor_status_callback); private: std::function msg)> detection_list_callback_; std::function msg)> object_list_callback_; - std::function sensor_status_callback_; + std::function sensor_status_callback_; - ContinentalARS548Status radar_status_{}; + ContinentalArs548Status radar_status_{}; /// @brief SensorConfiguration for this decoder - std::shared_ptr sensor_configuration_; + std::shared_ptr + sensor_configuration_; }; } // namespace continental_ars548 diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp index e5f0f5efa..2e9518312 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp @@ -39,10 +39,14 @@ class ContinentalPacketsDecoder virtual ~ContinentalPacketsDecoder() = default; ContinentalPacketsDecoder() = default; + /// @brief Get current status of this driver + /// @return Current status + virtual Status GetStatus() = 0; + /// @brief Virtual function for parsing NebulaPackets based on packet structure /// @param nebula_packets /// @return Resulting flag - virtual bool ProcessPackets(const nebula_msgs::msg::NebulaPackets & nebula_packets) = 0; + virtual bool ProcessPacket(const nebula_msgs::msg::NebulaPacket & nebula_packet) = 0; }; } // namespace drivers } // namespace nebula diff --git a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp index fb2a44f60..a7bf2cc31 100644 --- a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp @@ -25,14 +25,19 @@ namespace drivers { namespace continental_ars548 { -ContinentalARS548Decoder::ContinentalARS548Decoder( - const std::shared_ptr & +ContinentalArs548Decoder::ContinentalArs548Decoder( + const std::shared_ptr & sensor_configuration) { sensor_configuration_ = sensor_configuration; } -Status ContinentalARS548Decoder::RegisterDetectionListCallback( +Status ContinentalArs548Decoder::GetStatus() +{ + return Status::OK; +} + +Status ContinentalArs548Decoder::RegisterDetectionListCallback( std::function)> detection_list_callback) { @@ -40,7 +45,7 @@ Status ContinentalARS548Decoder::RegisterDetectionListCallback( return Status::OK; } -Status ContinentalARS548Decoder::RegisterObjectListCallback( +Status ContinentalArs548Decoder::RegisterObjectListCallback( std::function)> object_list_callback) { @@ -48,21 +53,16 @@ Status ContinentalARS548Decoder::RegisterObjectListCallback( return Status::OK; } -Status ContinentalARS548Decoder::RegisterSensorStatusCallback( - std::function sensor_status_callback) +Status ContinentalArs548Decoder::RegisterSensorStatusCallback( + std::function sensor_status_callback) { sensor_status_callback_ = std::move(sensor_status_callback); return Status::OK; } -bool ContinentalARS548Decoder::ProcessPackets( - const nebula_msgs::msg::NebulaPackets & nebula_packets) +bool ContinentalArs548Decoder::ProcessPacket(const nebula_msgs::msg::NebulaPacket & nebula_packet) { - if (nebula_packets.packets.size() != 1) { - return false; - } - - const auto & data = nebula_packets.packets[0].data; + const auto & data = nebula_packet.data; if (data.size() < sizeof(HeaderPacket)) { return false; @@ -82,13 +82,13 @@ bool ContinentalARS548Decoder::ProcessPackets( return false; } - return ParseDetectionsListPacket(data, nebula_packets.header); + return ParseDetectionsListPacket(data, nebula_packet.stamp); } 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); + return ParseObjectsListPacket(data, nebula_packet.stamp); } else if (header.method_id.value() == SENSOR_STATUS_METHOD_ID) { if ( data.size() != SENSOR_STATUS_UDP_PAYLOAD || @@ -96,14 +96,14 @@ bool ContinentalARS548Decoder::ProcessPackets( return false; } - return ParseSensorStatusPacket(data, nebula_packets.header); + return ParseSensorStatusPacket(data, nebula_packet.stamp); } return true; } -bool ContinentalARS548Decoder::ParseDetectionsListPacket( - const std::vector & data, const std_msgs::msg::Header & header) +bool ContinentalArs548Decoder::ParseDetectionsListPacket( + const std::vector & data, const builtin_interfaces::msg::Time & stamp) { auto msg_ptr = std::make_unique(); auto & msg = *msg_ptr; @@ -119,7 +119,7 @@ bool ContinentalARS548Decoder::ParseDetectionsListPacket( 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 = stamp; } msg.stamp_sync_status = detection_list.stamp.timestamp_sync_status; @@ -227,8 +227,8 @@ bool ContinentalARS548Decoder::ParseDetectionsListPacket( return true; } -bool ContinentalARS548Decoder::ParseObjectsListPacket( - const std::vector & data, const std_msgs::msg::Header & header) +bool ContinentalArs548Decoder::ParseObjectsListPacket( + const std::vector & data, const builtin_interfaces::msg::Time & stamp) { auto msg_ptr = std::make_unique(); auto & msg = *msg_ptr; @@ -244,7 +244,7 @@ bool ContinentalARS548Decoder::ParseObjectsListPacket( 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 = stamp; } msg.stamp_sync_status = object_list.stamp.timestamp_sync_status; @@ -379,8 +379,8 @@ bool ContinentalARS548Decoder::ParseObjectsListPacket( return true; } -bool ContinentalARS548Decoder::ParseSensorStatusPacket( - const std::vector & data, [[maybe_unused]] const std_msgs::msg::Header & header) +bool ContinentalArs548Decoder::ParseSensorStatusPacket( + const std::vector & data, [[maybe_unused]] const builtin_interfaces::msg::Time & stamp) { SensorStatusPacket sensor_status_packet; std::memcpy(&sensor_status_packet, data.data(), sizeof(SensorStatusPacket)); diff --git a/nebula_hw_interfaces/CMakeLists.txt b/nebula_hw_interfaces/CMakeLists.txt index 32893d1f4..7cd8b8982 100644 --- a/nebula_hw_interfaces/CMakeLists.txt +++ b/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 ) diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp index 5b46415e7..f5692f5f5 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp +++ b/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 #include #include #include @@ -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 & buffer) {} + virtual void ReceiveSensorPacketCallback([[maybe_unused]] std::vector & buffer) {} // virtual Status RegisterScanCallback( // std::function>>)> scan_callback) = 0; diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp index 3e150b18e..c9389079c 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp @@ -32,7 +32,6 @@ #include #include -#include #include #include @@ -50,15 +49,13 @@ namespace drivers namespace continental_ars548 { /// @brief Hardware interface of the Continental ARS548 radar -class ContinentalARS548HwInterface : NebulaHwInterfaceBase +class ContinentalArs548HwInterface /* : NebulaHwInterfaceBase */ { private: std::unique_ptr<::drivers::common::IoContext> sensor_io_context_; std::unique_ptr<::drivers::udp_driver::UdpDriver> sensor_udp_driver_; - std::shared_ptr sensor_configuration_; - std::unique_ptr nebula_packets_ptr_; - std::function buffer)> - nebula_packets_reception_callback_; + std::shared_ptr sensor_configuration_; + std::function &)> callback_; std::mutex sensor_status_mutex_; @@ -85,49 +82,48 @@ class ContinentalARS548HwInterface : NebulaHwInterfaceBase public: /// @brief Constructor - ContinentalARS548HwInterface(); + ContinentalArs548HwInterface(); /// @brief Process a new filter status packet /// @param buffer The buffer containing the status packet - void ProcessFilterStatusPacket(const std::vector & buffer); + void ProcessFilterStatusPacket(std::vector & buffer); /// @brief Process a new data packet /// @param buffer The buffer containing the data packet - void ProcessDataPacket(const std::vector & buffer); + void ProcessDataPacket(std::vector & buffer); /// @brief Callback function to receive the Cloud Packet data from the UDP Driver /// @param buffer Buffer containing the data received from the UDP socket void ReceiveSensorPacketCallbackWithSender( - const std::vector & buffer, const std::string & sender_ip); + std::vector & buffer, const std::string & sender_ip); /// @brief Callback function to receive the Cloud Packet data from the UDP Driver /// @param buffer Buffer containing the data received from the UDP socket - void ReceiveSensorPacketCallback(const std::vector & buffer) final; + void ReceiveSensorPacketCallback(std::vector & buffer); /// @brief Starting the interface that handles UDP streams /// @return Resulting status - Status SensorInterfaceStart() final; + Status SensorInterfaceStart(); /// @brief Function for stopping the interface that handles UDP streams /// @return Resulting status - Status SensorInterfaceStop() final; + Status SensorInterfaceStop(); /// @brief Printing sensor configuration /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status - Status GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) final; + Status GetSensorConfiguration(SensorConfigurationBase & sensor_configuration); /// @brief Setting sensor configuration /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status Status SetSensorConfiguration( - std::shared_ptr sensor_configuration) final; + std::shared_ptr sensor_configuration); - /// @brief Registering callback for PandarScan - /// @param scan_callback Callback function + /// @brief Registering callback + /// @param callback Callback function /// @return Resulting status - Status RegisterScanCallback( - std::function)> scan_callback); + Status RegisterCallback(std::function &)> callback); /// @brief Set the sensor mounting parameters /// @param longitudinal_autosar Desired longitudinal value in autosar coordinates diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/multi_continental_ars548_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/multi_continental_ars548_hw_interface.hpp deleted file mode 100644 index b029ae40e..000000000 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/multi_continental_ars548_hw_interface.hpp +++ /dev/null @@ -1,179 +0,0 @@ -// 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. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef NEBULA_MULTI_CONTINENTAL_ARS548_HW_INTERFACE_H -#define NEBULA_MULTI_CONTINENTAL_ARS548_HW_INTERFACE_H -// Have to define macros to silence warnings about deprecated headers being used by -// boost/property_tree/ in some versions of boost. -// See: https://github.com/boostorg/property_tree/issues/51 -#include -#if (BOOST_VERSION / 100 >= 1073 && BOOST_VERSION / 100 <= 1076) // Boost 1.73 - 1.76 -#define BOOST_BIND_GLOBAL_PLACEHOLDERS -#endif -#if (BOOST_VERSION / 100 == 1074) // Boost 1.74 -#define BOOST_ALLOW_DEPRECATED_HEADERS -#endif -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include - -namespace nebula -{ -namespace drivers -{ -namespace continental_ars548 -{ -/// @brief Hardware interface of the Continental ARS548 radar -class MultiContinentalARS548HwInterface : NebulaHwInterfaceBase -{ -private: - std::unique_ptr<::drivers::common::IoContext> sensor_io_context_; - std::vector> sensor_udp_drivers_; - std::shared_ptr sensor_configuration_; - std::unique_ptr nebula_packets_ptr_; - std::function buffer, const std::string & sensor_ip)> - nebula_packets_reception_callback_; - - std::mutex sensor_status_mutex_; - - SensorStatusPacket sensor_status_packet_{}; - FilterStatusPacket filter_status_{}; - std::unordered_map sensor_ip_to_frame_; - - std::shared_ptr parent_node_logger; - - /// @brief Printing the string to RCLCPP_INFO_STREAM - /// @param info Target string - void PrintInfo(std::string info); - - /// @brief Printing the string to RCLCPP_ERROR_STREAM - /// @param error Target string - void PrintError(std::string error); - - /// @brief Printing the string to RCLCPP_DEBUG_STREAM - /// @param debug Target string - void PrintDebug(std::string debug); - - /// @brief Printing the bytes to RCLCPP_DEBUG_STREAM - /// @param bytes Target byte vector - void PrintDebug(const std::vector & bytes); - -public: - /// @brief Constructor - MultiContinentalARS548HwInterface(); - - /// @brief Process a new filter status packet - /// @param buffer The buffer containing the status packet - void ProcessFilterStatusPacket(const std::vector & buffer); - - /// @brief Process a new data packet - /// @param buffer The buffer containing the data packet - void ProcessDataPacket(const std::vector & buffer, const std::string & sensor_ip); - - /// @brief Callback function to receive the Cloud Packet data from the UDP Driver - /// @param buffer Buffer containing the data received from the UDP socket - void ReceiveSensorPacketCallback( - const std::vector & buffer, const std::string & sensor_ip); - - /// @brief Starting the interface that handles UDP streams - /// @return Resulting status - Status SensorInterfaceStart() final; - - /// @brief Function for stopping the interface that handles UDP streams - /// @return Resulting status - Status SensorInterfaceStop() final; - - /// @brief Printing sensor configuration - /// @param sensor_configuration SensorConfiguration for this interface - /// @return Resulting status - Status GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) final; - - /// @brief Setting sensor configuration - /// @param sensor_configuration SensorConfiguration for this interface - /// @return Resulting status - Status SetSensorConfiguration( - std::shared_ptr sensor_configuration) final; - - /// @brief Registering callback for PandarScan - /// @param scan_callback Callback function - /// @return Resulting status - Status RegisterScanCallback( - std::function, const std::string &)> - scan_callback); - - /// @brief Set the current lateral acceleration - /// @param lateral_acceleration Current lateral acceleration - /// @return Resulting status - Status SetAccelerationLateralCog(float lateral_acceleration); - - /// @brief Set the current longitudinal acceleration - /// @param longitudinal_acceleration Current longitudinal acceleration - /// @return Resulting status - Status SetAccelerationLongitudinalCog(float longitudinal_acceleration); - - /// @brief Set the characteristic speed - /// @param characteristic_speed Characteristic speed - /// @return Resulting status - Status SetCharacteristicSpeed(float characteristic_speed); - - /// @brief Set the current direction - /// @param direction Current driving direction - /// @return Resulting status - Status SetDrivingDirection(int direction); - - /// @brief Set the current steering angle - /// @param angle_rad Current steering angle in radians - /// @return Resulting status - Status SetSteeringAngleFrontAxle(float angle_rad); - - /// @brief Set the current vehicle velocity - /// @param velocity Current vehicle velocity - /// @return Resulting status - Status SetVelocityVehicle(float velocity); - - /// @brief Set the current yaw rate - /// @param yaw_rate Current yaw rate - /// @return Resulting status - Status SetYawRate(float yaw_rate); - - /// @brief Checking the current settings and changing the difference point - /// @return Resulting status - Status CheckAndSetConfig(); - - /// @brief Setting rclcpp::Logger - /// @param node Logger - void SetLogger(std::shared_ptr node); -}; -} // namespace continental_ars548 -} // namespace drivers -} // namespace nebula - -#endif // NEBULA_MULTI_CONTINENTAL_ARS548_HW_INTERFACE_H diff --git a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp index 2deeb7a28..151b06066 100644 --- a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp @@ -24,31 +24,21 @@ namespace drivers { namespace continental_ars548 { -ContinentalARS548HwInterface::ContinentalARS548HwInterface() +ContinentalArs548HwInterface::ContinentalArs548HwInterface() : sensor_io_context_{new ::drivers::common::IoContext(1)}, - sensor_udp_driver_{new ::drivers::udp_driver::UdpDriver(*sensor_io_context_)}, - nebula_packets_ptr_{std::make_unique()} + sensor_udp_driver_{new ::drivers::udp_driver::UdpDriver(*sensor_io_context_)} { } -Status ContinentalARS548HwInterface::SetSensorConfiguration( - std::shared_ptr sensor_configuration) +Status ContinentalArs548HwInterface::SetSensorConfiguration( + std::shared_ptr sensor_configuration) { - Status status = Status::OK; - - try { - sensor_configuration_ = - std::static_pointer_cast(sensor_configuration); - } catch (const std::exception & ex) { - status = Status::SENSOR_CONFIG_ERROR; - std::cerr << status << std::endl; - return status; - } + sensor_configuration_ = sensor_configuration; return Status::OK; } -Status ContinentalARS548HwInterface::SensorInterfaceStart() +Status ContinentalArs548HwInterface::SensorInterfaceStart() { try { sensor_udp_driver_->init_receiver( @@ -58,7 +48,7 @@ Status ContinentalARS548HwInterface::SensorInterfaceStart() sensor_udp_driver_->receiver()->open(); sensor_udp_driver_->receiver()->bind(); sensor_udp_driver_->receiver()->asyncReceiveWithSender(std::bind( - &ContinentalARS548HwInterface::ReceiveSensorPacketCallbackWithSender, this, + &ContinentalArs548HwInterface::ReceiveSensorPacketCallbackWithSender, this, std::placeholders::_1, std::placeholders::_2)); sensor_udp_driver_->init_sender( @@ -80,21 +70,21 @@ Status ContinentalARS548HwInterface::SensorInterfaceStart() return Status::OK; } -Status ContinentalARS548HwInterface::RegisterScanCallback( - std::function)> callback) +Status ContinentalArs548HwInterface::RegisterCallback( + std::function &)> callback) { - nebula_packets_reception_callback_ = std::move(callback); + callback_ = std::move(callback); return Status::OK; } -void ContinentalARS548HwInterface::ReceiveSensorPacketCallbackWithSender( - const std::vector & buffer, const std::string & sender_ip) +void ContinentalArs548HwInterface::ReceiveSensorPacketCallbackWithSender( + std::vector & buffer, const std::string & sender_ip) { if (sender_ip == sensor_configuration_->sensor_ip) { ReceiveSensorPacketCallback(buffer); } } -void ContinentalARS548HwInterface::ReceiveSensorPacketCallback(const std::vector & buffer) +void ContinentalArs548HwInterface::ReceiveSensorPacketCallback(std::vector & buffer) { if (buffer.size() < sizeof(HeaderPacket)) { PrintError("Unrecognized packet. Too short"); @@ -145,38 +135,35 @@ void ContinentalARS548HwInterface::ReceiveSensorPacketCallback(const std::vector } } -void ContinentalARS548HwInterface::ProcessFilterStatusPacket(const std::vector & buffer) +void ContinentalArs548HwInterface::ProcessFilterStatusPacket(std::vector & buffer) { assert(buffer.size() == sizeof(FilterStatusPacket)); std::memcpy(&filter_status_, buffer.data(), sizeof(FilterStatusPacket)); } -void ContinentalARS548HwInterface::ProcessDataPacket(const std::vector & buffer) +void ContinentalArs548HwInterface::ProcessDataPacket(std::vector & buffer) { - nebula_msgs::msg::NebulaPacket nebula_packet; - nebula_packet.data = buffer; - auto now = std::chrono::system_clock::now(); - auto now_secs = std::chrono::duration_cast(now.time_since_epoch()).count(); - auto now_nanosecs = - std::chrono::duration_cast(now.time_since_epoch()).count(); - nebula_packet.stamp.sec = static_cast(now_secs); - nebula_packet.stamp.nanosec = - static_cast((now_nanosecs / 1000000000.0 - static_cast(now_secs)) * 1000000000); - nebula_packets_ptr_->packets.emplace_back(nebula_packet); - - nebula_packets_ptr_->header.stamp = nebula_packets_ptr_->packets.front().stamp; - nebula_packets_ptr_->header.frame_id = sensor_configuration_->frame_id; - - nebula_packets_reception_callback_(std::move(nebula_packets_ptr_)); - nebula_packets_ptr_ = std::make_unique(); + /* auto nebula_packet_ptr = std::make_unique(); + nebula_packet_ptr->data = buffer; + auto now = std::chrono::system_clock::now(); + auto now_secs = + std::chrono::duration_cast(now.time_since_epoch()).count(); auto + now_nanosecs = + std::chrono::duration_cast(now.time_since_epoch()).count(); + nebula_packet_ptr->stamp.sec = static_cast(now_secs); + nebula_packet_ptr->stamp.nanosec = + static_cast((now_nanosecs / 1000000000.0 - static_cast(now_secs)) * 1000000000); + */ + + callback_(buffer); } -Status ContinentalARS548HwInterface::SensorInterfaceStop() +Status ContinentalArs548HwInterface::SensorInterfaceStop() { return Status::ERROR_1; } -Status ContinentalARS548HwInterface::GetSensorConfiguration( +Status ContinentalArs548HwInterface::GetSensorConfiguration( SensorConfigurationBase & sensor_configuration) { std::stringstream ss; @@ -185,7 +172,7 @@ Status ContinentalARS548HwInterface::GetSensorConfiguration( return Status::ERROR_1; } -Status ContinentalARS548HwInterface::SetSensorMounting( +Status ContinentalArs548HwInterface::SetSensorMounting( float longitudinal_autosar, float lateral_autosar, float vertical_autosar, float yaw_autosar, float pitch_autosar, uint8_t plug_orientation) { @@ -229,7 +216,7 @@ Status ContinentalARS548HwInterface::SetSensorMounting( return Status::OK; } -Status ContinentalARS548HwInterface::SetVehicleParameters( +Status ContinentalArs548HwInterface::SetVehicleParameters( float length_autosar, float width_autosar, float height_autosar, float wheel_base_autosar) { if ( @@ -268,7 +255,7 @@ Status ContinentalARS548HwInterface::SetVehicleParameters( return Status::OK; } -Status ContinentalARS548HwInterface::SetRadarParameters( +Status ContinentalArs548HwInterface::SetRadarParameters( uint16_t maximum_distance, uint8_t frequency_slot, uint8_t cycle_time, uint8_t time_slot, uint8_t hcc, uint8_t power_save_standstill) { @@ -312,7 +299,7 @@ Status ContinentalARS548HwInterface::SetRadarParameters( return Status::OK; } -Status ContinentalARS548HwInterface::SetSensorIPAddress(const std::string & sensor_ip_address) +Status ContinentalArs548HwInterface::SetSensorIPAddress(const std::string & sensor_ip_address) { std::array ip_bytes; @@ -353,7 +340,7 @@ Status ContinentalARS548HwInterface::SetSensorIPAddress(const std::string & sens return Status::OK; } -Status ContinentalARS548HwInterface::SetAccelerationLateralCog(float lateral_acceleration) +Status ContinentalArs548HwInterface::SetAccelerationLateralCog(float lateral_acceleration) { constexpr uint16_t ACCELERATION_LATERAL_COG_SERVICE_ID = 0; constexpr uint16_t ACCELERATION_LATERAL_COG_METHOD_ID = 321; @@ -384,7 +371,7 @@ Status ContinentalARS548HwInterface::SetAccelerationLateralCog(float lateral_acc return Status::OK; } -Status ContinentalARS548HwInterface::SetAccelerationLongitudinalCog(float longitudinal_acceleration) +Status ContinentalArs548HwInterface::SetAccelerationLongitudinalCog(float longitudinal_acceleration) { constexpr uint16_t ACCELERATION_LONGITUDINAL_COG_SERVICE_ID = 0; constexpr uint16_t ACCELERATION_LONGITUDINAL_COG_METHOD_ID = 322; @@ -417,7 +404,7 @@ Status ContinentalARS548HwInterface::SetAccelerationLongitudinalCog(float longit return Status::OK; } -Status ContinentalARS548HwInterface::SetCharacteristicSpeed(float characteristic_speed) +Status ContinentalArs548HwInterface::SetCharacteristicSpeed(float characteristic_speed) { constexpr uint16_t CHARACTERISTIC_SPEED_SERVICE_ID = 0; constexpr uint16_t CHARACTERISTIC_SPEED_METHOD_ID = 328; @@ -448,7 +435,7 @@ Status ContinentalARS548HwInterface::SetCharacteristicSpeed(float characteristic return Status::OK; } -Status ContinentalARS548HwInterface::SetDrivingDirection(int direction) +Status ContinentalArs548HwInterface::SetDrivingDirection(int direction) { constexpr uint16_t DRIVING_DIRECTION_SERVICE_ID = 0; constexpr uint16_t DRIVING_DIRECTION_METHOD_ID = 325; @@ -484,7 +471,7 @@ Status ContinentalARS548HwInterface::SetDrivingDirection(int direction) return Status::OK; } -Status ContinentalARS548HwInterface::SetSteeringAngleFrontAxle(float angle_rad) +Status ContinentalArs548HwInterface::SetSteeringAngleFrontAxle(float angle_rad) { constexpr uint16_t STEERING_ANGLE_SERVICE_ID = 0; constexpr uint16_t STEERING_ANGLE_METHOD_ID = 327; @@ -516,7 +503,7 @@ Status ContinentalARS548HwInterface::SetSteeringAngleFrontAxle(float angle_rad) return Status::OK; } -Status ContinentalARS548HwInterface::SetVelocityVehicle(float velocity_kmh) +Status ContinentalArs548HwInterface::SetVelocityVehicle(float velocity_kmh) { if (velocity_kmh < 0.f || velocity_kmh > 350.f) { PrintError("Invalid velocity value"); @@ -547,7 +534,7 @@ Status ContinentalARS548HwInterface::SetVelocityVehicle(float velocity_kmh) return Status::OK; } -Status ContinentalARS548HwInterface::SetYawRate(float yaw_rate) +Status ContinentalArs548HwInterface::SetYawRate(float yaw_rate) { if (yaw_rate < -163.83 || yaw_rate > 163.83) { PrintError("Invalid yaw rate value"); @@ -578,12 +565,12 @@ Status ContinentalARS548HwInterface::SetYawRate(float yaw_rate) return Status::OK; } -void ContinentalARS548HwInterface::SetLogger(std::shared_ptr logger) +void ContinentalArs548HwInterface::SetLogger(std::shared_ptr logger) { parent_node_logger = logger; } -void ContinentalARS548HwInterface::PrintInfo(std::string info) +void ContinentalArs548HwInterface::PrintInfo(std::string info) { if (parent_node_logger) { RCLCPP_INFO_STREAM((*parent_node_logger), info); @@ -592,7 +579,7 @@ void ContinentalARS548HwInterface::PrintInfo(std::string info) } } -void ContinentalARS548HwInterface::PrintError(std::string error) +void ContinentalArs548HwInterface::PrintError(std::string error) { if (parent_node_logger) { RCLCPP_ERROR_STREAM((*parent_node_logger), error); @@ -601,7 +588,7 @@ void ContinentalARS548HwInterface::PrintError(std::string error) } } -void ContinentalARS548HwInterface::PrintDebug(std::string debug) +void ContinentalArs548HwInterface::PrintDebug(std::string debug) { if (parent_node_logger) { RCLCPP_DEBUG_STREAM((*parent_node_logger), debug); @@ -610,7 +597,7 @@ void ContinentalARS548HwInterface::PrintDebug(std::string debug) } } -void ContinentalARS548HwInterface::PrintDebug(const std::vector & bytes) +void ContinentalArs548HwInterface::PrintDebug(const std::vector & bytes) { std::stringstream ss; for (const auto & b : bytes) { diff --git a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/multi_continental_ars548_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/multi_continental_ars548_hw_interface.cpp deleted file mode 100644 index 58cd25f09..000000000 --- a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/multi_continental_ars548_hw_interface.cpp +++ /dev/null @@ -1,453 +0,0 @@ -// 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. -// You may obtain a copy of the License at -// - -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#include -#include -namespace nebula -{ -namespace drivers -{ -namespace continental_ars548 -{ -MultiContinentalARS548HwInterface::MultiContinentalARS548HwInterface() -: sensor_io_context_{new ::drivers::common::IoContext(1)}, - nebula_packets_ptr_{std::make_unique()} -{ -} - -Status MultiContinentalARS548HwInterface::SetSensorConfiguration( - std::shared_ptr sensor_configuration) -{ - Status status = Status::OK; - - try { - sensor_configuration_ = - std::static_pointer_cast(sensor_configuration); - } catch (const std::exception & ex) { - status = Status::SENSOR_CONFIG_ERROR; - std::cerr << status << std::endl; - return status; - } - - return Status::OK; -} - -Status MultiContinentalARS548HwInterface::SensorInterfaceStart() -{ - for (std::size_t sensor_id = 0; sensor_id < sensor_configuration_->sensor_ips.size(); - sensor_id++) { - auto udp_driver = std::make_unique<::drivers::udp_driver::UdpDriver>(*sensor_io_context_); - sensor_ip_to_frame_[sensor_configuration_->sensor_ips[sensor_id]] = - sensor_configuration_->frame_ids[sensor_id]; - - try { - if (sensor_id == 0) { - udp_driver->init_receiver( - sensor_configuration_->multicast_ip, sensor_configuration_->data_port, - sensor_configuration_->host_ip, sensor_configuration_->data_port, 2 << 16); - udp_driver->receiver()->setMulticast(true); - udp_driver->receiver()->open(); - udp_driver->receiver()->bind(); - udp_driver->receiver()->asyncReceiveWithSender(std::bind( - &MultiContinentalARS548HwInterface::ReceiveSensorPacketCallback, this, - std::placeholders::_1, std::placeholders::_2)); - } - - udp_driver->init_sender( - sensor_configuration_->sensor_ips[sensor_id], - sensor_configuration_->configuration_sensor_port, sensor_configuration_->host_ip, - sensor_configuration_->configuration_host_port); - - udp_driver->sender()->open(); - udp_driver->sender()->bind(); - - if (!udp_driver->sender()->isOpen()) { - return Status::ERROR_1; - } - } catch (const std::exception & ex) { - Status status = Status::UDP_CONNECTION_ERROR; - std::cerr << status << sensor_configuration_->sensor_ip << "," - << sensor_configuration_->data_port << std::endl; - return status; - } - - sensor_udp_drivers_.emplace_back(std::move(udp_driver)); - } - - return Status::OK; -} - -Status MultiContinentalARS548HwInterface::RegisterScanCallback( - std::function, const std::string &)> - callback) -{ - nebula_packets_reception_callback_ = std::move(callback); - return Status::OK; -} - -void MultiContinentalARS548HwInterface::ReceiveSensorPacketCallback( - const std::vector & buffer, const std::string & sender_ip) -{ - if (buffer.size() < sizeof(HeaderPacket)) { - PrintError("Unrecognized packet. Too short"); - return; - } - - HeaderPacket header_packet{}; - std::memcpy(&header_packet, buffer.data(), sizeof(HeaderPacket)); - - if (header_packet.service_id.value() != 0) { - PrintError("Invalid service id"); - return; - } else if (header_packet.method_id.value() == SENSOR_STATUS_METHOD_ID) { - if ( - buffer.size() != SENSOR_STATUS_UDP_PAYLOAD || - header_packet.length.value() != SENSOR_STATUS_PDU_LENGTH) { - PrintError("SensorStatus message with invalid size"); - return; - } - ProcessDataPacket(buffer, sender_ip); - } else if (header_packet.method_id.value() == FILTER_STATUS_METHOD_ID) { - if ( - buffer.size() != FILTER_STATUS_UDP_PAYLOAD || - header_packet.length.value() != FILTER_STATUS_PDU_LENGTH) { - PrintError("FilterStatus message with invalid size"); - return; - } - - ProcessFilterStatusPacket(buffer); - } else if (header_packet.method_id.value() == DETECTION_LIST_METHOD_ID) { - if ( - buffer.size() != DETECTION_LIST_UDP_PAYLOAD || - header_packet.length.value() != DETECTION_LIST_PDU_LENGTH) { - PrintError("DetectionList message with invalid size"); - return; - } - - ProcessDataPacket(buffer, sender_ip); - } else if (header_packet.method_id.value() == OBJECT_LIST_METHOD_ID) { - if ( - buffer.size() != OBJECT_LIST_UDP_PAYLOAD || - header_packet.length.value() != OBJECT_LIST_PDU_LENGTH) { - PrintError("ObjectList message with invalid size"); - return; - } - - ProcessDataPacket(buffer, sender_ip); - } -} - -void MultiContinentalARS548HwInterface::ProcessFilterStatusPacket( - const std::vector & buffer) -{ - assert(buffer.size() == sizeof(FilterStatusPacket)); - std::memcpy(&filter_status_, buffer.data(), sizeof(FilterStatusPacket)); -} - -void MultiContinentalARS548HwInterface::ProcessDataPacket( - const std::vector & buffer, const std::string & sensor_ip) -{ - nebula_msgs::msg::NebulaPacket nebula_packet; - nebula_packet.data = buffer; - auto now = std::chrono::system_clock::now(); - auto now_secs = std::chrono::duration_cast(now.time_since_epoch()).count(); - auto now_nanosecs = - std::chrono::duration_cast(now.time_since_epoch()).count(); - nebula_packet.stamp.sec = static_cast(now_secs); - nebula_packet.stamp.nanosec = - static_cast((now_nanosecs / 1000000000.0 - static_cast(now_secs)) * 1000000000); - nebula_packets_ptr_->packets.emplace_back(nebula_packet); - - nebula_packets_ptr_->header.stamp = nebula_packets_ptr_->packets.front().stamp; - nebula_packets_ptr_->header.frame_id = sensor_configuration_->frame_id; - - nebula_packets_reception_callback_(std::move(nebula_packets_ptr_), sensor_ip); - nebula_packets_ptr_ = std::make_unique(); -} - -Status MultiContinentalARS548HwInterface::SensorInterfaceStop() -{ - return Status::ERROR_1; -} - -Status MultiContinentalARS548HwInterface::GetSensorConfiguration( - SensorConfigurationBase & sensor_configuration) -{ - std::stringstream ss; - ss << sensor_configuration; - PrintDebug(ss.str()); - return Status::ERROR_1; -} - -Status MultiContinentalARS548HwInterface::SetAccelerationLateralCog(float lateral_acceleration) -{ - constexpr uint16_t ACCELERATION_LATERAL_COG_SERVICE_ID = 0; - constexpr uint16_t ACCELERATION_LATERAL_COG_METHOD_ID = 321; - constexpr uint8_t ACCELERATION_LATERAL_COG_LENGTH = 32; - const int ACCELERATION_LATERAL_COG_UDP_LENGTH = 40; - - if (lateral_acceleration < -65.f || lateral_acceleration > 65.f) { - PrintError("Invalid lateral_acceleration value"); - return Status::ERROR_1; - } - - AccelerationLateralCoGPacket acceleration_lateral_cog{}; - static_assert(sizeof(AccelerationLateralCoGPacket) == ACCELERATION_LATERAL_COG_UDP_LENGTH); - acceleration_lateral_cog.header.service_id = ACCELERATION_LATERAL_COG_SERVICE_ID; - acceleration_lateral_cog.header.method_id = ACCELERATION_LATERAL_COG_METHOD_ID; - acceleration_lateral_cog.header.length = ACCELERATION_LATERAL_COG_LENGTH; - acceleration_lateral_cog.acceleration_lateral = lateral_acceleration; - - std::vector send_vector(sizeof(AccelerationLateralCoGPacket)); - std::memcpy(send_vector.data(), &acceleration_lateral_cog, sizeof(AccelerationLateralCoGPacket)); - - for (auto & udp_driver : sensor_udp_drivers_) { - udp_driver->sender()->asyncSend(send_vector); - } - - return Status::OK; -} - -Status MultiContinentalARS548HwInterface::SetAccelerationLongitudinalCog( - float longitudinal_acceleration) -{ - constexpr uint16_t ACCELERATION_LONGITUDINAL_COG_SERVICE_ID = 0; - constexpr uint16_t ACCELERATION_LONGITUDINAL_COG_METHOD_ID = 322; - constexpr uint8_t ACCELERATION_LONGITUDINAL_COG_LENGTH = 32; - const int ACCELERATION_LONGITUDINAL_COG_UDP_LENGTH = 40; - - if (longitudinal_acceleration < -65.f || longitudinal_acceleration > 65.f) { - PrintError("Invalid longitudinal_acceleration value"); - return Status::ERROR_1; - } - - AccelerationLongitudinalCoGPacket acceleration_longitudinal_cog{}; - static_assert( - sizeof(AccelerationLongitudinalCoGPacket) == ACCELERATION_LONGITUDINAL_COG_UDP_LENGTH); - acceleration_longitudinal_cog.header.service_id = ACCELERATION_LONGITUDINAL_COG_SERVICE_ID; - acceleration_longitudinal_cog.header.method_id = ACCELERATION_LONGITUDINAL_COG_METHOD_ID; - acceleration_longitudinal_cog.header.length = ACCELERATION_LONGITUDINAL_COG_LENGTH; - acceleration_longitudinal_cog.acceleration_lateral = longitudinal_acceleration; - - std::vector send_vector(sizeof(AccelerationLongitudinalCoGPacket)); - std::memcpy( - send_vector.data(), &acceleration_longitudinal_cog, sizeof(AccelerationLongitudinalCoGPacket)); - - for (auto & udp_driver : sensor_udp_drivers_) { - udp_driver->sender()->asyncSend(send_vector); - } - - return Status::OK; -} - -Status MultiContinentalARS548HwInterface::SetCharacteristicSpeed(float characteristic_speed) -{ - constexpr uint16_t CHARACTERISTIC_SPEED_SERVICE_ID = 0; - constexpr uint16_t CHARACTERISTIC_SPEED_METHOD_ID = 328; - constexpr uint8_t CHARACTERISTIC_SPEED_LENGTH = 11; - const int CHARACTERISTIC_SPEED_UDP_LENGTH = 19; - - if (characteristic_speed < 0.f || characteristic_speed > 255.f) { - PrintError("Invalid characteristic_speed value"); - return Status::ERROR_1; - } - - CharacteristicSpeedPacket characteristic_speed_packet{}; - static_assert(sizeof(CharacteristicSpeedPacket) == CHARACTERISTIC_SPEED_UDP_LENGTH); - characteristic_speed_packet.header.service_id = CHARACTERISTIC_SPEED_SERVICE_ID; - characteristic_speed_packet.header.method_id = CHARACTERISTIC_SPEED_METHOD_ID; - characteristic_speed_packet.header.length = CHARACTERISTIC_SPEED_LENGTH; - characteristic_speed_packet.characteristic_speed = characteristic_speed; - - std::vector send_vector(sizeof(CharacteristicSpeedPacket)); - std::memcpy(send_vector.data(), &characteristic_speed_packet, sizeof(CharacteristicSpeedPacket)); - - for (auto & udp_driver : sensor_udp_drivers_) { - udp_driver->sender()->asyncSend(send_vector); - } - - return Status::OK; -} - -Status MultiContinentalARS548HwInterface::SetDrivingDirection(int direction) -{ - constexpr uint16_t DRIVING_DIRECTION_SERVICE_ID = 0; - constexpr uint16_t DRIVING_DIRECTION_METHOD_ID = 325; - constexpr uint8_t DRIVING_DIRECTION_LENGTH = 22; - constexpr uint8_t DRIVING_DIRECTION_STANDSTILL = 0; - constexpr uint8_t DRIVING_DIRECTION_FORWARD = 1; - constexpr uint8_t DRIVING_DIRECTION_BACKWARDS = 2; - const int DRIVING_DIRECTION_UDP_LENGTH = 30; - - DrivingDirectionPacket driving_direction_packet{}; - static_assert(sizeof(DrivingDirectionPacket) == DRIVING_DIRECTION_UDP_LENGTH); - driving_direction_packet.header.service_id = DRIVING_DIRECTION_SERVICE_ID; - driving_direction_packet.header.method_id = DRIVING_DIRECTION_METHOD_ID; - driving_direction_packet.header.length = DRIVING_DIRECTION_LENGTH; - - if (direction == 0) { - driving_direction_packet.driving_direction = DRIVING_DIRECTION_STANDSTILL; - } else if (direction > 0) { - driving_direction_packet.driving_direction = DRIVING_DIRECTION_FORWARD; - } else { - driving_direction_packet.driving_direction = DRIVING_DIRECTION_BACKWARDS; - } - - std::vector send_vector(sizeof(DrivingDirectionPacket)); - std::memcpy(send_vector.data(), &driving_direction_packet, sizeof(DrivingDirectionPacket)); - - for (auto & udp_driver : sensor_udp_drivers_) { - udp_driver->sender()->asyncSend(send_vector); - } - - return Status::OK; -} - -Status MultiContinentalARS548HwInterface::SetSteeringAngleFrontAxle(float angle_rad) -{ - constexpr uint16_t STEERING_ANGLE_SERVICE_ID = 0; - constexpr uint16_t STEERING_ANGLE_METHOD_ID = 327; - constexpr uint8_t STEERING_ANGLE_LENGTH = 32; - const int STEERING_ANGLE_UDP_LENGTH = 40; - - if (angle_rad < -90.f || angle_rad > 90.f) { - PrintError("Invalid angle_rad value"); - return Status::ERROR_1; - } - - SteeringAngleFrontAxlePacket steering_angle_front_axle_packet{}; - static_assert(sizeof(SteeringAngleFrontAxlePacket) == STEERING_ANGLE_UDP_LENGTH); - steering_angle_front_axle_packet.header.service_id = STEERING_ANGLE_SERVICE_ID; - steering_angle_front_axle_packet.header.method_id = STEERING_ANGLE_METHOD_ID; - steering_angle_front_axle_packet.header.length = STEERING_ANGLE_LENGTH; - steering_angle_front_axle_packet.steering_angle_front_axle = angle_rad; - - std::vector send_vector(sizeof(SteeringAngleFrontAxlePacket)); - std::memcpy( - send_vector.data(), &steering_angle_front_axle_packet, sizeof(SteeringAngleFrontAxlePacket)); - - for (auto & udp_driver : sensor_udp_drivers_) { - udp_driver->sender()->asyncSend(send_vector); - } - - return Status::OK; -} - -Status MultiContinentalARS548HwInterface::SetVelocityVehicle(float velocity_kmh) -{ - if (velocity_kmh < 0.f || velocity_kmh > 350.f) { - PrintError("Invalid velocity value"); - return Status::ERROR_1; - } - - constexpr uint16_t VELOCITY_VEHICLE_SERVICE_ID = 0; - constexpr uint16_t VELOCITY_VEHICLE_METHOD_ID = 323; - constexpr uint8_t VELOCITY_VEHICLE_LENGTH = 28; - const int VELOCITY_VEHICLE_UDP_SIZE = 36; - - VelocityVehiclePacket steering_angle_front_axle_packet{}; - static_assert(sizeof(VelocityVehiclePacket) == VELOCITY_VEHICLE_UDP_SIZE); - steering_angle_front_axle_packet.header.service_id = VELOCITY_VEHICLE_SERVICE_ID; - steering_angle_front_axle_packet.header.method_id = VELOCITY_VEHICLE_METHOD_ID; - steering_angle_front_axle_packet.header.length = VELOCITY_VEHICLE_LENGTH; - steering_angle_front_axle_packet.velocity_vehicle = velocity_kmh; - - std::vector send_vector(sizeof(VelocityVehiclePacket)); - std::memcpy(send_vector.data(), &steering_angle_front_axle_packet, sizeof(VelocityVehiclePacket)); - - for (auto & udp_driver : sensor_udp_drivers_) { - udp_driver->sender()->asyncSend(send_vector); - } - - return Status::OK; -} - -Status MultiContinentalARS548HwInterface::SetYawRate(float yaw_rate) -{ - if (yaw_rate < -163.83 || yaw_rate > 163.83) { - PrintError("Invalid yaw rate value"); - return Status::ERROR_1; - } - - constexpr uint16_t YAW_RATE_SERVICE_ID = 0; - constexpr uint16_t YAW_RATE_METHOD_ID = 326; - constexpr uint8_t YAW_RATE_LENGTH = 32; - const int YAW_RATE_UDP_SIZE = 40; - - YawRatePacket yaw_rate_packet{}; - static_assert(sizeof(YawRatePacket) == YAW_RATE_UDP_SIZE); - yaw_rate_packet.header.service_id = YAW_RATE_SERVICE_ID; - yaw_rate_packet.header.method_id = YAW_RATE_METHOD_ID; - yaw_rate_packet.header.length = YAW_RATE_LENGTH; - yaw_rate_packet.yaw_rate = yaw_rate; - - std::vector send_vector(sizeof(YawRatePacket)); - std::memcpy(send_vector.data(), &yaw_rate_packet, sizeof(YawRatePacket)); - - for (auto & udp_driver : sensor_udp_drivers_) { - udp_driver->sender()->asyncSend(send_vector); - } - - return Status::OK; -} - -void MultiContinentalARS548HwInterface::SetLogger(std::shared_ptr logger) -{ - parent_node_logger = logger; -} - -void MultiContinentalARS548HwInterface::PrintInfo(std::string info) -{ - if (parent_node_logger) { - RCLCPP_INFO_STREAM((*parent_node_logger), info); - } else { - std::cout << info << std::endl; - } -} - -void MultiContinentalARS548HwInterface::PrintError(std::string error) -{ - if (parent_node_logger) { - RCLCPP_ERROR_STREAM((*parent_node_logger), error); - } else { - std::cerr << error << std::endl; - } -} - -void MultiContinentalARS548HwInterface::PrintDebug(std::string debug) -{ - if (parent_node_logger) { - RCLCPP_DEBUG_STREAM((*parent_node_logger), debug); - } else { - std::cout << debug << std::endl; - } -} - -void MultiContinentalARS548HwInterface::PrintDebug(const std::vector & bytes) -{ - std::stringstream ss; - for (const auto & b : bytes) { - ss << static_cast(b) << ", "; - } - ss << std::endl; - PrintDebug(ss.str()); -} - -} // namespace continental_ars548 -} // namespace drivers -} // namespace nebula diff --git a/nebula_ros/CMakeLists.txt b/nebula_ros/CMakeLists.txt index be5377e91..0c54ae945 100644 --- a/nebula_ros/CMakeLists.txt +++ b/nebula_ros/CMakeLists.txt @@ -78,30 +78,16 @@ rclcpp_components_register_node(robosense_ros_wrapper ) ## Continental -# Hw Interface -ament_auto_add_library(continental_ars548_hw_ros_wrapper SHARED - src/continental/continental_ars548_hw_interface_ros_wrapper.cpp - ) -rclcpp_components_register_node(continental_ars548_hw_ros_wrapper - PLUGIN "ContinentalARS548HwInterfaceRosWrapper" - EXECUTABLE continental_ars548_hw_interface_ros_wrapper_node - ) - -ament_auto_add_library(multi_continental_ars548_hw_ros_wrapper SHARED - src/continental/multi_continental_ars548_hw_interface_ros_wrapper.cpp - ) -rclcpp_components_register_node(multi_continental_ars548_hw_ros_wrapper - PLUGIN "MultiContinentalARS548HwInterfaceRosWrapper" - EXECUTABLE multi_continental_ars548_hw_interface_ros_wrapper_node +ament_auto_add_library(continental_ars548_ros_wrapper SHARED + src/continental/continental_ars548_ros_wrapper.cpp + src/continental/continental_ars548_decoder_wrapper.cpp + src/continental/continental_ars548_hw_interface_wrapper.cpp + src/common/parameter_descriptors.cpp ) -# DriverDecoder -ament_auto_add_library(continental_ars548_driver_ros_wrapper SHARED - src/continental/continental_ars548_decoder_ros_wrapper.cpp - ) -rclcpp_components_register_node(continental_ars548_driver_ros_wrapper - PLUGIN "ContinentalARS548DriverRosWrapper" - EXECUTABLE continental_ars548_driver_ros_wrapper_node +rclcpp_components_register_node(continental_ars548_ros_wrapper + PLUGIN "ContinentalArs548RosWrapper" + EXECUTABLE continental_ars548_ros_wrapper_node ) if(BUILD_TESTING) diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp similarity index 64% rename from nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_ros_wrapper.hpp rename to nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp index 059388639..0b68c03b9 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp @@ -12,18 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NEBULA_ContinentalARS548DriverRosWrapper_H -#define NEBULA_ContinentalARS548DriverRosWrapper_H +#pragma once -#include #include #include -#include +#include #include -#include -#include +#include +#include #include -#include #include #include @@ -34,75 +31,39 @@ #include #include #include +#include #include -#include +#include #include +#include #include +#include namespace nebula { namespace ros { -/// @brief Ros wrapper of continental radar ethernet driver -class ContinentalARS548DriverRosWrapper final : public rclcpp::Node, NebulaDriverRosWrapperBase +class ContinentalArs548DecoderWrapper { - std::shared_ptr decoder_ptr_; - Status wrapper_status_; - - rclcpp::Subscription::SharedPtr packets_sub_; - - rclcpp::Publisher::SharedPtr - detection_list_pub_; - rclcpp::Publisher::SharedPtr object_list_pub_; - rclcpp::Publisher::SharedPtr object_pointcloud_pub_; - rclcpp::Publisher::SharedPtr detection_pointcloud_pub_; - rclcpp::Publisher::SharedPtr scan_raw_pub_; - rclcpp::Publisher::SharedPtr objects_raw_pub_; - rclcpp::Publisher::SharedPtr objects_markers_pub_; - rclcpp::Publisher::SharedPtr diagnostics_pub_; - - std::unordered_set previous_ids_; - - constexpr static int REFERENCE_POINTS_NUM = 9; - constexpr static std::array, 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}}}}; +public: + ContinentalArs548DecoderWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr< + const nebula::drivers::continental_ars548::ContinentalArs548SensorConfiguration> & config, + bool launch_hw); - std::shared_ptr - sensor_cfg_ptr_; + void ProcessPacket(std::unique_ptr packet_msg); - drivers::continental_ars548::ContinentalARS548HwInterface hw_interface_; + void OnConfigChange( + const std::shared_ptr< + const nebula::drivers::continental_ars548::ContinentalArs548SensorConfiguration> & + new_config); - /// @brief Initializing ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @return Resulting status - Status InitializeDriver(std::shared_ptr sensor_configuration); + rcl_interfaces::msg::SetParametersResult OnParameterChange( + const std::vector & p); - /// @brief Get configurations from ros parameters - /// @param sensor_configuration Output of SensorConfiguration - /// @param calibration_configuration Output of CalibrationConfiguration - /// @param correction_configuration Output of CorrectionConfiguration (for AT) - /// @return Resulting status - Status GetParameters( - drivers::continental_ars548::ContinentalARS548SensorConfiguration & sensor_configuration); - - /// @brief Convert seconds to chrono::nanoseconds - /// @param seconds - /// @return chrono::nanoseconds - static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) - { - return std::chrono::duration_cast( - std::chrono::duration(seconds)); - } + nebula::Status Status(); /// @brief Callback to process new ContinentalArs548DetectionList from the driver /// @param msg The new ContinentalArs548DetectionList from the driver @@ -113,32 +74,26 @@ class ContinentalARS548DriverRosWrapper final : public rclcpp::Node, NebulaDrive /// @param msg The new ContinentalArs548ObjectList from the driver void ObjectListCallback(std::unique_ptr msg); - /// @brief Callback to process new ContinentalARS548Status from the driver + /// @brief Callback to process new ContinentalArs548Status from the driver /// @param msg The new ContinentalArs548ObjectList from the driver void SensorStatusCallback( - const drivers::continental_ars548::ContinentalARS548Status & sensor_status); + const drivers::continental_ars548::ContinentalArs548Status & sensor_status); -public: - explicit ContinentalARS548DriverRosWrapper(const rclcpp::NodeOptions & options); - - /// @brief Callback for NebulaPackets subscriber - /// @param scan_msg Received NebulaPackets - void ReceivePacketsMsgCallback(const nebula_msgs::msg::NebulaPackets::SharedPtr scan_msg); - - /// @brief Get current status of this driver - /// @return Current status - Status GetStatus(); +private: + nebula::Status InitializeDriver( + const std::shared_ptr< + const nebula::drivers::continental_ars548::ContinentalArs548SensorConfiguration> & config); /// @brief Convert ARS548 detections to a pointcloud /// @param msg The ARS548 detection list msg /// @return Resulting detection pointcloud - pcl::PointCloud::Ptr + pcl::PointCloud::Ptr ConvertToPointcloud(const continental_msgs::msg::ContinentalArs548DetectionList & msg); /// @brief Convert ARS548 objects to a pointcloud /// @param msg The ARS548 object list msg /// @return Resulting object pointcloud - pcl::PointCloud::Ptr ConvertToPointcloud( + pcl::PointCloud::Ptr ConvertToPointcloud( const continental_msgs::msg::ContinentalArs548ObjectList & msg); /// @brief Convert ARS548 detections to a standard RadarScan msg @@ -158,9 +113,53 @@ class ContinentalARS548DriverRosWrapper final : public rclcpp::Node, NebulaDrive /// @return Resulting MarkerArray msg visualization_msgs::msg::MarkerArray ConvertToMarkers( const continental_msgs::msg::ContinentalArs548ObjectList & msg); -}; + /// @brief Convert seconds to chrono::nanoseconds + /// @param seconds + /// @return chrono::nanoseconds + static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) + { + return std::chrono::duration_cast( + std::chrono::duration(seconds)); + } + + nebula::Status status_; + rclcpp::Logger logger_; + + std::shared_ptr + sensor_cfg_{}; + + std::shared_ptr driver_ptr_{}; + std::mutex mtx_driver_ptr_; + + rclcpp::Publisher::SharedPtr packets_pub_{}; + + rclcpp::Publisher::SharedPtr + detection_list_pub_{}; + rclcpp::Publisher::SharedPtr + object_list_pub_{}; + rclcpp::Publisher::SharedPtr object_pointcloud_pub_{}; + rclcpp::Publisher::SharedPtr detection_pointcloud_pub_{}; + rclcpp::Publisher::SharedPtr scan_raw_pub_{}; + rclcpp::Publisher::SharedPtr objects_raw_pub_{}; + rclcpp::Publisher::SharedPtr objects_markers_pub_{}; + rclcpp::Publisher::SharedPtr diagnostics_pub_{}; + + std::unordered_set previous_ids_; + + constexpr static int REFERENCE_POINTS_NUM = 9; + constexpr static std::array, 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 cloud_watchdog_; +}; } // namespace ros } // namespace nebula - -#endif // NEBULA_ContinentalARS548DriverRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp similarity index 55% rename from nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp rename to nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp index f9b66e951..4250a5031 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp @@ -12,16 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NEBULA_ContinentalARS548HwInterfaceRosWrapper_H -#define NEBULA_ContinentalARS548HwInterfaceRosWrapper_H +#pragma once -#include -#include -#include +#include #include -#include +#include #include -#include #include #include @@ -29,77 +25,35 @@ #include #include #include -#include -#include -#include -#include - -#include #include -#include -#include -#include -#include namespace nebula { namespace ros { -/// @brief Get parameter from rclcpp::Parameter -/// @tparam T -/// @param p Parameter from rclcpp parameter callback -/// @param name Target parameter name -/// @param value Corresponding value -/// @return Whether the target name existed -template -bool get_param(const std::vector & p, const std::string & name, T & value) -{ - auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { - return parameter.get_name() == name; - }); - if (it != p.cend()) { - value = it->template get_value(); - return true; - } - return false; -} - -/// @brief Hardware interface ros wrapper of continental radar ethernet driver -class ContinentalARS548HwInterfaceRosWrapper final : public rclcpp::Node, - NebulaHwInterfaceWrapperBase +class ContinentalArs548HwInterfaceWrapper { - drivers::continental_ars548::ContinentalARS548HwInterface hw_interface_; - Status interface_status_; +public: + ContinentalArs548HwInterfaceWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr & + config); - drivers::continental_ars548::ContinentalARS548SensorConfiguration sensor_configuration_; + /// @brief Starts the hw interface and subscribes to the input topics + void SensorInterfaceStart(); - /// @brief Received Continental Radar message publisher - rclcpp::Publisher::SharedPtr packets_pub_; - rclcpp::Subscription::SharedPtr odometry_sub_; - rclcpp::Subscription::SharedPtr acceleration_sub_; - rclcpp::Subscription::SharedPtr steering_angle_sub_; - - bool standstill_{true}; + void OnConfigChange( + const std::shared_ptr & + new_config); - rclcpp::Service::SharedPtr - set_network_configuration_service_server_; - rclcpp::Service::SharedPtr - set_sensor_mounting_service_server_; - rclcpp::Service::SharedPtr - set_vehicle_parameters_service_server_; - rclcpp::Service::SharedPtr - set_radar_parameters_service_server_; + /// @brief Get current status of the hw interface + /// @return Current status + nebula::Status Status(); - /// @brief Initializing hardware interface ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @return Resulting status - Status InitializeHwInterface( - const drivers::SensorConfigurationBase & sensor_configuration) override; - /// @brief Callback for receiving NebulaPackets - /// @param packets_buffer Received NebulaPackets - void ReceivePacketsDataCallback(std::unique_ptr packets_buffer); + std::shared_ptr HwInterface() const; +private: /// @brief Callback to send the odometry information to the radar device /// @param msg The odometry message void OdometryCallback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); @@ -148,41 +102,27 @@ class ContinentalARS548HwInterfaceRosWrapper final : public rclcpp::Node, const std::shared_ptr response); -public: - explicit ContinentalARS548HwInterfaceRosWrapper(const rclcpp::NodeOptions & options); - ~ContinentalARS548HwInterfaceRosWrapper() noexcept override; - - /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) - /// @return Resulting status - Status StreamStart() override; - /// @brief Stop point cloud streaming (not used) - /// @return Resulting status - Status StreamStop() override; - /// @brief Shutdown (not used) - /// @return Resulting status - Status Shutdown() override; - /// @brief Get configurations from ros parameters - /// @param sensor_configuration Output of SensorConfiguration - /// @return Resulting status - Status GetParameters( - drivers::continental_ars548::ContinentalARS548SensorConfiguration & sensor_configuration); + rclcpp::Node * const parent_node_; + std::shared_ptr hw_interface_; + rclcpp::Logger logger_; + nebula::Status status_; + std::shared_ptr + config_; -private: - std::mutex mtx_config_; - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - - /// @brief rclcpp parameter callback - /// @param parameters Received parameters - /// @return SetParametersResult - rcl_interfaces::msg::SetParametersResult paramCallback( - const std::vector & parameters); - - /// @brief Updating rclcpp parameter - /// @return SetParametersResult - std::vector updateParameters(); -}; + rclcpp::Subscription::SharedPtr odometry_sub_; + rclcpp::Subscription::SharedPtr acceleration_sub_; + rclcpp::Subscription::SharedPtr steering_angle_sub_; + bool standstill_{true}; + + rclcpp::Service::SharedPtr + set_network_configuration_service_server_; + rclcpp::Service::SharedPtr + set_sensor_mounting_service_server_; + rclcpp::Service::SharedPtr + set_vehicle_parameters_service_server_; + rclcpp::Service::SharedPtr + set_radar_parameters_service_server_; +}; } // namespace ros } // namespace nebula - -#endif // NEBULA_ContinentalARS548HwInterfaceRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp new file mode 100644 index 000000000..ebff025df --- /dev/null +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp @@ -0,0 +1,106 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace nebula +{ +namespace ros +{ + +/// @brief Ros wrapper of continental ars548 driver +class ContinentalArs548RosWrapper final : public rclcpp::Node +{ +public: + explicit ContinentalArs548RosWrapper(const rclcpp::NodeOptions & options); + ~ContinentalArs548RosWrapper() noexcept {}; + + /// @brief Get current status of this driver + /// @return Current status + Status GetStatus(); + + /// @brief Start data streaming (Call SensorInterfaceStart of HwInterface) + /// @return Resulting status + Status StreamStart(); + +private: + /// @brief Callback from the hw interface's raw data + void ReceivePacketCallback(std::vector & packet); + + /// @brief Callback from replayed NebulaPackets + void ReceivePacketsMessageCallback(std::unique_ptr packets_msg); + + /// @brief Retrieve the parameters from ROS and set the driver and hw interface + /// @return Resulting status + Status DeclareAndGetSensorConfigParams(); + + /// @brief rclcpp parameter callback + /// @param parameters Received parameters + /// @return SetParametersResult + rcl_interfaces::msg::SetParametersResult OnParameterChange( + const std::vector & p); + + Status ValidateAndSetConfig( + std::shared_ptr & + new_config); + + Status wrapper_status_; + + std::shared_ptr + sensor_cfg_ptr_{}; + + /// @brief Stores received packets that have not been processed yet by the decoder thread + mt_queue> packet_queue_; + /// @brief Thread to isolate decoding from receiving + std::thread decoder_thread_; + + rclcpp::Subscription::SharedPtr packets_sub_{}; + + bool launch_hw_{}; + + std::optional hw_interface_wrapper_{}; + std::optional decoder_wrapper_{}; + + std::mutex mtx_config_; + + OnSetParametersCallbackHandle::SharedPtr parameter_event_cb_; +}; + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/continental/multi_continental_ars548_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/multi_continental_ars548_hw_interface_ros_wrapper.hpp deleted file mode 100644 index 19429b99c..000000000 --- a/nebula_ros/include/nebula_ros/continental/multi_continental_ars548_hw_interface_ros_wrapper.hpp +++ /dev/null @@ -1,146 +0,0 @@ -// 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. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef NEBULA_ContinentalARS548HwInterfaceRosWrapper_H -#define NEBULA_ContinentalARS548HwInterfaceRosWrapper_H - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include - -namespace nebula -{ -namespace ros -{ -/// @brief Get parameter from rclcpp::Parameter -/// @tparam T -/// @param p Parameter from rclcpp parameter callback -/// @param name Target parameter name -/// @param value Corresponding value -/// @return Whether the target name existed -template -bool get_param(const std::vector & p, const std::string & name, T & value) -{ - auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { - return parameter.get_name() == name; - }); - if (it != p.cend()) { - value = it->template get_value(); - return true; - } - return false; -} - -/// @brief Hardware interface ros wrapper of continental radar ethernet driver -/// NOTE: this is a temporary class that acts as a single hw interface for multiple devices -/// The reason behind this is a not-so-efficient multicasting and package processing when having N -/// interfaces for N devices If we end up having problems because of that we may switch to this -/// implementation. Otherwise this implementation will be removed at a later date -class MultiContinentalARS548HwInterfaceRosWrapper final : public rclcpp::Node, - NebulaHwInterfaceWrapperBase -{ - drivers::continental_ars548::MultiContinentalARS548HwInterface hw_interface_; - Status interface_status_; - - drivers::continental_ars548::MultiContinentalARS548SensorConfiguration sensor_configuration_; - - /// @brief Received Continental Radar message publisher - std::unordered_map::SharedPtr> - packets_pub_map_; - rclcpp::Subscription::SharedPtr odometry_sub_; - rclcpp::Subscription::SharedPtr acceleration_sub_; - rclcpp::Subscription::SharedPtr steering_angle_sub_; - - bool standstill_{true}; - - /// @brief Initializing hardware interface ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @return Resulting status - Status InitializeHwInterface( - const drivers::SensorConfigurationBase & sensor_configuration) override; - /// @brief Callback for receiving NebulaPackets - /// @param packets_buffer Received NebulaPackets - void ReceivePacketsDataCallback( - std::unique_ptr packets_buffer, const std::string & sensor_ip); - - /// @brief Callback to send the odometry information to the radar device - /// @param msg The odometry message - void OdometryCallback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); - - /// @brief Callback to send the acceleration information to the radar device - /// @param msg The acceleration message - void AccelerationCallback(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg); - - /// @brief Callback to send the steering angle information to the radar device - /// @param msg The steering angle message - void SteeringAngleCallback(const std_msgs::msg::Float32::SharedPtr msg); - -public: - explicit MultiContinentalARS548HwInterfaceRosWrapper(const rclcpp::NodeOptions & options); - ~MultiContinentalARS548HwInterfaceRosWrapper() noexcept override; - - /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) - /// @return Resulting status - Status StreamStart() override; - /// @brief Stop point cloud streaming (not used) - /// @return Resulting status - Status StreamStop() override; - /// @brief Shutdown (not used) - /// @return Resulting status - Status Shutdown() override; - /// @brief Get configurations from ros parameters - /// @param sensor_configuration Output of SensorConfiguration - /// @return Resulting status - Status GetParameters( - drivers::continental_ars548::MultiContinentalARS548SensorConfiguration & sensor_configuration); - -private: - std::mutex mtx_config_; - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - - /// @brief rclcpp parameter callback - /// @param parameters Received parameters - /// @return SetParametersResult - rcl_interfaces::msg::SetParametersResult paramCallback( - const std::vector & parameters); - - /// @brief Updating rclcpp parameter - /// @return SetParametersResult - std::vector updateParameters(); -}; - -} // namespace ros -} // namespace nebula - -#endif // NEBULA_ContinentalARS548HwInterfaceRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp index 0a60d3e8e..63ce94915 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp @@ -45,7 +45,7 @@ class HesaiRosWrapper final : public rclcpp::Node /// @return Current status Status GetStatus(); - /// @brief Start point cloud streaming (Call CloudInterfaceStart of HwInterface) + /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) /// @return Resulting status Status StreamStart(); diff --git a/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp index 93cfc5b0c..5b8d17697 100644 --- a/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp @@ -46,7 +46,7 @@ class VelodyneRosWrapper final : public rclcpp::Node /// @return Current status Status GetStatus(); - /// @brief Start point cloud streaming (Call CloudInterfaceStart of HwInterface) + /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) /// @return Resulting status Status StreamStart(); diff --git a/nebula_ros/launch/continental_launch_all_hw.xml b/nebula_ros/launch/continental_launch_all_hw.xml index d5ed36078..24016d89c 100644 --- a/nebula_ros/launch/continental_launch_all_hw.xml +++ b/nebula_ros/launch/continental_launch_all_hw.xml @@ -26,29 +26,14 @@ - - - - - - - - - - - - - - - + - + diff --git a/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp similarity index 70% rename from nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp rename to nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp index ecad7752f..f6d4e69ba 100644 --- a/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include @@ -20,222 +20,120 @@ namespace nebula { namespace ros { -ContinentalARS548DriverRosWrapper::ContinentalARS548DriverRosWrapper( - const rclcpp::NodeOptions & options) -: rclcpp::Node("continental_ars548_driver_ros_wrapper", options), hw_interface_() +ContinentalArs548DecoderWrapper::ContinentalArs548DecoderWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr & + config, + bool launch_hw) +: status_(nebula::Status::NOT_INITIALIZED), + logger_(parent_node->get_logger().get_child("ContinentalArs548Decoder")), + sensor_cfg_(config) { - drivers::continental_ars548::ContinentalARS548SensorConfiguration sensor_configuration; + using std::chrono_literals::operator""us; + if (!config) { + throw std::runtime_error( + "ContinentalArs548DecoderWrapper cannot be instantiated without a valid config!"); + } - setvbuf(stdout, NULL, _IONBF, BUFSIZ); + RCLCPP_INFO(logger_, "Starting Decoder"); - hw_interface_.SetLogger(std::make_shared(this->get_logger())); + InitializeDriver(config); + status_ = driver_ptr_->GetStatus(); - wrapper_status_ = GetParameters(sensor_configuration); - if (Status::OK != wrapper_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); - return; + if (Status::OK != status_) { + throw std::runtime_error( + (std::stringstream() << "Error instantiating decoder: " << status_).str()); } - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); - - sensor_cfg_ptr_ = - std::make_shared( - sensor_configuration); - wrapper_status_ = InitializeDriver( - std::const_pointer_cast( - sensor_cfg_ptr_)); + // Publish packets only if HW interface is connected + if (launch_hw) { + packets_pub_ = parent_node->create_publisher( + "nebula_packets", rclcpp::SensorDataQoS()); + } - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); - packets_sub_ = create_subscription( - "nebula_packets", rclcpp::SensorDataQoS(), - std::bind( - &ContinentalARS548DriverRosWrapper::ReceivePacketsMsgCallback, this, std::placeholders::_1)); + auto qos_profile = rmw_qos_profile_sensor_data; + auto pointcloud_qos = + rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); detection_list_pub_ = - this->create_publisher( + parent_node->create_publisher( "continental_detections", rclcpp::SensorDataQoS()); - object_list_pub_ = this->create_publisher( - "continental_objects", rclcpp::SensorDataQoS()); + object_list_pub_ = + parent_node->create_publisher( + "continental_objects", rclcpp::SensorDataQoS()); - detection_pointcloud_pub_ = this->create_publisher( + detection_pointcloud_pub_ = parent_node->create_publisher( "detection_points", rclcpp::SensorDataQoS()); object_pointcloud_pub_ = - this->create_publisher("object_points", rclcpp::SensorDataQoS()); + parent_node->create_publisher("object_points", pointcloud_qos); scan_raw_pub_ = - this->create_publisher("scan_raw", rclcpp::SensorDataQoS()); + parent_node->create_publisher("scan_raw", pointcloud_qos); objects_raw_pub_ = - this->create_publisher("objects_raw", rclcpp::SensorDataQoS()); + parent_node->create_publisher("objects_raw", pointcloud_qos); objects_markers_pub_ = - this->create_publisher("marker_array", 10); + parent_node->create_publisher("marker_array", 10); diagnostics_pub_ = - this->create_publisher("diagnostics", 10); -} + parent_node->create_publisher("diagnostics", 10); -void ContinentalARS548DriverRosWrapper::ReceivePacketsMsgCallback( - const nebula_msgs::msg::NebulaPackets::SharedPtr scan_msg) -{ - decoder_ptr_->ProcessPackets(*scan_msg); + RCLCPP_INFO_STREAM(logger_, ". Wrapper=" << status_); + + cloud_watchdog_ = + std::make_shared(*parent_node, 100'000us, [this, parent_node](bool ok) { + if (ok) return; + RCLCPP_WARN_THROTTLE( + logger_, *parent_node->get_clock(), 5000, "Missed pointcloud output deadline"); + }); } -Status ContinentalARS548DriverRosWrapper::InitializeDriver( - std::shared_ptr sensor_configuration) +Status ContinentalArs548DecoderWrapper::InitializeDriver( + const std::shared_ptr< + const nebula::drivers::continental_ars548::ContinentalArs548SensorConfiguration> & config) { - decoder_ptr_ = std::make_shared( - std::static_pointer_cast( - sensor_configuration)); + driver_ptr_.reset(); + driver_ptr_ = std::make_shared(config); - decoder_ptr_->RegisterDetectionListCallback(std::bind( - &ContinentalARS548DriverRosWrapper::DetectionListCallback, this, std::placeholders::_1)); - decoder_ptr_->RegisterObjectListCallback( - std::bind(&ContinentalARS548DriverRosWrapper::ObjectListCallback, this, std::placeholders::_1)); - decoder_ptr_->RegisterSensorStatusCallback(std::bind( - &ContinentalARS548DriverRosWrapper::SensorStatusCallback, this, std::placeholders::_1)); + driver_ptr_->RegisterDetectionListCallback(std::bind( + &ContinentalArs548DecoderWrapper::DetectionListCallback, this, std::placeholders::_1)); + driver_ptr_->RegisterObjectListCallback( + std::bind(&ContinentalArs548DecoderWrapper::ObjectListCallback, this, std::placeholders::_1)); + driver_ptr_->RegisterSensorStatusCallback( + std::bind(&ContinentalArs548DecoderWrapper::SensorStatusCallback, this, std::placeholders::_1)); return Status::OK; } -Status ContinentalARS548DriverRosWrapper::GetStatus() +void ContinentalArs548DecoderWrapper::OnConfigChange( + const std::shared_ptr< + const nebula::drivers::continental_ars548::ContinentalArs548SensorConfiguration> & new_config) { - return wrapper_status_; + std::lock_guard lock(mtx_driver_ptr_); + InitializeDriver(new_config); + sensor_cfg_ = new_config; } -Status ContinentalARS548DriverRosWrapper::GetParameters( - drivers::continental_ars548::ContinentalARS548SensorConfiguration & sensor_configuration) +void ContinentalArs548DecoderWrapper::ProcessPacket( + std::unique_ptr packet_msg) { - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("host_ip", descriptor); - sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_ip", descriptor); - sensor_configuration.sensor_ip = this->get_parameter("sensor_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("data_port", descriptor); - sensor_configuration.data_port = this->get_parameter("data_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model"); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("base_frame", descriptor); - sensor_configuration.base_frame = this->get_parameter("base_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("object_frame", descriptor); - sensor_configuration.object_frame = this->get_parameter("object_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("use_sensor_time", descriptor); - sensor_configuration.use_sensor_time = this->get_parameter("use_sensor_time").as_bool(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_length", descriptor); - sensor_configuration.configuration_vehicle_length = - static_cast(this->get_parameter("configuration_vehicle_length").as_double()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_width", descriptor); - sensor_configuration.configuration_vehicle_width = - static_cast(this->get_parameter("configuration_vehicle_width").as_double()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_height", descriptor); - sensor_configuration.configuration_vehicle_height = - static_cast(this->get_parameter("configuration_vehicle_height").as_double()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_wheelbase", descriptor); - sensor_configuration.configuration_vehicle_wheelbase = - static_cast(this->get_parameter("configuration_vehicle_wheelbase").as_double()); - } + driver_ptr_->ProcessPacket(*packet_msg); - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; + if ( + packets_pub_ && (packets_pub_->get_subscription_count() > 0 || + packets_pub_->get_intra_process_subscription_count() > 0)) { + auto nebula_packets_msg = std::make_unique(); + nebula_packets_msg->header.stamp = packet_msg->stamp; + nebula_packets_msg->header.frame_id = sensor_cfg_->frame_id; + nebula_packets_msg->packets.emplace_back(std::move(*packet_msg)); + packets_pub_->publish(std::move(nebula_packets_msg)); } - std::shared_ptr sensor_cfg_ptr = - std::make_shared( - sensor_configuration); - - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; + cloud_watchdog_->update(); } -void ContinentalARS548DriverRosWrapper::DetectionListCallback( +void ContinentalArs548DecoderWrapper::DetectionListCallback( std::unique_ptr msg) { if ( @@ -264,7 +162,7 @@ void ContinentalARS548DriverRosWrapper::DetectionListCallback( } } -void ContinentalARS548DriverRosWrapper::ObjectListCallback( +void ContinentalArs548DecoderWrapper::ObjectListCallback( std::unique_ptr msg) { if ( @@ -300,20 +198,20 @@ void ContinentalARS548DriverRosWrapper::ObjectListCallback( } } -void ContinentalARS548DriverRosWrapper::SensorStatusCallback( - const drivers::continental_ars548::ContinentalARS548Status & sensor_status) +void ContinentalArs548DecoderWrapper::SensorStatusCallback( + const drivers::continental_ars548::ContinentalArs548Status & sensor_status) { diagnostic_msgs::msg::DiagnosticArray diagnostic_array_msg; diagnostic_array_msg.header.stamp.sec = sensor_status.timestamp_seconds; diagnostic_array_msg.header.stamp.nanosec = sensor_status.timestamp_nanoseconds; - diagnostic_array_msg.header.frame_id = sensor_cfg_ptr_->frame_id; + diagnostic_array_msg.header.frame_id = sensor_cfg_->frame_id; diagnostic_array_msg.status.resize(1); auto & status = diagnostic_array_msg.status[0]; status.values.reserve(36); status.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - status.hardware_id = sensor_cfg_ptr_->frame_id; - status.name = sensor_cfg_ptr_->frame_id; + status.hardware_id = sensor_cfg_->frame_id; + status.name = sensor_cfg_->frame_id; status.message = "Diagnostic messages from ARS548"; auto add_diagnostic = [&status](const std::string & key, const std::string & value) { @@ -414,15 +312,15 @@ void ContinentalARS548DriverRosWrapper::SensorStatusCallback( diagnostics_pub_->publish(diagnostic_array_msg); } -pcl::PointCloud::Ptr -ContinentalARS548DriverRosWrapper::ConvertToPointcloud( +pcl::PointCloud::Ptr +ContinentalArs548DecoderWrapper::ConvertToPointcloud( const continental_msgs::msg::ContinentalArs548DetectionList & msg) { - pcl::PointCloud::Ptr output_pointcloud( - new pcl::PointCloud); + pcl::PointCloud::Ptr output_pointcloud( + new pcl::PointCloud); output_pointcloud->reserve(msg.detections.size()); - nebula::drivers::continental_ars548::PointARS548Detection point{}; + nebula::drivers::continental_ars548::PointArs548Detection point{}; for (const auto & detection : msg.detections) { point.x = std::cos(detection.elevation_angle) * std::cos(detection.azimuth_angle) * detection.range; @@ -454,15 +352,15 @@ ContinentalARS548DriverRosWrapper::ConvertToPointcloud( return output_pointcloud; } -pcl::PointCloud::Ptr -ContinentalARS548DriverRosWrapper::ConvertToPointcloud( +pcl::PointCloud::Ptr +ContinentalArs548DecoderWrapper::ConvertToPointcloud( const continental_msgs::msg::ContinentalArs548ObjectList & msg) { - pcl::PointCloud::Ptr output_pointcloud( - new pcl::PointCloud); + pcl::PointCloud::Ptr output_pointcloud( + new pcl::PointCloud); output_pointcloud->reserve(msg.objects.size()); - nebula::drivers::continental_ars548::PointARS548Object point{}; + nebula::drivers::continental_ars548::PointArs548Object point{}; for (const auto & object : msg.objects) { point.x = static_cast(object.position.x); point.y = static_cast(object.position.y); @@ -494,7 +392,7 @@ ContinentalARS548DriverRosWrapper::ConvertToPointcloud( return output_pointcloud; } -radar_msgs::msg::RadarScan ContinentalARS548DriverRosWrapper::ConvertToRadarScan( +radar_msgs::msg::RadarScan ContinentalArs548DecoderWrapper::ConvertToRadarScan( const continental_msgs::msg::ContinentalArs548DetectionList & msg) { radar_msgs::msg::RadarScan output_msg; @@ -520,7 +418,7 @@ radar_msgs::msg::RadarScan ContinentalARS548DriverRosWrapper::ConvertToRadarScan return output_msg; } -radar_msgs::msg::RadarTracks ContinentalARS548DriverRosWrapper::ConvertToRadarTracks( +radar_msgs::msg::RadarTracks ContinentalArs548DecoderWrapper::ConvertToRadarTracks( const continental_msgs::msg::ContinentalArs548ObjectList & msg) { radar_msgs::msg::RadarTracks output_msg; @@ -629,7 +527,7 @@ radar_msgs::msg::RadarTracks ContinentalARS548DriverRosWrapper::ConvertToRadarTr return output_msg; } -visualization_msgs::msg::MarkerArray ContinentalARS548DriverRosWrapper::ConvertToMarkers( +visualization_msgs::msg::MarkerArray ContinentalArs548DecoderWrapper::ConvertToMarkers( const continental_msgs::msg::ContinentalArs548ObjectList & msg) { visualization_msgs::msg::MarkerArray marker_array; @@ -692,7 +590,7 @@ visualization_msgs::msg::MarkerArray ContinentalARS548DriverRosWrapper::ConvertT current_ids.emplace(object.object_id); visualization_msgs::msg::Marker box_marker; - box_marker.header.frame_id = sensor_cfg_ptr_->object_frame; + box_marker.header.frame_id = sensor_cfg_->object_frame; box_marker.header.stamp = msg.header.stamp; box_marker.ns = "boxes"; box_marker.id = object.object_id; @@ -773,7 +671,7 @@ visualization_msgs::msg::MarkerArray ContinentalARS548DriverRosWrapper::ConvertT } visualization_msgs::msg::Marker delete_marker; - delete_marker.header.frame_id = sensor_cfg_ptr_->object_frame; + delete_marker.header.frame_id = sensor_cfg_->object_frame; delete_marker.header.stamp = msg.header.stamp; delete_marker.ns = "boxes"; delete_marker.id = previous_id; @@ -797,6 +695,15 @@ visualization_msgs::msg::MarkerArray ContinentalARS548DriverRosWrapper::ConvertT return marker_array; } -RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalARS548DriverRosWrapper) +nebula::Status ContinentalArs548DecoderWrapper::Status() +{ + std::lock_guard lock(mtx_driver_ptr_); + + if (!driver_ptr_) { + return nebula::Status::NOT_INITIALIZED; + } + + return driver_ptr_->GetStatus(); +} } // namespace ros } // namespace nebula diff --git a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp deleted file mode 100644 index d7baa3f6a..000000000 --- a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp +++ /dev/null @@ -1,561 +0,0 @@ -// 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. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include -#include - -#include -#include - -namespace nebula -{ -namespace ros -{ -ContinentalARS548HwInterfaceRosWrapper::ContinentalARS548HwInterfaceRosWrapper( - const rclcpp::NodeOptions & options) -: rclcpp::Node("continental_ars548_hw_interface_ros_wrapper", options), hw_interface_() -{ - if (mtx_config_.try_lock()) { - interface_status_ = GetParameters(sensor_configuration_); - mtx_config_.unlock(); - } - if (Status::OK != interface_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); - return; - } - hw_interface_.SetLogger(std::make_shared(this->get_logger())); - std::shared_ptr - sensor_cfg_ptr = - std::make_shared( - sensor_configuration_); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - - hw_interface_.RegisterScanCallback(std::bind( - &ContinentalARS548HwInterfaceRosWrapper::ReceivePacketsDataCallback, this, - std::placeholders::_1)); - packets_pub_ = this->create_publisher( - "nebula_packets", rclcpp::SensorDataQoS()); - - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&ContinentalARS548HwInterfaceRosWrapper::paramCallback, this, std::placeholders::_1)); - - StreamStart(); -} - -ContinentalARS548HwInterfaceRosWrapper::~ContinentalARS548HwInterfaceRosWrapper() -{ -} - -Status ContinentalARS548HwInterfaceRosWrapper::StreamStart() -{ - if (Status::OK == interface_status_) { - interface_status_ = hw_interface_.SensorInterfaceStart(); - } - - if (Status::OK == interface_status_) { - odometry_sub_ = this->create_subscription( - "odometry_input", rclcpp::QoS{1}, - std::bind( - &ContinentalARS548HwInterfaceRosWrapper::OdometryCallback, this, std::placeholders::_1)); - - acceleration_sub_ = create_subscription( - "acceleration_input", rclcpp::QoS{1}, - std::bind( - &ContinentalARS548HwInterfaceRosWrapper::AccelerationCallback, this, - std::placeholders::_1)); - - steering_angle_sub_ = create_subscription( - "steering_angle_input", rclcpp::SensorDataQoS(), - std::bind( - &ContinentalARS548HwInterfaceRosWrapper::SteeringAngleCallback, this, - std::placeholders::_1)); - - set_network_configuration_service_server_ = - this->create_service( - "set_network_configuration", - std::bind( - &ContinentalARS548HwInterfaceRosWrapper::SetNetworkConfigurationRequestCallback, this, - std::placeholders::_1, std::placeholders::_2)); - - set_sensor_mounting_service_server_ = - this->create_service( - "set_sensor_mounting", - std::bind( - &ContinentalARS548HwInterfaceRosWrapper::SetSensorMountingRequestCallback, this, - std::placeholders::_1, std::placeholders::_2)); - - set_vehicle_parameters_service_server_ = - this->create_service( - "set_vehicle_parameters", - std::bind( - &ContinentalARS548HwInterfaceRosWrapper::SetVehicleParametersRequestCallback, this, - std::placeholders::_1, std::placeholders::_2)); - - set_radar_parameters_service_server_ = - this->create_service( - "set_radar_parameters", - std::bind( - &ContinentalARS548HwInterfaceRosWrapper::SetRadarParametersRequestCallback, this, - std::placeholders::_1, std::placeholders::_2)); - } - - return interface_status_; -} - -Status ContinentalARS548HwInterfaceRosWrapper::StreamStop() -{ - return Status::OK; -} -Status ContinentalARS548HwInterfaceRosWrapper::Shutdown() -{ - return Status::OK; -} - -Status ContinentalARS548HwInterfaceRosWrapper::InitializeHwInterface( // todo: don't think - // this is needed - const drivers::SensorConfigurationBase & sensor_configuration) -{ - std::stringstream ss; - ss << sensor_configuration; - RCLCPP_DEBUG_STREAM(this->get_logger(), ss.str()); - return Status::OK; -} - -Status ContinentalARS548HwInterfaceRosWrapper::GetParameters( - drivers::continental_ars548::ContinentalARS548SensorConfiguration & sensor_configuration) -{ - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", descriptor); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("host_ip", descriptor); - sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_ip", descriptor); - sensor_configuration.sensor_ip = this->get_parameter("sensor_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("multicast_ip", descriptor); - sensor_configuration.multicast_ip = this->get_parameter("multicast_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("base_frame", descriptor); - sensor_configuration.base_frame = this->get_parameter("base_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("object_frame", descriptor); - sensor_configuration.object_frame = this->get_parameter("object_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("data_port", descriptor); - sensor_configuration.data_port = this->get_parameter("data_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_host_port", descriptor); - sensor_configuration.configuration_host_port = - this->get_parameter("configuration_host_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_sensor_port", descriptor); - sensor_configuration.configuration_sensor_port = - this->get_parameter("configuration_sensor_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("use_sensor_time", descriptor); - sensor_configuration.use_sensor_time = this->get_parameter("use_sensor_time").as_bool(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_length", descriptor); - sensor_configuration.configuration_vehicle_length = - static_cast(this->get_parameter("configuration_vehicle_length").as_double()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_width", descriptor); - sensor_configuration.configuration_vehicle_width = - static_cast(this->get_parameter("configuration_vehicle_width").as_double()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_height", descriptor); - sensor_configuration.configuration_vehicle_height = - static_cast(this->get_parameter("configuration_vehicle_height").as_double()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_wheelbase", descriptor); - sensor_configuration.configuration_vehicle_wheelbase = - static_cast(this->get_parameter("configuration_vehicle_wheelbase").as_double()); - } - - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; -} - -void ContinentalARS548HwInterfaceRosWrapper::ReceivePacketsDataCallback( - std::unique_ptr scan_buffer) -{ - packets_pub_->publish(std::move(scan_buffer)); -} - -rcl_interfaces::msg::SetParametersResult ContinentalARS548HwInterfaceRosWrapper::paramCallback( - const std::vector & p) -{ - std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback"); - RCLCPP_DEBUG_STREAM(this->get_logger(), p); - RCLCPP_DEBUG_STREAM(this->get_logger(), sensor_configuration_); - RCLCPP_INFO_STREAM(this->get_logger(), p); - - drivers::continental_ars548::ContinentalARS548SensorConfiguration new_param{ - sensor_configuration_}; - RCLCPP_INFO_STREAM(this->get_logger(), new_param); - std::string sensor_model_str; - - if ( - get_param(p, "sensor_model", sensor_model_str) | get_param(p, "host_ip", new_param.host_ip) | - get_param(p, "sensor_ip", new_param.sensor_ip) | get_param(p, "frame_id", new_param.frame_id) | - get_param(p, "data_port", new_param.data_port) | - get_param(p, "multicast_ip", new_param.multicast_ip) | - get_param(p, "base_frame", new_param.base_frame) | - get_param(p, "object_frame", new_param.object_frame) | - get_param(p, "configuration_host_port", new_param.configuration_host_port) | - get_param(p, "configuration_sensor_port", new_param.configuration_sensor_port) | - get_param(p, "configuration_vehicle_length", new_param.configuration_vehicle_length) | - get_param(p, "configuration_vehicle_width", new_param.configuration_vehicle_width) | - get_param(p, "configuration_vehicle_height", new_param.configuration_vehicle_height) | - get_param(p, "configuration_vehicle_wheelbase", new_param.configuration_vehicle_wheelbase) | - get_param(p, "configuration_host_port", new_param.configuration_host_port) | - get_param(p, "configuration_sensor_port", new_param.configuration_sensor_port)) { - if (0 < sensor_model_str.length()) - new_param.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_str); - - sensor_configuration_ = new_param; - RCLCPP_INFO_STREAM(this->get_logger(), "Update sensor_configuration"); - std::shared_ptr sensor_cfg_ptr = - std::make_shared( - sensor_configuration_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - hw_interface_.CheckAndSetConfig(); - } - - auto result = std::make_shared(); - result->successful = true; - result->reason = "success"; - - RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback success"); - - return *result; -} - -void ContinentalARS548HwInterfaceRosWrapper::OdometryCallback( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) -{ - std::scoped_lock lock(mtx_config_); - - constexpr float speed_to_standstill = 0.5f; - constexpr float speed_to_moving = 2.f; - - if (standstill_ && std::abs(msg->twist.twist.linear.x) > speed_to_moving) { - standstill_ = false; - } else if (!standstill_ && std::abs(msg->twist.twist.linear.x) < speed_to_standstill) { - standstill_ = true; - } - - if (standstill_) { - hw_interface_.SetDrivingDirection(0); - } else { - hw_interface_.SetDrivingDirection(msg->twist.twist.linear.x > 0.f ? 1 : -1); - } - - constexpr float ms_to_kmh = 3.6f; - hw_interface_.SetVelocityVehicle(ms_to_kmh * std::abs(msg->twist.twist.linear.x)); - - constexpr float rad_to_deg = 180.f / M_PI; - hw_interface_.SetYawRate(rad_to_deg * msg->twist.twist.angular.z); -} - -void ContinentalARS548HwInterfaceRosWrapper::AccelerationCallback( - const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg) -{ - std::scoped_lock lock(mtx_config_); - hw_interface_.SetAccelerationLateralCog(msg->accel.accel.linear.y); - hw_interface_.SetAccelerationLongitudinalCog(msg->accel.accel.linear.x); -} - -void ContinentalARS548HwInterfaceRosWrapper::SteeringAngleCallback( - const std_msgs::msg::Float32::SharedPtr msg) -{ - std::scoped_lock lock(mtx_config_); - constexpr float rad_to_deg = 180.f / M_PI; - hw_interface_.SetSteeringAngleFrontAxle(rad_to_deg * msg->data); -} - -void ContinentalARS548HwInterfaceRosWrapper::SetNetworkConfigurationRequestCallback( - const std::shared_ptr - request, - const std::shared_ptr - response) -{ - std::scoped_lock lock(mtx_config_); - auto result = hw_interface_.SetSensorIPAddress(request->sensor_ip.data); - response->success = result == Status::OK; - response->message = (std::stringstream() << result).str(); -} - -void ContinentalARS548HwInterfaceRosWrapper::SetSensorMountingRequestCallback( - const std::shared_ptr request, - const std::shared_ptr - response) -{ - std::scoped_lock lock(mtx_config_); - - auto tf_buffer = std::make_unique(this->get_clock()); - auto tf_listener = std::make_unique(*tf_buffer); - - float longitudinal = request->longitudinal; - float lateral = request->lateral; - float vertical = request->vertical; - float yaw = request->yaw; - float pitch = request->pitch; - - if (request->autoconfigure_extrinsics) { - RCLCPP_INFO( - this->get_logger(), - "autoconfigure_extrinsics was set to true, so the mounting position will be derived from the " - "tfs"); - - geometry_msgs::msg::TransformStamped base_to_sensor_tf; - try { - base_to_sensor_tf = tf_buffer->lookupTransform( - sensor_configuration_.base_frame, sensor_configuration_.frame_id, rclcpp::Time(0), - rclcpp::Duration::from_seconds(0.5)); - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR( - this->get_logger(), "Could not obtain the transform from the base frame to %s (%s)", - sensor_configuration_.frame_id.c_str(), ex.what()); - response->success = false; - response->message = ex.what(); - return; - } - - const auto & quat = base_to_sensor_tf.transform.rotation; - geometry_msgs::msg::Vector3 rpy; - tf2::Matrix3x3(tf2::Quaternion(quat.x, quat.y, quat.z, quat.w)).getRPY(rpy.x, rpy.y, rpy.z); - - longitudinal = base_to_sensor_tf.transform.translation.x - - sensor_configuration_.configuration_vehicle_wheelbase; - lateral = base_to_sensor_tf.transform.translation.y; - vertical = base_to_sensor_tf.transform.translation.z; - yaw = rpy.z; - pitch = rpy.y; - } - - auto result = hw_interface_.SetSensorMounting( - longitudinal, lateral, vertical, yaw, pitch, request->plug_orientation); - - response->success = result == Status::OK; - response->message = (std::stringstream() << result).str(); -} - -void ContinentalARS548HwInterfaceRosWrapper::SetVehicleParametersRequestCallback( - [[maybe_unused]] const std::shared_ptr< - continental_srvs::srv::ContinentalArs548SetVehicleParameters::Request> - request, - const std::shared_ptr - response) -{ - float vehicle_length = request->vehicle_length; - float vehicle_width = request->vehicle_width; - float vehicle_height = request->vehicle_height; - float vehicle_wheelbase = request->vehicle_wheelbase; - - if (vehicle_length < 0.f) { - RCLCPP_INFO( - this->get_logger(), - "Service vehicle_length is invalid. Falling back to configuration value (%.2f)", - sensor_configuration_.configuration_vehicle_length); - vehicle_length = sensor_configuration_.configuration_vehicle_length; - } - - if (vehicle_width < 0.f) { - RCLCPP_INFO( - this->get_logger(), - "Service vehicle_width is invalid. Falling back to configuration value (%.2f)", - sensor_configuration_.configuration_vehicle_width); - vehicle_width = sensor_configuration_.configuration_vehicle_width; - } - - if (vehicle_height < 0.f) { - RCLCPP_INFO( - this->get_logger(), - "Service vehicle_height is invalid. Falling back to configuration value (%.2f)", - sensor_configuration_.configuration_vehicle_height); - vehicle_height = sensor_configuration_.configuration_vehicle_height; - } - - if (vehicle_wheelbase < 0.f) { - RCLCPP_INFO( - this->get_logger(), - "Service vehicle_wheelbase is invalid. Falling back to configuration value (%.2f)", - sensor_configuration_.configuration_vehicle_wheelbase); - vehicle_wheelbase = sensor_configuration_.configuration_vehicle_wheelbase; - } - - std::scoped_lock lock(mtx_config_); - auto result = hw_interface_.SetVehicleParameters( - vehicle_length, vehicle_width, vehicle_height, vehicle_wheelbase); - - response->success = result == Status::OK; - response->message = (std::stringstream() << result).str(); -} - -void ContinentalARS548HwInterfaceRosWrapper::SetRadarParametersRequestCallback( - const std::shared_ptr - request, - const std::shared_ptr - response) -{ - std::scoped_lock lock(mtx_config_); - auto result = hw_interface_.SetRadarParameters( - request->maximum_distance, request->frequency_slot, request->cycle_time, request->time_slot, - request->country_code, request->powersave_standstill); - - response->success = result == Status::OK; - response->message = (std::stringstream() << result).str(); -} - -std::vector -ContinentalARS548HwInterfaceRosWrapper::updateParameters() -{ - std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters start"); - std::ostringstream os_sensor_model; - os_sensor_model << sensor_configuration_.sensor_model; - RCLCPP_INFO_STREAM(this->get_logger(), "set_parameters"); - auto results = set_parameters( - {rclcpp::Parameter("sensor_model", os_sensor_model.str()), - rclcpp::Parameter("host_ip", sensor_configuration_.host_ip), - rclcpp::Parameter("sensor_ip", sensor_configuration_.sensor_ip), - rclcpp::Parameter("frame_id", sensor_configuration_.frame_id), - rclcpp::Parameter("data_port", sensor_configuration_.data_port), - rclcpp::Parameter("multicast_ip", sensor_configuration_.multicast_ip), - rclcpp::Parameter("base_frame", sensor_configuration_.base_frame), - rclcpp::Parameter("object_frame", sensor_configuration_.object_frame), - rclcpp::Parameter("configuration_host_port", sensor_configuration_.configuration_host_port), - rclcpp::Parameter( - "configuration_sensor_port", sensor_configuration_.configuration_sensor_port), - rclcpp::Parameter( - "configuration_vehicle_length", sensor_configuration_.configuration_vehicle_length), - rclcpp::Parameter( - "configuration_vehicle_width", sensor_configuration_.configuration_vehicle_width), - rclcpp::Parameter( - "configuration_vehicle_height", sensor_configuration_.configuration_vehicle_height), - rclcpp::Parameter( - "configuration_vehicle_wheelbase", sensor_configuration_.configuration_vehicle_wheelbase)}); - RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters end"); - return results; -} - -RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalARS548HwInterfaceRosWrapper) -} // namespace ros -} // namespace nebula diff --git a/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp new file mode 100644 index 000000000..bf4974a69 --- /dev/null +++ b/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp @@ -0,0 +1,286 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +namespace nebula +{ +namespace ros +{ + +ContinentalArs548HwInterfaceWrapper::ContinentalArs548HwInterfaceWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr & + config) +: parent_node_(parent_node), + hw_interface_( + std::make_shared()), + logger_(parent_node->get_logger().get_child("HwInterfaceWrapper")), + status_(Status::NOT_INITIALIZED), + config_(config) +{ + hw_interface_->SetLogger( + std::make_shared(parent_node->get_logger().get_child("HwInterface"))); + status_ = hw_interface_->SetSensorConfiguration(config); + + if (status_ != Status::OK) { + throw std::runtime_error( + (std::stringstream{} << "Could not initialize HW interface: " << status_).str()); + } + + status_ = Status::OK; +} + +void ContinentalArs548HwInterfaceWrapper::SensorInterfaceStart() +{ + if (Status::OK == status_) { + hw_interface_->SensorInterfaceStart(); + } + + if (Status::OK == status_) { + odometry_sub_ = + parent_node_->create_subscription( + "odometry_input", rclcpp::QoS{1}, + std::bind( + &ContinentalArs548HwInterfaceWrapper::OdometryCallback, this, std::placeholders::_1)); + + acceleration_sub_ = + parent_node_->create_subscription( + "acceleration_input", rclcpp::QoS{1}, + std::bind( + &ContinentalArs548HwInterfaceWrapper::AccelerationCallback, this, std::placeholders::_1)); + + steering_angle_sub_ = parent_node_->create_subscription( + "steering_angle_input", rclcpp::SensorDataQoS(), + std::bind( + &ContinentalArs548HwInterfaceWrapper::SteeringAngleCallback, this, std::placeholders::_1)); + + set_network_configuration_service_server_ = + parent_node_->create_service( + "set_network_configuration", + std::bind( + &ContinentalArs548HwInterfaceWrapper::SetNetworkConfigurationRequestCallback, this, + std::placeholders::_1, std::placeholders::_2)); + + set_sensor_mounting_service_server_ = + parent_node_->create_service( + "set_sensor_mounting", + std::bind( + &ContinentalArs548HwInterfaceWrapper::SetSensorMountingRequestCallback, this, + std::placeholders::_1, std::placeholders::_2)); + + set_vehicle_parameters_service_server_ = + parent_node_->create_service( + "set_vehicle_parameters", + std::bind( + &ContinentalArs548HwInterfaceWrapper::SetVehicleParametersRequestCallback, this, + std::placeholders::_1, std::placeholders::_2)); + + set_radar_parameters_service_server_ = + parent_node_->create_service( + "set_radar_parameters", + std::bind( + &ContinentalArs548HwInterfaceWrapper::SetRadarParametersRequestCallback, this, + std::placeholders::_1, std::placeholders::_2)); + } +} + +void ContinentalArs548HwInterfaceWrapper::OnConfigChange( + const std::shared_ptr< + const nebula::drivers::continental_ars548::ContinentalArs548SensorConfiguration> & new_config) +{ + hw_interface_->SetSensorConfiguration(new_config); + config_ = new_config; +} + +Status ContinentalArs548HwInterfaceWrapper::Status() +{ + return status_; +} + +std::shared_ptr +ContinentalArs548HwInterfaceWrapper::HwInterface() const +{ + return hw_interface_; +} + +void ContinentalArs548HwInterfaceWrapper::OdometryCallback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) +{ + constexpr float speed_to_standstill = 0.5f; + constexpr float speed_to_moving = 2.f; + + if (standstill_ && std::abs(msg->twist.twist.linear.x) > speed_to_moving) { + standstill_ = false; + } else if (!standstill_ && std::abs(msg->twist.twist.linear.x) < speed_to_standstill) { + standstill_ = true; + } + + if (standstill_) { + hw_interface_->SetDrivingDirection(0); + } else { + hw_interface_->SetDrivingDirection(msg->twist.twist.linear.x > 0.f ? 1 : -1); + } + + constexpr float ms_to_kmh = 3.6f; + hw_interface_->SetVelocityVehicle(ms_to_kmh * std::abs(msg->twist.twist.linear.x)); + + constexpr float rad_to_deg = 180.f / M_PI; + hw_interface_->SetYawRate(rad_to_deg * msg->twist.twist.angular.z); +} + +void ContinentalArs548HwInterfaceWrapper::AccelerationCallback( + const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg) +{ + hw_interface_->SetAccelerationLateralCog(msg->accel.accel.linear.y); + hw_interface_->SetAccelerationLongitudinalCog(msg->accel.accel.linear.x); +} + +void ContinentalArs548HwInterfaceWrapper::SteeringAngleCallback( + const std_msgs::msg::Float32::SharedPtr msg) +{ + constexpr float rad_to_deg = 180.f / M_PI; + hw_interface_->SetSteeringAngleFrontAxle(rad_to_deg * msg->data); +} + +void ContinentalArs548HwInterfaceWrapper::SetNetworkConfigurationRequestCallback( + const std::shared_ptr + request, + const std::shared_ptr + response) +{ + auto result = hw_interface_->SetSensorIPAddress(request->sensor_ip.data); + response->success = result == Status::OK; + response->message = (std::stringstream() << result).str(); +} + +void ContinentalArs548HwInterfaceWrapper::SetSensorMountingRequestCallback( + const std::shared_ptr request, + const std::shared_ptr + response) +{ + auto tf_buffer = std::make_unique(parent_node_->get_clock()); + auto tf_listener = std::make_unique(*tf_buffer); + + float longitudinal = request->longitudinal; + float lateral = request->lateral; + float vertical = request->vertical; + float yaw = request->yaw; + float pitch = request->pitch; + + if (request->autoconfigure_extrinsics) { + RCLCPP_INFO( + logger_, + "autoconfigure_extrinsics was set to true, so the mounting position will be derived from the " + "tfs"); + + geometry_msgs::msg::TransformStamped base_to_sensor_tf; + try { + base_to_sensor_tf = tf_buffer->lookupTransform( + config_->base_frame, config_->frame_id, rclcpp::Time(0), + rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR( + logger_, "Could not obtain the transform from the base frame to %s (%s)", + config_->frame_id.c_str(), ex.what()); + response->success = false; + response->message = ex.what(); + return; + } + + const auto & quat = base_to_sensor_tf.transform.rotation; + geometry_msgs::msg::Vector3 rpy; + tf2::Matrix3x3(tf2::Quaternion(quat.x, quat.y, quat.z, quat.w)).getRPY(rpy.x, rpy.y, rpy.z); + + longitudinal = + base_to_sensor_tf.transform.translation.x - config_->configuration_vehicle_wheelbase; + lateral = base_to_sensor_tf.transform.translation.y; + vertical = base_to_sensor_tf.transform.translation.z; + yaw = rpy.z; + pitch = rpy.y; + } + + auto result = hw_interface_->SetSensorMounting( + longitudinal, lateral, vertical, yaw, pitch, request->plug_orientation); + + response->success = result == Status::OK; + response->message = (std::stringstream() << result).str(); +} + +void ContinentalArs548HwInterfaceWrapper::SetVehicleParametersRequestCallback( + [[maybe_unused]] const std::shared_ptr< + continental_srvs::srv::ContinentalArs548SetVehicleParameters::Request> + request, + const std::shared_ptr + response) +{ + float vehicle_length = request->vehicle_length; + float vehicle_width = request->vehicle_width; + float vehicle_height = request->vehicle_height; + float vehicle_wheelbase = request->vehicle_wheelbase; + + if (vehicle_length < 0.f) { + RCLCPP_INFO( + logger_, "Service vehicle_length is invalid. Falling back to configuration value (%.2f)", + config_->configuration_vehicle_length); + vehicle_length = config_->configuration_vehicle_length; + } + + if (vehicle_width < 0.f) { + RCLCPP_INFO( + logger_, "Service vehicle_width is invalid. Falling back to configuration value (%.2f)", + config_->configuration_vehicle_width); + vehicle_width = config_->configuration_vehicle_width; + } + + if (vehicle_height < 0.f) { + RCLCPP_INFO( + logger_, "Service vehicle_height is invalid. Falling back to configuration value (%.2f)", + config_->configuration_vehicle_height); + vehicle_height = config_->configuration_vehicle_height; + } + + if (vehicle_wheelbase < 0.f) { + RCLCPP_INFO( + logger_, "Service vehicle_wheelbase is invalid. Falling back to configuration value (%.2f)", + config_->configuration_vehicle_wheelbase); + vehicle_wheelbase = config_->configuration_vehicle_wheelbase; + } + + auto result = hw_interface_->SetVehicleParameters( + vehicle_length, vehicle_width, vehicle_height, vehicle_wheelbase); + + response->success = result == Status::OK; + response->message = (std::stringstream() << result).str(); +} + +void ContinentalArs548HwInterfaceWrapper::SetRadarParametersRequestCallback( + const std::shared_ptr + request, + const std::shared_ptr + response) +{ + auto result = hw_interface_->SetRadarParameters( + request->maximum_distance, request->frequency_slot, request->cycle_time, request->time_slot, + request->country_code, request->powersave_standstill); + + response->success = result == Status::OK; + response->message = (std::stringstream() << result).str(); +} + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp new file mode 100644 index 000000000..fb7fcbcd9 --- /dev/null +++ b/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp @@ -0,0 +1,360 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +namespace nebula +{ +namespace ros +{ +ContinentalArs548RosWrapper::ContinentalArs548RosWrapper(const rclcpp::NodeOptions & options) +: rclcpp::Node( + "continental_ars548_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)), + wrapper_status_(Status::NOT_INITIALIZED), + sensor_cfg_ptr_(nullptr), + packet_queue_(3000), + hw_interface_wrapper_(), + decoder_wrapper_() +{ + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + + wrapper_status_ = DeclareAndGetSensorConfigParams(); + + if (wrapper_status_ != Status::OK) { + throw std::runtime_error( + (std::stringstream{} << "Sensor configuration invalid: " << wrapper_status_).str()); + } + + RCLCPP_INFO_STREAM(get_logger(), "SensorConfig:" << *sensor_cfg_ptr_); + + launch_hw_ = declare_parameter("launch_hw", param_read_only()); + + if (launch_hw_) { + hw_interface_wrapper_.emplace(this, sensor_cfg_ptr_); + } + + decoder_wrapper_.emplace(this, sensor_cfg_ptr_, launch_hw_); + + RCLCPP_DEBUG(get_logger(), "Starting stream"); + + decoder_thread_ = std::thread([this]() { + while (true) { + decoder_wrapper_->ProcessPacket(std::move(packet_queue_.pop())); + } + }); + + if (launch_hw_) { + hw_interface_wrapper_->HwInterface()->RegisterCallback( + std::bind(&ContinentalArs548RosWrapper::ReceivePacketCallback, this, std::placeholders::_1)); + StreamStart(); + } else { + packets_sub_ = create_subscription( + "nebula_packets", rclcpp::SensorDataQoS(), + std::bind( + &ContinentalArs548RosWrapper::ReceivePacketsMessageCallback, this, std::placeholders::_1)); + RCLCPP_INFO_STREAM( + get_logger(), + "Hardware connection disabled, listening for packets on " << packets_sub_->get_topic_name()); + } + + // Register parameter callback after all params have been declared. Otherwise it would be called + // once for each declaration + parameter_event_cb_ = add_on_set_parameters_callback( + std::bind(&ContinentalArs548RosWrapper::OnParameterChange, this, std::placeholders::_1)); +} + +nebula::Status ContinentalArs548RosWrapper::DeclareAndGetSensorConfigParams() +{ + nebula::drivers::continental_ars548::ContinentalArs548SensorConfiguration config; + + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("sensor_model"); + config.sensor_model = + nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("host_ip", descriptor); + config.host_ip = this->get_parameter("host_ip").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("sensor_ip", descriptor); + config.sensor_ip = this->get_parameter("sensor_ip").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("multicast_ip", descriptor); + config.multicast_ip = this->get_parameter("multicast_ip").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("frame_id", descriptor); + config.frame_id = this->get_parameter("frame_id").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("base_frame", descriptor); + config.base_frame = this->get_parameter("base_frame").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("object_frame", descriptor); + config.object_frame = this->get_parameter("object_frame").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("data_port", descriptor); + config.data_port = this->get_parameter("data_port").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_host_port", descriptor); + config.configuration_host_port = this->get_parameter("configuration_host_port").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_sensor_port", descriptor); + config.configuration_sensor_port = this->get_parameter("configuration_sensor_port").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("use_sensor_time", descriptor); + config.use_sensor_time = this->get_parameter("use_sensor_time").as_bool(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_vehicle_length", descriptor); + config.configuration_vehicle_length = + static_cast(this->get_parameter("configuration_vehicle_length").as_double()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_vehicle_width", descriptor); + config.configuration_vehicle_width = + static_cast(this->get_parameter("configuration_vehicle_width").as_double()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_vehicle_height", descriptor); + config.configuration_vehicle_height = + static_cast(this->get_parameter("configuration_vehicle_height").as_double()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_vehicle_wheelbase", descriptor); + config.configuration_vehicle_wheelbase = + static_cast(this->get_parameter("configuration_vehicle_wheelbase").as_double()); + } + + if (config.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + + auto new_cfg_ptr = std::make_shared< + const nebula::drivers::continental_ars548::ContinentalArs548SensorConfiguration>(config); + return ValidateAndSetConfig(new_cfg_ptr); +} + +Status ContinentalArs548RosWrapper::ValidateAndSetConfig( + std::shared_ptr & + new_config) +{ + if (new_config->sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + + if (new_config->frame_id.empty()) { + return Status::SENSOR_CONFIG_ERROR; + } + + if (hw_interface_wrapper_) { + hw_interface_wrapper_->OnConfigChange(new_config); + } + if (decoder_wrapper_) { + decoder_wrapper_->OnConfigChange(new_config); + } + + sensor_cfg_ptr_ = new_config; + return Status::OK; +} + +void ContinentalArs548RosWrapper::ReceivePacketsMessageCallback( + std::unique_ptr packets_msg) +{ + if (hw_interface_wrapper_) { + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 1000, + "Ignoring NebulaPackets PandarScan. Launch with launch_hw:=false to enable NebulaPackets " + "replay."); + return; + } + + for (auto & packet : packets_msg->packets) { + auto nebula_packet_ptr = std::make_unique(); + nebula_packet_ptr->stamp = packet.stamp; + std::copy(packet.data.begin(), packet.data.end(), std::back_inserter(nebula_packet_ptr->data)); + + packet_queue_.push(std::move(nebula_packet_ptr)); + } +} + +Status ContinentalArs548RosWrapper::GetStatus() +{ + return wrapper_status_; +} + +Status ContinentalArs548RosWrapper::StreamStart() +{ + if (!hw_interface_wrapper_) { + return Status::UDP_CONNECTION_ERROR; + } + + if (hw_interface_wrapper_->Status() != Status::OK) { + return hw_interface_wrapper_->Status(); + } + + hw_interface_wrapper_->SensorInterfaceStart(); + + return hw_interface_wrapper_->Status(); +} + +rcl_interfaces::msg::SetParametersResult ContinentalArs548RosWrapper::OnParameterChange( + const std::vector & p) +{ + using rcl_interfaces::msg::SetParametersResult; + + if (p.empty()) { + return rcl_interfaces::build().successful(true).reason(""); + } + + std::scoped_lock lock(mtx_config_); + + RCLCPP_INFO(get_logger(), "OnParameterChange"); + + drivers::continental_ars548::ContinentalArs548SensorConfiguration new_cfg(*sensor_cfg_ptr_); + + bool got_any = + get_param(p, "frame_id", new_cfg.frame_id) | get_param(p, "base_frame", new_cfg.base_frame) | + get_param(p, "object_frame", new_cfg.object_frame) | + get_param(p, "configuration_vehicle_length", new_cfg.configuration_vehicle_length) | + get_param(p, "configuration_vehicle_width", new_cfg.configuration_vehicle_width) | + get_param(p, "configuration_vehicle_height", new_cfg.configuration_vehicle_height) | + get_param(p, "configuration_vehicle_wheelbase", new_cfg.configuration_vehicle_wheelbase) | + get_param(p, "configuration_host_port", new_cfg.configuration_host_port) | + get_param(p, "configuration_sensor_port", new_cfg.configuration_sensor_port); + + if (!got_any) { + return rcl_interfaces::build().successful(true).reason(""); + } + + auto new_cfg_ptr = std::make_shared< + const nebula::drivers::continental_ars548::ContinentalArs548SensorConfiguration>(new_cfg); + auto status = ValidateAndSetConfig(new_cfg_ptr); + + if (status != Status::OK) { + RCLCPP_WARN_STREAM(get_logger(), "OnParameterChange aborted: " << status); + auto result = SetParametersResult(); + result.successful = false; + result.reason = (std::stringstream() << "Invalid configuration: " << status).str(); + return result; + } + + return rcl_interfaces::build().successful(true).reason(""); +} + +void ContinentalArs548RosWrapper::ReceivePacketCallback(std::vector & packet) +{ + if (!decoder_wrapper_ || decoder_wrapper_->Status() != Status::OK) { + return; + } + + const auto now = std::chrono::high_resolution_clock::now(); + const auto timestamp_ns = + std::chrono::duration_cast(now.time_since_epoch()).count(); + + auto msg_ptr = std::make_unique(); + msg_ptr->stamp.sec = static_cast(timestamp_ns / 1'000'000'000); + msg_ptr->stamp.nanosec = static_cast(timestamp_ns % 1'000'000'000); + msg_ptr->data.swap(packet); + + if (!packet_queue_.try_push(std::move(msg_ptr))) { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped"); + } +} + +RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalArs548RosWrapper) +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/continental/multi_continental_ars548_hw_interface_ros_wrapper.cpp b/nebula_ros/src/continental/multi_continental_ars548_hw_interface_ros_wrapper.cpp deleted file mode 100644 index b8af00fd2..000000000 --- a/nebula_ros/src/continental/multi_continental_ars548_hw_interface_ros_wrapper.cpp +++ /dev/null @@ -1,356 +0,0 @@ -// 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. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include -#include - -#include -#include - -namespace nebula -{ -namespace ros -{ -MultiContinentalARS548HwInterfaceRosWrapper::MultiContinentalARS548HwInterfaceRosWrapper( - const rclcpp::NodeOptions & options) -: rclcpp::Node("multi_continental_ars548_hw_interface_ros_wrapper", options), hw_interface_() -{ - if (mtx_config_.try_lock()) { - interface_status_ = GetParameters(sensor_configuration_); - mtx_config_.unlock(); - } - if (Status::OK != interface_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); - return; - } - hw_interface_.SetLogger(std::make_shared(this->get_logger())); - std::shared_ptr - sensor_cfg_ptr = - std::make_shared( - sensor_configuration_); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - - assert(sensor_configuration_.sensor_ips.size() == sensor_configuration_.frame_ids.size()); - hw_interface_.RegisterScanCallback(std::bind( - &MultiContinentalARS548HwInterfaceRosWrapper::ReceivePacketsDataCallback, this, - std::placeholders::_1, std::placeholders::_2)); - - for (std::size_t sensor_id = 0; sensor_id < sensor_configuration_.sensor_ips.size(); - sensor_id++) { - const std::string sensor_ip = sensor_configuration_.sensor_ips[sensor_id]; - const std::string frame_id = sensor_configuration_.frame_ids[sensor_id]; - packets_pub_map_[sensor_ip] = this->create_publisher( - frame_id + "/nebula_packets", rclcpp::SensorDataQoS()); - } - - set_param_res_ = this->add_on_set_parameters_callback(std::bind( - &MultiContinentalARS548HwInterfaceRosWrapper::paramCallback, this, std::placeholders::_1)); - - StreamStart(); -} - -MultiContinentalARS548HwInterfaceRosWrapper::~MultiContinentalARS548HwInterfaceRosWrapper() -{ -} - -Status MultiContinentalARS548HwInterfaceRosWrapper::StreamStart() -{ - if (Status::OK == interface_status_) { - interface_status_ = hw_interface_.SensorInterfaceStart(); - } - - if (Status::OK == interface_status_) { - odometry_sub_ = this->create_subscription( - "odometry_input", rclcpp::QoS{1}, - std::bind( - &MultiContinentalARS548HwInterfaceRosWrapper::OdometryCallback, this, - std::placeholders::_1)); - - acceleration_sub_ = create_subscription( - "acceleration_input", rclcpp::QoS{1}, - std::bind( - &MultiContinentalARS548HwInterfaceRosWrapper::AccelerationCallback, this, - std::placeholders::_1)); - - steering_angle_sub_ = create_subscription( - "steering_angle_input", rclcpp::QoS{1}, - std::bind( - &MultiContinentalARS548HwInterfaceRosWrapper::SteeringAngleCallback, this, - std::placeholders::_1)); - } - - return interface_status_; -} - -Status MultiContinentalARS548HwInterfaceRosWrapper::StreamStop() -{ - return Status::OK; -} -Status MultiContinentalARS548HwInterfaceRosWrapper::Shutdown() -{ - return Status::OK; -} - -Status MultiContinentalARS548HwInterfaceRosWrapper::InitializeHwInterface( // todo: don't think - // this is needed - const drivers::SensorConfigurationBase & sensor_configuration) -{ - std::stringstream ss; - ss << sensor_configuration; - RCLCPP_DEBUG_STREAM(this->get_logger(), ss.str()); - return Status::OK; -} - -Status MultiContinentalARS548HwInterfaceRosWrapper::GetParameters( - drivers::continental_ars548::MultiContinentalARS548SensorConfiguration & sensor_configuration) -{ - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", descriptor); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("host_ip", descriptor); - sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter>("sensor_ips", descriptor); - sensor_configuration.sensor_ips = this->get_parameter("sensor_ips").as_string_array(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("multicast_ip", descriptor); - sensor_configuration.multicast_ip = this->get_parameter("multicast_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter>("frame_ids", descriptor); - sensor_configuration.frame_ids = this->get_parameter("frame_ids").as_string_array(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("base_frame", descriptor); - sensor_configuration.base_frame = this->get_parameter("base_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("object_frame", descriptor); - sensor_configuration.object_frame = this->get_parameter("object_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("data_port", descriptor); - sensor_configuration.data_port = this->get_parameter("data_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_host_port", descriptor); - sensor_configuration.configuration_host_port = - this->get_parameter("configuration_host_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_sensor_port", descriptor); - sensor_configuration.configuration_sensor_port = - this->get_parameter("configuration_sensor_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("use_sensor_time", descriptor); - sensor_configuration.use_sensor_time = this->get_parameter("use_sensor_time").as_bool(); - } - - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; -} - -void MultiContinentalARS548HwInterfaceRosWrapper::ReceivePacketsDataCallback( - std::unique_ptr scan_buffer, const std::string & sensor_ip) -{ - packets_pub_map_[sensor_ip]->publish(std::move(scan_buffer)); -} - -rcl_interfaces::msg::SetParametersResult MultiContinentalARS548HwInterfaceRosWrapper::paramCallback( - const std::vector & p) -{ - std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback"); - RCLCPP_DEBUG_STREAM(this->get_logger(), p); - RCLCPP_DEBUG_STREAM(this->get_logger(), sensor_configuration_); - RCLCPP_INFO_STREAM(this->get_logger(), p); - - drivers::continental_ars548::MultiContinentalARS548SensorConfiguration new_param{ - sensor_configuration_}; - RCLCPP_INFO_STREAM(this->get_logger(), new_param); - std::string sensor_model_str; - - if ( - get_param(p, "sensor_model", sensor_model_str) | get_param(p, "host_ip", new_param.host_ip) | - get_param(p, "sensor_ips", new_param.sensor_ips) | - get_param(p, "frame_ids", new_param.frame_ids) | - get_param(p, "data_port", new_param.data_port) | - get_param(p, "multicast_ip", new_param.multicast_ip) | - get_param(p, "base_frame", new_param.base_frame) | - get_param(p, "object_frame", new_param.object_frame) | - get_param(p, "configuration_host_port", new_param.configuration_host_port) | - get_param(p, "configuration_sensor_port", new_param.configuration_sensor_port) | - get_param(p, "configuration_host_port", new_param.configuration_host_port) | - get_param(p, "configuration_sensor_port", new_param.configuration_sensor_port)) { - if (0 < sensor_model_str.length()) - new_param.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_str); - - sensor_configuration_ = new_param; - RCLCPP_INFO_STREAM(this->get_logger(), "Update sensor_configuration"); - std::shared_ptr sensor_cfg_ptr = - std::make_shared( - sensor_configuration_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - hw_interface_.CheckAndSetConfig(); - } - - auto result = std::make_shared(); - result->successful = true; - result->reason = "success"; - - RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback success"); - - return *result; -} - -void MultiContinentalARS548HwInterfaceRosWrapper::OdometryCallback( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) -{ - std::scoped_lock lock(mtx_config_); - - constexpr float speed_to_standstill = 0.5f; - constexpr float speed_to_moving = 2.f; - - if (standstill_ && std::abs(msg->twist.twist.linear.x) > speed_to_moving) { - standstill_ = false; - } else if (!standstill_ && std::abs(msg->twist.twist.linear.x) < speed_to_standstill) { - standstill_ = true; - } - - if (standstill_) { - hw_interface_.SetDrivingDirection(0); - } else { - hw_interface_.SetDrivingDirection(msg->twist.twist.linear.x > 0.f ? 1 : -1); - } - - constexpr float ms_to_kmh = 3.6f; - hw_interface_.SetVelocityVehicle(ms_to_kmh * std::abs(msg->twist.twist.linear.x)); - - constexpr float rad_to_deg = 180.f / M_PI; - hw_interface_.SetYawRate(rad_to_deg * msg->twist.twist.angular.z); -} - -void MultiContinentalARS548HwInterfaceRosWrapper::AccelerationCallback( - const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg) -{ - std::scoped_lock lock(mtx_config_); - hw_interface_.SetAccelerationLateralCog(msg->accel.accel.linear.y); - hw_interface_.SetAccelerationLongitudinalCog(msg->accel.accel.linear.x); -} - -void MultiContinentalARS548HwInterfaceRosWrapper::SteeringAngleCallback( - const std_msgs::msg::Float32::SharedPtr msg) -{ - std::scoped_lock lock(mtx_config_); - constexpr float rad_to_deg = 180.f / M_PI; - hw_interface_.SetSteeringAngleFrontAxle(rad_to_deg * msg->data); -} - -std::vector -MultiContinentalARS548HwInterfaceRosWrapper::updateParameters() -{ - std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters start"); - std::ostringstream os_sensor_model; - os_sensor_model << sensor_configuration_.sensor_model; - RCLCPP_INFO_STREAM(this->get_logger(), "set_parameters"); - auto results = set_parameters( - {rclcpp::Parameter("sensor_model", os_sensor_model.str()), - rclcpp::Parameter("host_ip", sensor_configuration_.host_ip), - rclcpp::Parameter("sensor_ips", sensor_configuration_.sensor_ips), - rclcpp::Parameter("frame_ids", sensor_configuration_.frame_ids), - rclcpp::Parameter("data_port", sensor_configuration_.data_port), - rclcpp::Parameter("multicast_ip", sensor_configuration_.multicast_ip), - rclcpp::Parameter("base_frame", sensor_configuration_.base_frame), - rclcpp::Parameter("object_frame", sensor_configuration_.object_frame), - rclcpp::Parameter("configuration_host_port", sensor_configuration_.configuration_host_port), - rclcpp::Parameter( - "configuration_sensor_port", sensor_configuration_.configuration_sensor_port)}); - RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters end"); - return results; -} - -RCLCPP_COMPONENTS_REGISTER_NODE(MultiContinentalARS548HwInterfaceRosWrapper) -} // namespace ros -} // namespace nebula diff --git a/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp b/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp index d31398c5b..7cfb75999 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp +++ b/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp @@ -39,7 +39,7 @@ ContinentalRosDecoderTest::ContinentalRosDecoderTest( const rclcpp::NodeOptions & options, const std::string & node_name) : rclcpp::Node(node_name, options) { - drivers::continental_ars548::ContinentalARS548SensorConfiguration sensor_configuration; + drivers::continental_ars548::ContinentalArs548SensorConfiguration sensor_configuration; wrapper_status_ = GetParameters(sensor_configuration); if (Status::OK != wrapper_status_) { @@ -49,12 +49,12 @@ ContinentalRosDecoderTest::ContinentalRosDecoderTest( RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); sensor_cfg_ptr_ = - std::make_shared( + std::make_shared( sensor_configuration); RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); wrapper_status_ = InitializeDriver( - std::const_pointer_cast( + std::const_pointer_cast( sensor_cfg_ptr_)); driver_ptr_->RegisterDetectionListCallback( @@ -66,12 +66,12 @@ ContinentalRosDecoderTest::ContinentalRosDecoderTest( } Status ContinentalRosDecoderTest::InitializeDriver( - std::shared_ptr + std::shared_ptr sensor_configuration) { // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared( - std::static_pointer_cast( + driver_ptr_ = std::make_shared( + std::static_pointer_cast( sensor_configuration)); return Status::OK; } @@ -82,7 +82,7 @@ Status ContinentalRosDecoderTest::GetStatus() } Status ContinentalRosDecoderTest::GetParameters( - drivers::continental_ars548::ContinentalARS548SensorConfiguration & sensor_configuration) + drivers::continental_ars548::ContinentalArs548SensorConfiguration & sensor_configuration) { std::filesystem::path bag_root_dir = _SRC_RESOURCES_DIR_PATH; // variable defined in CMakeLists.txt; @@ -278,8 +278,9 @@ void ContinentalRosDecoderTest::ReadBag() ASSERT_EQ(1, extracted_msg.packets.size()); - auto extracted_msg_ptr = std::make_shared(extracted_msg); - driver_ptr_->ProcessPackets(extracted_msg); + // auto extracted_msg_ptr = + // std::make_shared(extracted_msg); + driver_ptr_->ProcessPacket(extracted_msg.packets[0]); } } } diff --git a/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp b/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp index aab813cb2..fe606c069 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp +++ b/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp @@ -41,18 +41,18 @@ namespace ros class ContinentalRosDecoderTest final : public rclcpp::Node, NebulaDriverRosWrapperBase //, testing::Test { - std::shared_ptr driver_ptr_; + std::shared_ptr driver_ptr_; Status wrapper_status_; - std::shared_ptr + std::shared_ptr sensor_cfg_ptr_; Status InitializeDriver( - std::shared_ptr + std::shared_ptr sensor_configuration); Status GetParameters( - drivers::continental_ars548::ContinentalARS548SensorConfiguration & sensor_configuration); + drivers::continental_ars548::ContinentalArs548SensorConfiguration & sensor_configuration); void CheckResult(const std::string msg_as_string, const std::string & gt_path); From 5d62cb8d09ce4c79d56377120c48c3b95c1516f4 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 23 May 2024 01:19:49 +0900 Subject: [PATCH 02/47] feat: mid implmementation of the single node scheme Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental/continental_srr520.hpp | 295 +++++ .../include/nebula_common/continental/crc.hpp | 64 + .../decoders/continental_ars548_decoder.hpp | 19 +- .../decoders/continental_srr520_decoder.hpp | 213 ++++ .../decoders/continental_ars548_decoder.cpp | 50 +- .../decoders/continental_srr520_decoder.cpp | 1133 +++++++++++++++++ .../continental_ars548_hw_interface.hpp | 7 +- .../continental_srr520_hw_interface.hpp | 187 +++ .../continental_ars548_hw_interface.cpp | 9 - .../continental_srr520_hw_interface.cpp | 435 +++++++ ...continental_ars548_decoder_ros_wrapper.cpp | 802 ++++++++++++ ...continental_ars548_decoder_ros_wrapper.hpp | 166 +++ ...nental_ars548_hw_interface_ros_wrapper.cpp | 561 ++++++++ ...nental_ars548_hw_interface_ros_wrapper.hpp | 188 +++ .../continental_ars548_decoder_wrapper.hpp | 4 + ...continental_srr520_decoder_ros_wrapper.hpp | 162 +++ .../continental_srr520_decoder_wrapper.hpp | 183 +++ ...nental_srr520_hw_interface_ros_wrapper.hpp | 161 +++ ...ontinental_srr520_hw_interface_wrapper.hpp | 99 ++ .../continental_srr520_ros_wrapper.hpp | 106 ++ ...nental_ars548_hw_interface_ros_wrapper.cpp | 356 ++++++ ...nental_ars548_hw_interface_ros_wrapper.hpp | 146 +++ .../continental_ars548_decoder_wrapper.cpp | 24 +- ...continental_srr520_decoder_ros_wrapper.cpp | 646 ++++++++++ .../continental_srr520_decoder_wrapper.cpp | 603 +++++++++ ...nental_srr520_hw_interface_ros_wrapper.cpp | 384 ++++++ ...ontinental_srr520_hw_interface_wrapper.cpp | 183 +++ .../continental_srr520_ros_wrapper.cpp | 298 +++++ .../continental_ros_decoder_test_srr520.cpp | 300 +++++ .../continental_ros_decoder_test_srr520.hpp | 97 ++ 30 files changed, 7832 insertions(+), 49 deletions(-) create mode 100644 nebula_common/include/nebula_common/continental/continental_srr520.hpp create mode 100644 nebula_common/include/nebula_common/continental/crc.hpp create mode 100644 nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp create mode 100644 nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp create mode 100644 nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp create mode 100644 nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp create mode 100644 nebula_ros/continental_ars548_decoder_ros_wrapper.cpp create mode 100644 nebula_ros/continental_ars548_decoder_ros_wrapper.hpp create mode 100644 nebula_ros/continental_ars548_hw_interface_ros_wrapper.cpp create mode 100644 nebula_ros/continental_ars548_hw_interface_ros_wrapper.hpp create mode 100644 nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_ros_wrapper.hpp create mode 100644 nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp create mode 100644 nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_ros_wrapper.hpp create mode 100644 nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp create mode 100644 nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp create mode 100644 nebula_ros/multi_continental_ars548_hw_interface_ros_wrapper.cpp create mode 100644 nebula_ros/multi_continental_ars548_hw_interface_ros_wrapper.hpp create mode 100644 nebula_ros/src/continental/continental_srr520_decoder_ros_wrapper.cpp create mode 100644 nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp create mode 100644 nebula_ros/src/continental/continental_srr520_hw_interface_ros_wrapper.cpp create mode 100644 nebula_ros/src/continental/continental_srr520_hw_interface_wrapper.cpp create mode 100644 nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp create mode 100644 nebula_tests/continental/continental_ros_decoder_test_srr520.cpp create mode 100644 nebula_tests/continental/continental_ros_decoder_test_srr520.hpp diff --git a/nebula_common/include/nebula_common/continental/continental_srr520.hpp b/nebula_common/include/nebula_common/continental/continental_srr520.hpp new file mode 100644 index 000000000..1d21d4f9e --- /dev/null +++ b/nebula_common/include/nebula_common/continental/continental_srr520.hpp @@ -0,0 +1,295 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include + +#include "boost/endian/buffers.hpp" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +namespace continental_srr520 +{ + +/// @brief struct for SRR520 sensor configuration +struct ContinentalSrr520SensorConfiguration : CANSensorConfigurationBase +{ + std::string base_frame{}; + bool sync_use_bus_time{}; + float configuration_vehicle_wheelbase{}; +}; + +/// @brief Convert ContinentalSrr520SensorConfiguration to string (Overloading the << +/// operator) +/// @param os +/// @param arg +/// @return stream +inline std::ostream & operator<<( + std::ostream & os, ContinentalSrr520SensorConfiguration const & arg) +{ + os << (CANSensorConfigurationBase)(arg) << ", BaseFrame: " << arg.base_frame + << ", SyncUseBusTime: " << arg.sync_use_bus_time + << ", ConfigurationVehicleWheelbase: " << arg.configuration_vehicle_wheelbase; + return os; +} + +using boost::endian::big_float32_buf_t; +using boost::endian::big_uint16_buf_t; +using boost::endian::big_uint32_buf_t; +using boost::endian::big_uint64_buf_t; + +// CAN MSG IDS +constexpr int RDI_NEAR_HEADER_CAN_MESSAGE_ID = 900; +constexpr int RDI_NEAR_ELEMENT_CAN_MESSAGE_ID = 901; +constexpr int RDI_HRR_HEADER_CAN_MESSAGE_ID = 1100; +constexpr int RDI_HRR_ELEMENT_CAN_MESSAGE_ID = 1101; +constexpr int OBJECT_HEADER_CAN_MESSAGE_ID = 1200; +constexpr int OBJECT_CAN_MESSAGE_ID = 1201; +constexpr int CRC_LIST_CAN_MESSAGE_ID = 800; +constexpr int STATUS_CAN_MESSAGE_ID = 700; +constexpr int SYNC_FUP_CAN_MESSAGE_ID = 53; +constexpr int VEH_DYN_CAN_MESSAGE_ID = 600; +constexpr int SENSOR_CONFIG_CAN_MESSAGE_ID = 601; + +// CRC IDS +constexpr int NEAR_CRC_ID = 0; +constexpr int HRR_CRC_ID = 1; +constexpr int OBJECT_CRC_ID = 2; +constexpr int TIME_DOMAIN_ID = 0; + +constexpr int RDI_NEAR_HEADER_PACKET_SIZE = 32; +constexpr int RDI_NEAR_ELEMENT_PACKET_SIZE = 64; +constexpr int RDI_HRR_HEADER_PACKET_SIZE = 32; +constexpr int RDI_HRR_ELEMENT_PACKET_SIZE = 64; +constexpr int OBJECT_HEADER_PACKET_SIZE = 32; +constexpr int OBJECT_PACKET_SIZE = 64; +constexpr int CRC_LIST_PACKET_SIZE = 4; +constexpr int STATUS_PACKET_SIZE = 64; +constexpr int SYNC_FUP_CAN_PACKET_SIZE = 8; +constexpr int VEH_DYN_CAN_PACKET_SIZE = 8; +constexpr int CONFIGURATION_PACKET_SIZE = 16; + +constexpr int RDI_NEAR_PACKET_NUM = 50; +constexpr int RDI_HRR_PACKET_NUM = 20; +constexpr int OBJECT_PACKET_NUM = 20; + +constexpr int FRAGMENTS_PER_DETECTION_PACKET = 10; +constexpr int FRAGMENTS_PER_OBJECT_PACKET = 2; +constexpr int DETECTION_FRAGMENT_SIZE = 6; +constexpr int OBJECT_FRAGMENT_SIZE = 31; + +constexpr int MAX_RDI_NEAR_DETECTIONS = RDI_NEAR_PACKET_NUM * FRAGMENTS_PER_DETECTION_PACKET; +constexpr int MAX_RDI_HRR_DETECTIONS = RDI_HRR_PACKET_NUM * FRAGMENTS_PER_DETECTION_PACKET; +constexpr int MAX_OBJECTS = OBJECT_PACKET_NUM * FRAGMENTS_PER_OBJECT_PACKET; + +#pragma pack(push, 1) + +struct StatusPacket +{ + big_uint32_buf_t u_time_stamp; // 0 + big_uint32_buf_t u_global_time_stamp_sec; // 4 + big_uint32_buf_t u_global_time_stamp_nsec; // 8 + uint8_t u_global_time_stamp_sync_status; // 12 + uint8_t u_sw_version_major; // 13 + uint8_t u_sw_version_minor; // 14 + uint8_t u_sw_version_patch; // 15 + uint8_t u_sensor_id; // 16 + big_uint16_buf_t u_long_pos; // 17 + big_uint16_buf_t u_lat_pos; // 19 + big_uint16_buf_t u_vert_pos; // 21 + big_uint16_buf_t u_long_pos_cog; // 23 + big_uint16_buf_t u_wheelbase; // 25 + big_uint16_buf_t u_yaw_angle; // 27 + big_uint16_buf_t u_cover_damping; // 29 + uint8_t u_plug_orientation; // 31 + uint8_t u_defective; // 32 + uint8_t u_supply_voltage_limit; // 33 + uint8_t u_sensor_off_temp; // 34 + uint8_t u_dynamics_invalid0; // 35 + uint8_t u_dynamics_invalid1; // 36 + uint8_t u_ext_disturbed; // 37 + uint8_t u_com_error; // 38 + big_uint16_buf_t u_sw_error; // 39 + uint8_t u_aln_status_azimuth_available; // 41 + big_uint16_buf_t u_aln_current_azimuth_std; // 42 + big_uint16_buf_t u_aln_current_azimuth; // 44 + big_uint16_buf_t u_aln_current_delta; // 46 + uint8_t reserved1[13]; // Reserved fields, no change needed + big_uint16_buf_t u_crc; // 61 + uint8_t u_sequence_counter; // 63 +}; + +struct ScanHeaderPacket +{ + big_uint32_buf_t u_time_stamp; // 0 + big_uint32_buf_t u_global_time_stamp_sec; // 4 + big_uint32_buf_t u_global_time_stamp_nsec; // 8 + uint8_t u_global_time_stamp_sync_status; // 12 + uint8_t u_signal_status; // 13 + uint8_t u_sequence_counter; // 14 + big_uint32_buf_t u_cycle_counter; // 15 + big_uint16_buf_t u_vambig; // 19. cSpell:ignore vambig + big_uint16_buf_t u_max_range; // 21 + big_uint16_buf_t u_number_of_detections; // 23 + uint8_t reserved[7]; // 25 +}; + +struct DetectionFragmentPacket +{ + uint8_t data[DETECTION_FRAGMENT_SIZE]; +}; + +struct DetectionPacket +{ + DetectionFragmentPacket fragments[FRAGMENTS_PER_DETECTION_PACKET]; // 0 - 59 + uint8_t reserved0; // 60 + uint8_t reserved1; // 61 + uint8_t u_message_counter; // 62 + uint8_t u_sequence_counter; // 63 +}; + +struct ObjectHeaderPacket +{ + big_uint32_buf_t u_time_stamp; // 0 + big_uint32_buf_t u_global_time_stamp_sec; // 4 + big_uint32_buf_t u_global_time_stamp_nsec; // 8 + uint8_t u_global_time_stamp_sync_status; // 12 + uint8_t u_signal_status; // 13 + uint8_t u_sequence_counter; // 14 + big_uint32_buf_t u_cycle_counter; // 15 + big_uint16_buf_t u_ego_vx; // 19 + big_uint16_buf_t u_ego_yaw_rate; // 21 + uint8_t u_motion_type; // 23 + uint8_t u_number_of_objects; // 24 + uint8_t reserved[7]; // 25 +}; + +struct ObjectFragmentPacket +{ + uint8_t data[OBJECT_FRAGMENT_SIZE]; +}; + +struct ObjectPacket +{ + ObjectFragmentPacket fragments[FRAGMENTS_PER_OBJECT_PACKET]; // 0 - 61 + uint8_t u_message_counter; // 62 + uint8_t u_sequence_counter; // 63 +}; + +struct ConfigurationPacket +{ + uint8_t u_sensor_id; + big_uint16_buf_t u_long_pose; + big_uint16_buf_t u_lat_pose; + big_uint16_buf_t u_vert_pos; + big_uint16_buf_t u_long_pos_cog; + big_uint16_buf_t u_wheelbase; + big_uint16_buf_t u_yaw_angle; + big_uint16_buf_t u_cover_damping; + uint8_t u_plug_orientation_and_default_settings; +}; + +struct SyncPacket +{ + uint8_t u_type; + uint8_t u_crc; + uint8_t u_time_domain_and_sequence_counter; + uint8_t u_user_data0; + big_uint32_buf_t u_sync_time_sec; +}; + +struct FollowUpPacket +{ + uint8_t u_type; + uint8_t u_crc; + uint8_t u_time_domain_and_sequence_counter; + uint8_t u_reserved; + big_uint32_buf_t u_sync_time_nsec; +}; + +#pragma pack(pop) + +struct PointSrr520Detection +{ + PCL_ADD_POINT4D; + float range; + float azimuth; + float range_rate; + float rcs; + uint8_t pdh00; + uint8_t pdh01; + uint8_t pdh02; + uint8_t pdh03; + uint8_t pdh04; + uint8_t pdh05; + float snr; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +} EIGEN_ALIGN16; + +// Note we only use a subset of the data since POINT_CLOUD_REGISTER_POINT_STRUCT has a limit in the +// number of fields +struct PointSrr520Object +{ + PCL_ADD_POINT4D; + uint32_t id; + uint16_t age; + float orientation; + float rcs; + float score; + uint8_t object_status; + float dynamics_abs_vel_x; + float dynamics_abs_vel_y; + float dynamics_abs_acc_x; + float dynamics_abs_acc_y; + float box_length; + float box_width; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +} EIGEN_ALIGN16; + +} // namespace continental_srr520 +} // namespace drivers +} // namespace nebula + +POINT_CLOUD_REGISTER_POINT_STRUCT( + nebula::drivers::continental_srr520::PointSrr520Detection, + (float, x, x)(float, y, y)(float, z, z)(float, azimuth, azimuth)(float, range, range)( + float, range_rate, range_rate)(int8_t, rcs, rcs)(uint8_t, pdh00, pdh00)(uint8_t, pdh01, pdh01)( + uint8_t, pdh02, + pdh02)(uint16_t, pdh03, pdh03)(uint8_t, pdh04, pdh04)(uint8_t, pdh05, pdh05)(float, snr, snr)) + +POINT_CLOUD_REGISTER_POINT_STRUCT( + nebula::drivers::continental_srr520::PointSrr520Object, + (float, x, x)(float, y, y)(float, z, z)(uint32_t, id, id)(uint16_t, age, age)( + float, orientation, + orientation)(float, rcs, rcs)(float, score, score)(uint8_t, object_status, object_status)( + float, dynamics_abs_vel_x, dynamics_abs_vel_x)(float, dynamics_abs_vel_y, dynamics_abs_vel_y)( + float, dynamics_abs_acc_x, dynamics_abs_acc_x)(float, dynamics_abs_acc_y, dynamics_abs_acc_y)( + float, box_length, box_length)(float, box_width, box_width)) diff --git a/nebula_common/include/nebula_common/continental/crc.hpp b/nebula_common/include/nebula_common/continental/crc.hpp new file mode 100644 index 000000000..d3085b776 --- /dev/null +++ b/nebula_common/include/nebula_common/continental/crc.hpp @@ -0,0 +1,64 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +template +int crc16_packets(Iterator begin, Iterator end, int payload_offset) +{ + uint16_t crc_word = 0xffff; + + for (Iterator it = begin; it != end; ++it) { + for (auto it2 = it->data.begin() + payload_offset; it2 != it->data.end(); ++it2) { + auto byte = *it2; + crc_word ^= byte << 8; + for (int i = 0; i < 8; i++) + crc_word = crc_word & 0x8000 ? (crc_word << 1) ^ 0x1021 : crc_word << 1; + } + } + + return crc_word; +} + +template +int crc16_packet(Iterator begin, Iterator end) +{ + uint16_t crc_word = 0xffff; + for (Iterator it = begin; it != end; ++it) { + crc_word ^= (*it) << 8; + for (int i = 0; i < 8; i++) + crc_word = crc_word & 0x8000 ? (crc_word << 1) ^ 0x1021 : crc_word << 1; + } + + return crc_word; +} + +template +uint8_t crc8h2f(Iterator begin, Iterator end) +{ + uint8_t crc_word = 0xFF; + uint8_t bit = 0; + + for (Iterator it = begin; it != end; ++it) { + crc_word ^= *it; + for (bit = 0; bit < 8; bit++) { + if ((crc_word & 0x80) != 0) { + crc_word <<= 1; + crc_word ^= 0x2F; + } else { + crc_word <<= 1; + } + } + } + + return crc_word ^ 0xFF; +} diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp index d5579a077..e4d600bc8 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp @@ -50,25 +50,22 @@ class ContinentalArs548Decoder : public ContinentalPacketsDecoder /// @brief Function for parsing NebulaPackets /// @param nebula_packets /// @return Resulting flag - bool ProcessPacket(const nebula_msgs::msg::NebulaPacket & nebula_packet) override; + bool ProcessPacket(std::unique_ptr packet_msg) override; /// @brief Function for parsing detection lists /// @param data /// @return Resulting flag - bool ParseDetectionsListPacket( - const std::vector & data, const builtin_interfaces::msg::Time & stamp); + bool ParseDetectionsListPacket(std::unique_ptr packet_msg); /// @brief Function for parsing object lists /// @param data /// @return Resulting flag - bool ParseObjectsListPacket( - const std::vector & data, const builtin_interfaces::msg::Time & stamp); + bool ParseObjectsListPacket(std::unique_ptr packet_msg); /// @brief Function for parsing sensor status messages /// @param data /// @return Resulting flag - bool ParseSensorStatusPacket( - const std::vector & data, const builtin_interfaces::msg::Time & stamp); + bool ParseSensorStatusPacket(std::unique_ptr packet_msg); /// @brief Register function to call when a new detection list is processed /// @param detection_list_callback @@ -90,12 +87,20 @@ class ContinentalArs548Decoder : public ContinentalPacketsDecoder Status RegisterSensorStatusCallback( std::function 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)> packets_callback); + private: std::function msg)> detection_list_callback_; std::function msg)> object_list_callback_; std::function sensor_status_callback_; + std::function msg)> + nebula_packets_callback_; ContinentalArs548Status radar_status_{}; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp new file mode 100644 index 000000000..3e09ffaa9 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp @@ -0,0 +1,213 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +namespace continental_srr520 +{ +/// @brief Continental Radar decoder (SRR520) +class ContinentalSrr520Decoder : public ContinentalPacketsDecoder +{ +public: + /// @brief Constructor + /// @param sensor_configuration SensorConfiguration for this decoder + explicit ContinentalSrr520Decoder( + const std::shared_ptr & 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 ProcessPacket(std::unique_ptr packet_msg) override; + + bool ParseDetectionsListPacket(const nebula_msgs::msg::NebulaPackets & nebula_packets, bool near); + bool ParseObjectsListPacket(const nebula_msgs::msg::NebulaPackets & nebula_packets); + bool ParseStatusPacket(const nebula_msgs::msg::NebulaPackets & nebula_packets); + + /// @brief Register function to call whenever a new rdi near detection list is processed + /// @param detection_list_callback + /// @return Resulting status + Status RegisterNearDetectionListCallback( + std::function)> + detection_list_callback); + + /// @brief Register function to call whenever a new rdi hrr detection list is processed + /// @param detection_list_callback + /// @return Resulting status + Status RegisterHRRDetectionListCallback( + std::function)> + detection_list_callback); + + /// @brief Register function to call whenever a new object list is processed + /// @param object_list_callback + /// @return Resulting status + Status RegisterObjectListCallback( + std::function)> + object_list_callback); + + /// @brief Register function to call whenever a new object list is processed + /// @param object_list_callback + /// @return Resulting status + Status RegisterStatusCallback( + std::function)> status_callback); + + /// @brief Register function to call whenever a sync fup packet is processed + /// @param sync_fup_callback + /// @return Resulting status + Status RegisterSyncFupCallback(std::function sync_fup_callback); + + /// @brief Register function to call whenever enough packets have been processed + /// @param object_list_callback + /// @return Resulting status + Status RegisterPacketsCallback( + std::function)> nebula_packets_callback); + + /// @brief Setting rclcpp::Logger + /// @param node Logger + void SetLogger(std::shared_ptr node); + +private: + /// @brief Process a new near detection header packet + /// @param buffer The buffer containing the packet + /// @param stamp The stamp in nanoseconds + void ProcessNearHeaderPacket(std::unique_ptr packet_msg); + + /// @brief Process a new near element packet + /// @param buffer The buffer containing the packet + /// @param stamp The stamp in nanoseconds + void ProcessNearElementPacket(std::unique_ptr packet_msg); + + /// @brief Process a new hrr header packet + /// @param buffer The buffer containing the packet + /// @param stamp The stamp in nanoseconds + void ProcessHRRHeaderPacket(std::unique_ptr packet_msg); + + /// @brief Process a new hrr element packet + /// @param buffer The buffer containing the packet + /// @param stamp The stamp in nanoseconds + void ProcessHRRElementPacket(std::unique_ptr packet_msg); + + /// @brief Process a new object header packet + /// @param buffer The buffer containing the packet + /// @param stamp The stamp in nanoseconds + void ProcessObjectHeaderPacket(std::unique_ptr packet_msg); + + /// @brief Process a new object element packet + /// @param buffer The buffer containing the packet + /// @param stamp The stamp in nanoseconds + void ProcessObjectElementPacket(std::unique_ptr packet_msg); + + /// @brief Process a new crc list packet + /// @param buffer The buffer containing the packet + /// @param stamp The stamp in nanoseconds + void ProcessCRCListPacket(std::unique_ptr packet_msg); + + /// @brief Process a new Near detections crc list packet + /// @param buffer The buffer containing the packet + /// @param stamp The stamp in nanoseconds + void ProcessNearCRCListPacket(std::unique_ptr packet_msg); + + /// @brief Process a new HRR crc list packet + /// @param buffer The buffer containing the packet + /// @param stamp The stamp in nanoseconds + void ProcessHRRCRCListPacket( + std::unique_ptr packet_msg); // cspell:ignore HRRCRC + + /// @brief Process a new objects crc list packet + /// @param buffer The buffer containing the packet + /// @param stamp The stamp in nanoseconds + void ProcessObjectCRCListPacket(std::unique_ptr packet_msg); + + /// @brief Process a new sensor status packet + /// @param buffer The buffer containing the status packet + /// @param stamp The stamp in nanoseconds + void ProcessSensorStatusPacket(std::unique_ptr packet_msg); + + /// @brief Process a new sensor status packet + /// @param buffer The buffer containing the status packet + /// @param stamp The stamp in nanoseconds + void ProcessSyncFupPacket(std::unique_ptr packet_msg); + + /// @brief Printing the string to RCLCPP_INFO_STREAM + /// @param info Target string + void PrintInfo(std::string info); + + /// @brief Printing the string to RCLCPP_ERROR_STREAM + /// @param error Target string + void PrintError(std::string error); + + /// @brief Printing the string to RCLCPP_DEBUG_STREAM + /// @param debug Target string + void PrintDebug(std::string debug); + + std::function msg)> + near_detection_list_callback_{}; + std::function msg)> + hrr_detection_list_callback_{}; + std::function msg)> + object_list_callback_{}; + std::function msg)> + status_callback_{}; + std::function sync_fup_callback_{}; + std::function msg)> + nebula_packets_callback_{}; + + std::unique_ptr rdi_near_packets_ptr_{}; + std::unique_ptr rdi_hrr_packets_ptr_{}; + std::unique_ptr object_packets_ptr_{}; + + std::unique_ptr near_detection_list_ptr_{}; + std::unique_ptr hrr_detection_list_ptr_{}; + std::unique_ptr object_list_ptr_{}; + + bool first_rdi_near_packet_{true}; + bool first_rdi_hrr_packet_{true}; + bool first_object_packet_{true}; + + ScanHeaderPacket rdi_near_header_packet_; + ScanHeaderPacket rdi_hrr_header_packet_; + ObjectHeaderPacket object_header_packet_; + + /// @brief SensorConfiguration for this decoder + std::shared_ptr + sensor_configuration_{}; + + std::shared_ptr parent_node_logger; +}; + +} // namespace continental_srr520 +} // namespace drivers +} // namespace nebula diff --git a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp index a7bf2cc31..762048de8 100644 --- a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp @@ -60,9 +60,17 @@ Status ContinentalArs548Decoder::RegisterSensorStatusCallback( return Status::OK; } -bool ContinentalArs548Decoder::ProcessPacket(const nebula_msgs::msg::NebulaPacket & nebula_packet) +Status ContinentalArs548Decoder::RegisterPacketsCallback( + std::function)> nebula_packets_callback) { - const auto & data = nebula_packet.data; + nebula_packets_callback_ = std::move(nebula_packets_callback); + return Status::OK; +} + +bool ContinentalArs548Decoder::ProcessPacket( + std::unique_ptr packet_msg) +{ + const auto & data = packet_msg->data; if (data.size() < sizeof(HeaderPacket)) { return false; @@ -82,13 +90,13 @@ bool ContinentalArs548Decoder::ProcessPacket(const nebula_msgs::msg::NebulaPacke return false; } - return ParseDetectionsListPacket(data, nebula_packet.stamp); + return ParseDetectionsListPacket(std::move(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_packet.stamp); + return ParseObjectsListPacket(std::move(packet_msg)); } else if (header.method_id.value() == SENSOR_STATUS_METHOD_ID) { if ( data.size() != SENSOR_STATUS_UDP_PAYLOAD || @@ -96,22 +104,22 @@ bool ContinentalArs548Decoder::ProcessPacket(const nebula_msgs::msg::NebulaPacke return false; } - return ParseSensorStatusPacket(data, nebula_packet.stamp); + return ParseSensorStatusPacket(std::move(packet_msg)); } return true; } bool ContinentalArs548Decoder::ParseDetectionsListPacket( - const std::vector & data, const builtin_interfaces::msg::Time & stamp) + std::unique_ptr packet_msg) { auto msg_ptr = std::make_unique(); 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; @@ -119,7 +127,7 @@ bool ContinentalArs548Decoder::ParseDetectionsListPacket( msg.header.stamp.nanosec = detection_list.stamp.timestamp_nanoseconds.value(); msg.header.stamp.sec = detection_list.stamp.timestamp_seconds.value(); } else { - msg.header.stamp = stamp; + msg.header.stamp = packet_msg->stamp; } msg.stamp_sync_status = detection_list.stamp.timestamp_sync_status; @@ -228,15 +236,15 @@ bool ContinentalArs548Decoder::ParseDetectionsListPacket( } bool ContinentalArs548Decoder::ParseObjectsListPacket( - const std::vector & data, const builtin_interfaces::msg::Time & stamp) + std::unique_ptr packet_msg) { auto msg_ptr = std::make_unique(); 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; @@ -244,7 +252,7 @@ bool ContinentalArs548Decoder::ParseObjectsListPacket( msg.header.stamp.nanosec = object_list.stamp.timestamp_nanoseconds.value(); msg.header.stamp.sec = object_list.stamp.timestamp_seconds.value(); } else { - msg.header.stamp = stamp; + msg.header.stamp = packet_msg->stamp; } msg.stamp_sync_status = object_list.stamp.timestamp_sync_status; @@ -380,10 +388,10 @@ bool ContinentalArs548Decoder::ParseObjectsListPacket( } bool ContinentalArs548Decoder::ParseSensorStatusPacket( - const std::vector & data, [[maybe_unused]] const builtin_interfaces::msg::Time & stamp) + std::unique_ptr 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 +621,17 @@ 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_); + } + + if (nebula_packets_callback_) { + auto packets_msg = std::make_unique(); + packets_msg->packets.emplace_back(std::move(*packet_msg)); + packets_msg->header.stamp = packet_msg->stamp; + packets_msg->header.frame_id = sensor_configuration_->frame_id; + nebula_packets_callback_(std::move(packets_msg)); + } return true; } diff --git a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp new file mode 100644 index 000000000..761a505f2 --- /dev/null +++ b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp @@ -0,0 +1,1133 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp" + +#include "nebula_common/continental/continental_srr520.hpp" + +#include + +#include + +#include +#include + +namespace nebula +{ +namespace drivers +{ +namespace continental_srr520 +{ +ContinentalSrr520Decoder::ContinentalSrr520Decoder( + const std::shared_ptr & + sensor_configuration) +{ + sensor_configuration_ = sensor_configuration; +} + +Status ContinentalSrr520Decoder::GetStatus() +{ + return Status::OK; +} + +Status ContinentalSrr520Decoder::RegisterNearDetectionListCallback( + std::function)> + detection_list_callback) +{ + near_detection_list_callback_ = std::move(detection_list_callback); + return Status::OK; +} + +Status ContinentalSrr520Decoder::RegisterHRRDetectionListCallback( + std::function)> + detection_list_callback) +{ + hrr_detection_list_callback_ = std::move(detection_list_callback); + return Status::OK; +} + +Status ContinentalSrr520Decoder::RegisterObjectListCallback( + std::function)> + object_list_callback) +{ + object_list_callback_ = std::move(object_list_callback); + return Status::OK; +} + +Status ContinentalSrr520Decoder::RegisterStatusCallback( + std::function)> status_callback) +{ + status_callback_ = std::move(status_callback); + return Status::OK; +} + +Status ContinentalSrr520Decoder::RegisterSyncFupCallback(std::function sync_fup_callback) +{ + sync_fup_callback_ = std::move(sync_fup_callback); + return Status::OK; +} + +Status ContinentalSrr520Decoder::RegisterPacketsCallback( + std::function)> nebula_packets_callback) +{ + nebula_packets_callback_ = std::move(nebula_packets_callback); + return Status::OK; +} + +bool ContinentalSrr520Decoder::ProcessPacket( + std::unique_ptr packet_msg) +{ + const uint32_t can_message_id = (static_cast(packet_msg->data[0]) << 24) | + (static_cast(packet_msg->data[1]) << 16) | + (static_cast(packet_msg->data[2]) << 8) | + static_cast(packet_msg->data[3]); + + std::size_t payload_size = packet_msg->data.size() - 4; + + if (can_message_id == RDI_NEAR_HEADER_CAN_MESSAGE_ID) { + if (payload_size != RDI_NEAR_HEADER_PACKET_SIZE) { + PrintError("RDI_NEAR_HEADER_CAN_MESSAGE_ID message with invalid size"); + return false; + } + ProcessNearHeaderPacket(std::move(packet_msg)); + } else if (can_message_id == RDI_NEAR_ELEMENT_CAN_MESSAGE_ID) { + if (payload_size != RDI_NEAR_ELEMENT_PACKET_SIZE) { + PrintError("RDI_NEAR_ELEMENT_CAN_MESSAGE_ID message with invalid size"); + return false; + } + + ProcessNearElementPacket(std::move(packet_msg)); + } else if (can_message_id == RDI_HRR_HEADER_CAN_MESSAGE_ID) { + if (payload_size != RDI_HRR_HEADER_PACKET_SIZE) { + PrintError("RDI_HRR_HEADER_CAN_MESSAGE_ID message with invalid size"); + return false; + } + ProcessHRRHeaderPacket(std::move(packet_msg)); + } else if (can_message_id == RDI_HRR_ELEMENT_CAN_MESSAGE_ID) { + if (payload_size != RDI_HRR_ELEMENT_PACKET_SIZE) { + PrintError("RDI_HRR_ELEMENT_CAN_MESSAGE_ID message with invalid size"); + return false; + } + + ProcessHRRElementPacket(std::move(packet_msg)); + } else if (can_message_id == OBJECT_HEADER_CAN_MESSAGE_ID) { + if (payload_size != OBJECT_HEADER_PACKET_SIZE) { + PrintError("OBJECT_HEADER_CAN_MESSAGE_ID message with invalid size"); + return false; + } + ProcessObjectHeaderPacket(std::move(packet_msg)); + } else if (can_message_id == OBJECT_CAN_MESSAGE_ID) { + if (payload_size != OBJECT_PACKET_SIZE) { + PrintError("OBJECT_ELEMENT_CAN_MESSAGE_ID message with invalid size"); + return false; + } + + ProcessObjectElementPacket(std::move(packet_msg)); + } else if (can_message_id == CRC_LIST_CAN_MESSAGE_ID) { + if (payload_size != CRC_LIST_PACKET_SIZE) { + PrintError("CRC_LIST_CAN_MESSAGE_ID message with invalid size"); + return false; + } + + ProcessCRCListPacket(std::move(packet_msg)); + } else if (can_message_id == STATUS_CAN_MESSAGE_ID) { + if (payload_size != STATUS_PACKET_SIZE) { + PrintError("CRC_LIST_CAN_MESSAGE_ID message with invalid size"); + return false; + } + + ProcessSensorStatusPacket(std::move(packet_msg)); + } else if (can_message_id == SYNC_FUP_CAN_MESSAGE_ID) { + if (payload_size != SYNC_FUP_CAN_PACKET_SIZE) { + PrintError("SYNC_FUP_CAN_MESSAGE_ID message with invalid size"); + return false; + } + + ProcessSyncFupPacket(std::move(packet_msg)); + } else if ( + can_message_id != VEH_DYN_CAN_MESSAGE_ID && can_message_id != SENSOR_CONFIG_CAN_MESSAGE_ID) { + PrintError("Unrecognized message ID=" + std::to_string(can_message_id)); + return false; + } + + return true; +} + +void ContinentalSrr520Decoder::ProcessNearHeaderPacket( + std::unique_ptr packet_msg) +{ + rdi_near_packets_ptr_ = std::make_unique(); + near_detection_list_ptr_ = + std::make_unique(); + first_rdi_near_packet_ = false; + + static_assert(sizeof(ScanHeaderPacket) == RDI_NEAR_HEADER_PACKET_SIZE); + static_assert(sizeof(DetectionPacket) == RDI_NEAR_ELEMENT_PACKET_SIZE); + assert(packet_msg->data.size() == RDI_NEAR_HEADER_PACKET_SIZE + 4); + + std::memcpy( + &rdi_near_header_packet_, packet_msg->data.data() + 4 * sizeof(uint8_t), + sizeof(ScanHeaderPacket)); + + assert( + rdi_near_header_packet_.u_global_time_stamp_sync_status >= 1 && + rdi_near_header_packet_.u_global_time_stamp_sync_status <= 3); + + near_detection_list_ptr_->header.frame_id = sensor_configuration_->frame_id; + + if (rdi_near_header_packet_.u_global_time_stamp_sync_status == 1) { + near_detection_list_ptr_->header.stamp.sec = + rdi_near_header_packet_.u_global_time_stamp_sec.value(); + near_detection_list_ptr_->header.stamp.nanosec = + rdi_near_header_packet_.u_global_time_stamp_nsec.value(); + } else { + near_detection_list_ptr_->header.stamp = packet_msg->stamp; + } + + rdi_near_packets_ptr_->header.stamp = packet_msg->stamp; + rdi_near_packets_ptr_->header.frame_id = sensor_configuration_->frame_id; + + near_detection_list_ptr_->internal_time_stamp_usec = rdi_near_header_packet_.u_time_stamp.value(); + near_detection_list_ptr_->global_time_stamp_sync_status = + rdi_near_header_packet_.u_global_time_stamp_sync_status; + near_detection_list_ptr_->signal_status = rdi_near_header_packet_.u_signal_status; + near_detection_list_ptr_->sequence_counter = rdi_near_header_packet_.u_sequence_counter; + near_detection_list_ptr_->cycle_counter = rdi_near_header_packet_.u_cycle_counter.value(); + near_detection_list_ptr_->vambig = + 0.003051851f * rdi_near_header_packet_.u_vambig.value() - 100.f; // cSpell:ignore vambig + near_detection_list_ptr_->max_range = 0.1f * rdi_near_header_packet_.u_max_range.value(); + + near_detection_list_ptr_->detections.reserve( + rdi_near_header_packet_.u_number_of_detections.value()); + + rdi_near_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); +} + +void ContinentalSrr520Decoder::ProcessNearElementPacket( + std::unique_ptr packet_msg) +{ + if (rdi_near_packets_ptr_->packets.size() == 0) { + if (!first_rdi_near_packet_) { + PrintError("Near element before header. This can happen during the first iteration"); + } + return; + } + + int parsed_detections = near_detection_list_ptr_->detections.size(); + + if ( + near_detection_list_ptr_->detections.size() >= + rdi_near_header_packet_.u_number_of_detections.value()) { + return; + } + + DetectionPacket detection_packet; + std::memcpy( + &detection_packet, packet_msg->data.data() + 4 * sizeof(uint8_t), sizeof(DetectionPacket)); + + static_assert(sizeof(DetectionPacket) == RDI_NEAR_ELEMENT_PACKET_SIZE); + assert(packet_msg->data.size() == RDI_NEAR_ELEMENT_PACKET_SIZE + 4); + assert(rdi_near_header_packet_.u_sequence_counter == detection_packet.u_sequence_counter); + assert(rdi_near_packets_ptr_->packets.size() == detection_packet.u_message_counter + 1); + + for (const auto & fragment : detection_packet.fragments) { + continental_msgs::msg::ContinentalSrr520Detection detection_msg; + const auto & data = fragment.data; + + if (parsed_detections >= rdi_near_header_packet_.u_number_of_detections.value()) { + break; + } + + uint16_t u_range = + (static_cast(data[0]) << 4) | (static_cast(data[1] & 0xF0) >> 4); + assert(u_range <= 4095); + detection_msg.range = 0.024420024 * u_range; + + uint16_t u_azimuth = + (static_cast(data[1] & 0x0f) << 5) | (static_cast(data[2] & 0xF8) >> 3); + assert(u_azimuth <= 510); + detection_msg.azimuth_angle = 0.006159986 * u_azimuth - 1.570796327; + + uint16_t u_range_rate = + (static_cast(data[2] & 0x07) << 8) | static_cast(data[3]); + assert(u_range_rate <= 2046); + detection_msg.range_rate = 0.014662757 * u_range_rate - 15.f; + + uint16_t u_rcs = (data[4] & 0xFE) >> 1; + assert(u_rcs <= 126); + detection_msg.rcs = 0.476190476 * u_rcs - 40.f; + + detection_msg.pdh00 = 100 * (data[4] & 0x01); + detection_msg.pdh01 = 100 * ((data[5] & 0x80) >> 7); + detection_msg.pdh02 = 100 * ((data[5] & 0x40) >> 6); + detection_msg.pdh03 = 100 * ((data[5] & 0x20) >> 5); + detection_msg.pdh04 = 100 * ((data[5] & 0x10) >> 4); + + uint8_t u_snr = data[5] & 0x0f; + detection_msg.snr = 1.7 * u_snr + 11.f; + + near_detection_list_ptr_->detections.push_back(detection_msg); + parsed_detections++; + } + + rdi_near_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); +} + +void ContinentalSrr520Decoder::ProcessHRRHeaderPacket( + std::unique_ptr packet_msg) +{ + rdi_hrr_packets_ptr_ = std::make_unique(); + hrr_detection_list_ptr_ = + std::make_unique(); + first_rdi_hrr_packet_ = false; + + static_assert(sizeof(ScanHeaderPacket) == RDI_HRR_HEADER_PACKET_SIZE); + assert(packet_msg->data.size() == RDI_NEAR_HEADER_PACKET_SIZE + 4); + + std::memcpy( + &rdi_hrr_header_packet_, packet_msg->data.data() + 4 * sizeof(uint8_t), + sizeof(ScanHeaderPacket)); + + assert( + rdi_hrr_header_packet_.u_global_time_stamp_sync_status >= 1 && + rdi_hrr_header_packet_.u_global_time_stamp_sync_status <= 3); + + hrr_detection_list_ptr_->header.frame_id = sensor_configuration_->frame_id; + + if (rdi_hrr_header_packet_.u_global_time_stamp_sync_status == 1) { + hrr_detection_list_ptr_->header.stamp.sec = + rdi_hrr_header_packet_.u_global_time_stamp_sec.value(); + hrr_detection_list_ptr_->header.stamp.nanosec = + rdi_hrr_header_packet_.u_global_time_stamp_nsec.value(); + } else { + hrr_detection_list_ptr_->header.stamp = packet_msg->stamp; + } + + rdi_hrr_packets_ptr_->header.stamp = packet_msg->stamp; + rdi_hrr_packets_ptr_->header.frame_id = sensor_configuration_->frame_id; + + near_detection_list_ptr_->internal_time_stamp_usec = rdi_near_header_packet_.u_time_stamp.value(); + near_detection_list_ptr_->global_time_stamp_sync_status = + rdi_near_header_packet_.u_global_time_stamp_sync_status; + near_detection_list_ptr_->signal_status = rdi_near_header_packet_.u_signal_status; + near_detection_list_ptr_->sequence_counter = rdi_near_header_packet_.u_sequence_counter; + near_detection_list_ptr_->cycle_counter = rdi_near_header_packet_.u_cycle_counter.value(); + near_detection_list_ptr_->vambig = + 0.003051851f * rdi_near_header_packet_.u_vambig.value() - 100.f; // cSpell:ignore vambig + near_detection_list_ptr_->max_range = 0.1f * rdi_near_header_packet_.u_max_range.value(); + + near_detection_list_ptr_->detections.reserve( + rdi_near_header_packet_.u_number_of_detections.value()); + + rdi_near_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); +} + +void ContinentalSrr520Decoder::ProcessHRRElementPacket( + std::unique_ptr packet_msg) +{ + if (rdi_hrr_packets_ptr_->packets.size() == 0) { + if (!first_rdi_hrr_packet_) { + PrintError("HRR element before header. This can happen during the first iteration"); + } + return; + } + + int parsed_detections = hrr_detection_list_ptr_->detections.size(); + + if ( + hrr_detection_list_ptr_->detections.size() >= + rdi_hrr_header_packet_.u_number_of_detections.value()) { + return; + } + + DetectionPacket detection_packet; + std::memcpy( + &detection_packet, packet_msg->data.data() + 4 * sizeof(uint8_t), sizeof(DetectionPacket)); + + static_assert(sizeof(DetectionPacket) == RDI_HRR_ELEMENT_PACKET_SIZE); + assert(packet_msg->data.size() == RDI_HRR_ELEMENT_PACKET_SIZE + 4); + assert(rdi_hrr_header_packet_.u_sequence_counter == detection_packet.u_sequence_counter); + assert(rdi_hrr_packets_ptr_->packets.size() == detection_packet.u_message_counter + 1); + + for (const auto & fragment : detection_packet.fragments) { + continental_msgs::msg::ContinentalSrr520Detection detection_msg; + const auto & data = fragment.data; + + if (parsed_detections >= rdi_hrr_header_packet_.u_number_of_detections.value()) { + break; + } + + uint16_t u_range = + (static_cast(data[0]) << 4) | (static_cast(data[1] & 0xF0) >> 4); + assert(u_range <= 4095); + detection_msg.range = 0.024420024 * u_range; + + uint16_t u_azimuth = + (static_cast(data[1] & 0x0f) << 5) | (static_cast(data[2] & 0xF8) >> 3); + assert(u_azimuth <= 510); + detection_msg.azimuth_angle = 0.006159986 * u_azimuth - 1.570796327; + + uint16_t u_range_rate = + (static_cast(data[2] & 0x07) << 8) | static_cast(data[3]); + assert(u_range_rate <= 2046); + detection_msg.range_rate = 0.014662757 * u_range_rate - 15.f; + + uint16_t u_rcs = (data[4] & 0xFE) >> 1; + assert(u_rcs <= 126); + detection_msg.rcs = 0.476190476 * u_rcs - 40.f; + + detection_msg.pdh00 = 100 * (data[4] & 0x01); + detection_msg.pdh01 = 100 * ((data[5] & 0x80) >> 7); + detection_msg.pdh02 = 100 * ((data[5] & 0x40) >> 6); + detection_msg.pdh03 = 100 * ((data[5] & 0x20) >> 5); + detection_msg.pdh04 = 100 * ((data[5] & 0x10) >> 4); + + uint8_t u_snr = data[5] & 0x0f; + detection_msg.snr = 1.7 * u_snr + 11.f; + + hrr_detection_list_ptr_->detections.push_back(detection_msg); + parsed_detections++; + } + + rdi_hrr_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); +} + +void ContinentalSrr520Decoder::ProcessObjectHeaderPacket( + std::unique_ptr packet_msg) +{ + object_packets_ptr_ = std::make_unique(); + object_list_ptr_ = std::make_unique(); + first_object_packet_ = false; + + static_assert(sizeof(ObjectHeaderPacket) == OBJECT_HEADER_PACKET_SIZE); + assert(packet_msg->data.size() == OBJECT_HEADER_PACKET_SIZE + 4); + + std::memcpy( + &object_header_packet_, packet_msg->data.data() + 4 * sizeof(uint8_t), + sizeof(ObjectHeaderPacket)); + + assert( + object_header_packet_.u_global_time_stamp_sync_status >= 1 && + object_header_packet_.u_global_time_stamp_sync_status <= 3); + + object_list_ptr_->header.frame_id = sensor_configuration_->frame_id; + + if (object_header_packet_.u_global_time_stamp_sync_status == 1) { + object_list_ptr_->header.stamp.sec = object_header_packet_.u_global_time_stamp_sec.value(); + object_list_ptr_->header.stamp.nanosec = object_header_packet_.u_global_time_stamp_nsec.value(); + } else { + object_list_ptr_->header.stamp = packet_msg->stamp; + } + + object_packets_ptr_->header.stamp = packet_msg->stamp; + object_packets_ptr_->header.frame_id = sensor_configuration_->frame_id; + + object_list_ptr_->internal_time_stamp_usec = object_header_packet_.u_time_stamp.value(); + object_list_ptr_->global_time_stamp_sync_status = + object_header_packet_.u_global_time_stamp_sync_status; + object_list_ptr_->signal_status = object_header_packet_.u_signal_status; + object_list_ptr_->sequence_counter = object_header_packet_.u_sequence_counter; + object_list_ptr_->cycle_counter = object_header_packet_.u_cycle_counter.value(); + object_list_ptr_->ego_vx = 0.003051851 * object_header_packet_.u_ego_vx.value() - 100.f; + object_list_ptr_->ego_yaw_rate = + 9.58766e-05 * object_header_packet_.u_ego_yaw_rate.value() - 3.14159; + object_list_ptr_->motion_type = object_header_packet_.u_motion_type; + + object_list_ptr_->objects.reserve(object_header_packet_.u_number_of_objects); + + object_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); +} + +void ContinentalSrr520Decoder::ProcessObjectElementPacket( + std::unique_ptr packet_msg) +{ + if (object_packets_ptr_->packets.size() == 0) { + if (!first_object_packet_) { + PrintError("Object element before header. This can happen during the first iteration"); + } + return; + } + + if (object_list_ptr_->objects.size() >= object_header_packet_.u_number_of_objects) { + return; + } + + ObjectPacket object_packet; + std::memcpy(&object_packet, packet_msg->data.data() + 4 * sizeof(uint8_t), sizeof(ObjectPacket)); + + static_assert(sizeof(ObjectPacket) == OBJECT_PACKET_SIZE); + assert(packet_msg->data.size() == OBJECT_PACKET_SIZE + 4); + assert(object_header_packet_.u_sequence_counter == object_packet.u_sequence_counter); + assert(object_packets_ptr_->packets.size() == object_packet.u_message_counter + 1); + + for (const auto & fragment : object_packet.fragments) { + continental_msgs::msg::ContinentalSrr520Object object_msg; + const auto & data = fragment.data; + + if (object_list_ptr_->objects.size() >= object_header_packet_.u_number_of_objects) { + break; + } + + object_msg.object_id = data[0]; + + uint16_t u_dist_x = ((static_cast(data[1]) << 8) | data[2]); + uint16_t u_dist_y = ((static_cast(data[3]) << 8) | data[4]); + assert(u_dist_x <= 65534); + assert(u_dist_y <= 65534); + object_msg.dist_x = 0.009155553 * u_dist_x - 300.f; + object_msg.dist_y = 0.009155553 * u_dist_y - 300.f; + + uint16_t u_v_abs_x = + (static_cast(data[5]) << 6) | (static_cast(data[6] & 0xfc) >> 2); + assert(u_v_abs_x <= 16382); + object_msg.v_abs_x = 0.009156391 * u_v_abs_x - 75.f; + + uint16_t u_v_abs_y = (static_cast(data[6] & 0x03) << 12) | + (static_cast(data[7]) << 4) | + (static_cast(data[8] & 0xF0) >> 4); + assert(u_v_abs_y <= 16382); + object_msg.v_abs_y = 0.009156391 * u_v_abs_y - 75.f; + + uint16_t u_a_abs_x = + (static_cast(data[8] & 0x0f) << 6) | (static_cast(data[9] & 0xfc) >> 2); + assert(u_a_abs_x <= 1022); + object_msg.a_abs_x = 0.019569472 * u_a_abs_x - 10.f; + + uint16_t u_a_abs_y = + (static_cast(data[9] & 0x03) << 8) | static_cast(data[10]); + assert(u_a_abs_y <= 1022); + object_msg.a_abs_y = 0.019569472 * u_a_abs_y - 10.f; + + uint16_t u_dist_x_std = + (static_cast(data[11]) << 6) | (static_cast(data[12] & 0xfc) >> 2); + assert(u_dist_x_std <= 16383); + object_msg.dist_x_std = 0.001831166 * u_dist_x_std; + + uint16_t u_dist_y_std = (static_cast(data[12] & 0x03) << 12) | + (static_cast(data[13]) << 4) | + (static_cast(data[14] & 0xF0) >> 4); + assert(u_dist_y_std <= 16383); + object_msg.dist_y_std = 0.001831166 * u_dist_y_std; + + uint16_t u_v_abs_x_std = (static_cast(data[14] & 0x0f) << 10) | + (static_cast(data[15]) << 2) | + (static_cast(data[15] & 0x03) >> 6); + assert(u_v_abs_x_std <= 16383); + object_msg.v_abs_x_std = 0.001831166 * u_v_abs_x_std; + + uint16_t u_v_abs_y_std = + (static_cast(data[16] & 0x3f) << 8) | static_cast(data[17]); + assert(u_v_abs_y_std <= 16383); + object_msg.v_abs_y_std = 0.001831166 * u_v_abs_y_std; + + uint16_t u_a_abs_x_std = + (static_cast(data[18]) << 6) | (static_cast(data[19] & 0xfc) >> 2); + assert(u_a_abs_x_std <= 16383); + object_msg.a_abs_x_std = 0.001831166 * u_a_abs_x_std; + + uint16_t u_a_abs_y_std = (static_cast(data[19] & 0x03) << 12) | + (static_cast(data[20]) << 4) | + (static_cast(data[21] & 0xF0) >> 4); + assert(u_a_abs_y_std <= 16383); + object_msg.a_abs_y_std = 0.001831166 * u_a_abs_y_std; + + uint16_t u_box_length = + (static_cast(data[21] & 0x0f) << 8) | static_cast(data[22]); + assert(u_box_length <= 4095); + object_msg.box_length = 0.007326007 * u_box_length; + + uint16_t u_box_width = + (static_cast(data[23]) << 4) | (static_cast(data[24] & 0xF0) >> 4); + assert(u_box_width <= 4095); + object_msg.box_width = 0.007326007 * u_box_width; + + uint16_t u_orientation = + (static_cast(data[24] & 0x0f) << 8) | static_cast(data[25]); + assert(u_orientation <= 4094); + object_msg.orientation = 0.001534729 * u_orientation - 3.14159; + + uint16_t u_rcs = + (static_cast(data[26]) << 4) | (static_cast(data[27] & 0xF0) >> 4); + assert(u_rcs <= 4094); + object_msg.rcs = 0.024425989 * u_rcs - 50.f; + + uint8_t u_score = data[27] & 0x0f; + assert(u_score <= 15); + object_msg.score = 6.666666667 * u_score; + + object_msg.life_cycles = (static_cast(data[28]) << 8) | data[29]; + assert(object_msg.life_cycles <= 65535); + + object_msg.box_valid = data[30] & 0x01; + object_msg.object_status = (data[30] & 0x06) >> 1; + + object_list_ptr_->objects.push_back(object_msg); + } + + object_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); +} + +void ContinentalSrr520Decoder::ProcessCRCListPacket( + std::unique_ptr packet_msg) +{ + const auto crc_id = packet_msg->data[4]; // first 4 bits are the can id + + if (crc_id == NEAR_CRC_ID) { + ProcessNearCRCListPacket(std::move(packet_msg)); + } else if (crc_id == HRR_CRC_ID) { + ProcessHRRCRCListPacket(std::move(packet_msg)); // cspell: ignore HRRCRC + } else if (crc_id == OBJECT_CRC_ID) { + ProcessObjectCRCListPacket(std::move(packet_msg)); + } else { + PrintError(std::string("Unrecognized CRC id=") + std::to_string(crc_id)); + } +} + +void ContinentalSrr520Decoder::ProcessNearCRCListPacket( + std::unique_ptr packet_msg) +{ + if (rdi_near_packets_ptr_->packets.size() != RDI_NEAR_PACKET_NUM + 1) { + if (!first_rdi_near_packet_) { + PrintError("Incorrect number of RDI Near elements before CRC list"); + } + + near_detection_list_ptr_.reset(); + rdi_near_packets_ptr_.reset(); + return; + } + + uint16_t transmitted_crc = + (static_cast(packet_msg->data[5]) << 8) | packet_msg->data[6]; + uint16_t computed_crc = + crc16_packets(rdi_near_packets_ptr_->packets.begin(), rdi_near_packets_ptr_->packets.end(), 4); + + if (transmitted_crc != computed_crc) { + PrintError( + "RDI Near: Transmitted CRC list does not coincide with the computed one. Ignoring packet"); + + near_detection_list_ptr_.reset(); + rdi_near_packets_ptr_.reset(); + return; + } + + rdi_near_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); + + if (near_detection_list_callback_) { + near_detection_list_callback_(std::move(near_detection_list_ptr_)); + } + + if (nebula_packets_callback_) { + nebula_packets_callback_(std::move(rdi_near_packets_ptr_)); + } + + near_detection_list_ptr_.reset(); + rdi_near_packets_ptr_.reset(); +} + +void ContinentalSrr520Decoder::ProcessHRRCRCListPacket( + std::unique_ptr packet_msg) +{ + if (rdi_hrr_packets_ptr_->packets.size() != RDI_HRR_PACKET_NUM + 1) { + if (!first_rdi_hrr_packet_) { + PrintError("Incorrect number of RDI HRR elements before CRC list"); + } + + hrr_detection_list_ptr_.reset(); + rdi_hrr_packets_ptr_.reset(); + return; + } + + uint16_t transmitted_crc = + (static_cast(packet_msg->data[5]) << 8) | packet_msg->data[6]; + uint16_t computed_crc = + crc16_packets(rdi_hrr_packets_ptr_->packets.begin(), rdi_hrr_packets_ptr_->packets.end(), 4); + + if (transmitted_crc != computed_crc) { + PrintError( + "RDI HRR: Transmitted CRC list does not coincide with the computed one. Ignoring packet"); + hrr_detection_list_ptr_.reset(); + rdi_hrr_packets_ptr_.reset(); + return; + } + + rdi_hrr_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); + + if (hrr_detection_list_callback_) { + hrr_detection_list_callback_(std::move(hrr_detection_list_ptr_)); + } + + if (nebula_packets_callback_) { + nebula_packets_callback_(std::move(rdi_hrr_packets_ptr_)); + } + + hrr_detection_list_ptr_.reset(); + rdi_hrr_packets_ptr_.reset(); +} + +void ContinentalSrr520Decoder::ProcessObjectCRCListPacket( + std::unique_ptr packet_msg) +{ + if (object_packets_ptr_->packets.size() != OBJECT_PACKET_NUM + 1) { + if (!first_object_packet_) { + PrintError("Incorrect number of object packages before CRC list"); + } + + object_list_ptr_.reset(); + object_packets_ptr_.reset(); + return; + } + + uint16_t transmitted_crc = + (static_cast(packet_msg->data[5]) << 8) | packet_msg->data[6]; + uint16_t computed_crc = + crc16_packets(object_packets_ptr_->packets.begin(), object_packets_ptr_->packets.end(), 4); + + // uint8_t current_seq = buffer[3]; + + if (transmitted_crc != computed_crc) { + PrintError( + "Object: Transmitted CRC list does not coincide with the computed one. Ignoring packet"); + + object_list_ptr_.reset(); + object_packets_ptr_.reset(); + return; + } + + object_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); + + if (object_list_callback_) { + object_list_callback_(std::move(object_list_ptr_)); + } + + if (nebula_packets_callback_) { + nebula_packets_callback_(std::move(object_packets_ptr_)); + } + + object_list_ptr_.reset(); + object_packets_ptr_.reset(); +} + +void ContinentalSrr520Decoder::ProcessSensorStatusPacket( + std::unique_ptr packet_msg) +{ + static_assert(sizeof(StatusPacket) == STATUS_PACKET_SIZE); + + StatusPacket status_packet; + std::memcpy(&status_packet, packet_msg->data.data() + 4 * sizeof(uint8_t), sizeof(status_packet)); + + auto diagnostic_array_msg_ptr = std::make_unique(); + + diagnostic_array_msg_ptr->header.frame_id = sensor_configuration_->frame_id; + diagnostic_array_msg_ptr->header.stamp = packet_msg->stamp; + diagnostic_array_msg_ptr->status.resize(1); + + auto & diagnostic_status = diagnostic_array_msg_ptr->status.front(); + auto & diagnostic_values = diagnostic_status.values; + diagnostic_values.reserve(33); + diagnostic_status.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + + diagnostic_status.message = "Sensor diagnostics for the SRR520"; + diagnostic_status.hardware_id = "SRR"; + diagnostic_status.name = "SRR"; + diagnostic_msgs::msg::KeyValue key_value; + + key_value.key = "time_stamp"; + key_value.value = std::to_string(status_packet.u_time_stamp.value()); + diagnostic_values.push_back(key_value); + + key_value.key = "global_time_stamp_sec"; + key_value.value = std::to_string(status_packet.u_global_time_stamp_sec.value()); + diagnostic_values.push_back(key_value); + + key_value.key = "global_time_stamp_nsec"; + key_value.value = std::to_string(status_packet.u_global_time_stamp_nsec.value()); + diagnostic_values.push_back(key_value); + + key_value.key = "global_time_stamp_sync_status"; + key_value.value = std::to_string(status_packet.u_global_time_stamp_sync_status); + diagnostic_values.push_back(key_value); + diagnostic_status.level = status_packet.u_global_time_stamp_sync_status != 1 + ? diagnostic_msgs::msg::DiagnosticStatus::WARN + : diagnostic_status.level; + + std::stringstream sw_version_ss; + sw_version_ss << static_cast(status_packet.u_sw_version_major) << "." + << static_cast(status_packet.u_sw_version_minor) << "." + << static_cast(status_packet.u_sw_version_patch); + key_value.key = "sw_version_patch"; + key_value.value = sw_version_ss.str(); + diagnostic_values.push_back(key_value); + + key_value.key = "sensor_id"; + key_value.value = std::to_string(status_packet.u_sensor_id); + diagnostic_values.push_back(key_value); + + key_value.key = "long_pos"; + key_value.value = std::to_string(1e-3 * status_packet.u_long_pos.value() - 32.767); + diagnostic_values.push_back(key_value); + + key_value.key = "lat_pos"; + key_value.value = std::to_string(1e-3 * status_packet.u_lat_pos.value() - 32.767); + diagnostic_values.push_back(key_value); + + key_value.key = "vert_pos"; + key_value.value = std::to_string(1e-3 * status_packet.u_vert_pos.value() - 32.767); + diagnostic_values.push_back(key_value); + + key_value.key = "long_pos_cog"; + key_value.value = std::to_string(1e-3 * status_packet.u_long_pos_cog.value() - 32.767); + diagnostic_values.push_back(key_value); + + key_value.key = "wheelbase"; + key_value.value = std::to_string(1e-3 * status_packet.u_wheelbase.value()); + diagnostic_values.push_back(key_value); + + key_value.key = "yaw_angle"; + key_value.value = std::to_string(9.58766e-05 * status_packet.u_yaw_angle.value() - 3.14159); + diagnostic_values.push_back(key_value); + + key_value.key = "cover_damping"; + key_value.value = std::to_string(1e-3 * status_packet.u_cover_damping.value() - 32.767); + diagnostic_values.push_back(key_value); + + uint8_t plug_orientation = status_packet.u_plug_orientation & 0b1; + key_value.key = "plug_orientation"; + key_value.value = plug_orientation == 0 ? "0:PLUG_BOTTOM" : "1:PLUG_TOP"; + diagnostic_values.push_back(key_value); + + std::vector defective_message_vector; + + if (status_packet.u_defective & 0x01) { + defective_message_vector.push_back("Current Near Scan RF Error"); + } + if (status_packet.u_defective & 0x02) { + defective_message_vector.push_back("Near Scan RF this OPC"); + } + if (status_packet.u_defective & 0x04) { + defective_message_vector.push_back("Current HRR Scan RF Error"); + } + if (status_packet.u_defective & 0x08) { + defective_message_vector.push_back("HRR Scan RF this OPC"); + } + if (status_packet.u_defective & 0x10) { + defective_message_vector.push_back("Current RF HW Error"); + } + if (status_packet.u_defective & 0x20) { + defective_message_vector.push_back("RF HW Error this OPC"); + } + if (status_packet.u_defective & 0x40) { + defective_message_vector.push_back("Current HW Error"); + } + if (status_packet.u_defective & 0x80) { + defective_message_vector.push_back("HW Error this OPC"); + } + + key_value.key = "defective"; + key_value.value = defective_message_vector.size() == 0 + ? "Ok" + : boost::algorithm::join(defective_message_vector, ", "); + diagnostic_values.push_back(key_value); + diagnostic_status.level = status_packet.u_defective != 0 + ? diagnostic_msgs::msg::DiagnosticStatus::ERROR + : diagnostic_status.level; + + std::vector supply_voltage_message_vector; + + if (status_packet.u_supply_voltage_limit & 0x01) { + supply_voltage_message_vector.push_back("Current Overvoltage Error"); + } + if (status_packet.u_supply_voltage_limit & 0x02) { + supply_voltage_message_vector.push_back("Overvoltage Error this OPC"); + } + if (status_packet.u_supply_voltage_limit & 0x04) { + supply_voltage_message_vector.push_back("Current Undervoltage Error"); + } + if (status_packet.u_supply_voltage_limit & 0x08) { + supply_voltage_message_vector.push_back("Undervoltage Error this OPC"); + } + + key_value.key = "supply_voltage_limit"; + key_value.value = supply_voltage_message_vector.size() == 0 + ? "Ok" + : boost::algorithm::join(supply_voltage_message_vector, ", "); + diagnostic_values.push_back(key_value); + diagnostic_status.level = status_packet.u_supply_voltage_limit != 0 + ? diagnostic_msgs::msg::DiagnosticStatus::ERROR + : diagnostic_status.level; + + std::vector temperature_message_vector; + + if (status_packet.u_sensor_off_temp & 0x01) { + temperature_message_vector.push_back("Current Overtemperature Error"); + } + if (status_packet.u_sensor_off_temp & 0x02) { + temperature_message_vector.push_back("Overtemperature Error this OPC"); + } + if (status_packet.u_sensor_off_temp & 0x04) { + temperature_message_vector.push_back("Current Undertemperature Error"); + } + if (status_packet.u_sensor_off_temp & 0x08) { + temperature_message_vector.push_back("Undertemperature Error this OPC"); + } + + key_value.key = "sensor_off_temp"; + key_value.value = temperature_message_vector.size() == 0 + ? "Ok" + : boost::algorithm::join(temperature_message_vector, ", "); + diagnostic_values.push_back(key_value); + diagnostic_status.level = status_packet.u_sensor_off_temp != 0 + ? diagnostic_msgs::msg::DiagnosticStatus::ERROR + : diagnostic_status.level; + + auto valid_flag_to_string = [](uint8_t status) -> std::string { + std::vector status_vector; + if (status == 0) { + return "Ok"; + } + if (status > 3) { + return "Invalid value"; + } + if (status & 0x01) { + status_vector.push_back("Current error"); + } + if (status & 0x02) { + status_vector.push_back("Error this OPC"); + } + + return boost::algorithm::join(status_vector, ", "); + }; + + uint8_t u_long_vel_invalid = status_packet.u_dynamics_invalid0 & 0b00000011; + key_value.key = "long_vel_invalid"; + key_value.value = valid_flag_to_string(u_long_vel_invalid); + diagnostic_values.push_back(key_value); + + uint8_t u_long_accel_invalid = (status_packet.u_dynamics_invalid0 & 0b00001100) >> 2; + key_value.key = "long_accel_invalid"; + key_value.value = valid_flag_to_string(u_long_accel_invalid); + diagnostic_values.push_back(key_value); + + uint8_t u_lat_accel_invalid = (status_packet.u_dynamics_invalid0 & 0b00110000) >> 4; + key_value.key = "lat_accel_invalid"; + key_value.value = valid_flag_to_string(u_lat_accel_invalid); + diagnostic_values.push_back(key_value); + + uint8_t u_long_dir_invalid = (status_packet.u_dynamics_invalid0 & 0b11000000) >> 6; + key_value.key = "long_dir_invalid"; + key_value.value = valid_flag_to_string(u_long_dir_invalid); + diagnostic_values.push_back(key_value); + diagnostic_status.level = status_packet.u_dynamics_invalid0 != 0 + ? diagnostic_msgs::msg::DiagnosticStatus::WARN + : diagnostic_status.level; + + uint8_t u_yaw_rate_invalid = status_packet.u_dynamics_invalid1 & 0b00000011; + key_value.key = "yaw_rate_invalid"; + key_value.value = valid_flag_to_string(u_yaw_rate_invalid); + diagnostic_values.push_back(key_value); + diagnostic_status.level = status_packet.u_dynamics_invalid1 != 0 + ? diagnostic_msgs::msg::DiagnosticStatus::WARN + : diagnostic_status.level; + + std::vector ext_disturbed_message_vector; + + if (status_packet.u_ext_disturbed & 0x01) { + ext_disturbed_message_vector.push_back("Current Near Scan RF Error"); + } + if (status_packet.u_ext_disturbed & 0x02) { + ext_disturbed_message_vector.push_back("Near Scan RF this OPC"); + } + if (status_packet.u_ext_disturbed & 0x04) { + ext_disturbed_message_vector.push_back("Current HRR Scan RF Error"); + } + if (status_packet.u_ext_disturbed & 0x08) { + ext_disturbed_message_vector.push_back("HRR Scan RF this OPC"); + } + if (status_packet.u_ext_disturbed & 0x10) { + ext_disturbed_message_vector.push_back("Current RF HW Error"); + } + if (status_packet.u_ext_disturbed & 0x20) { + ext_disturbed_message_vector.push_back("RF HW Error this OPC"); + } + if (status_packet.u_ext_disturbed & 0x40) { + ext_disturbed_message_vector.push_back("Current HW Error"); + } + if (status_packet.u_ext_disturbed & 0x80) { + ext_disturbed_message_vector.push_back("HW Error this OPC"); + } + + key_value.key = "ext_disturbed"; + key_value.value = ext_disturbed_message_vector.size() == 0 + ? "Ok" + : boost::algorithm::join(ext_disturbed_message_vector, ", "); + diagnostic_values.push_back(key_value); + diagnostic_status.level = status_packet.u_ext_disturbed != 0 + ? diagnostic_msgs::msg::DiagnosticStatus::ERROR + : diagnostic_status.level; + + key_value.key = "com_error"; + key_value.value = valid_flag_to_string(status_packet.u_com_error); + diagnostic_values.push_back(key_value); + diagnostic_status.level = status_packet.u_com_error != 0 + ? diagnostic_msgs::msg::DiagnosticStatus::ERROR + : diagnostic_status.level; + + std::vector sw_message_vector; + uint8_t u_sw_error = status_packet.u_sw_error.value() & 0xff; + + if (u_sw_error & 0x01) { + sw_message_vector.push_back("Current Internal SW Error"); + } + if (u_sw_error & 0x02) { + sw_message_vector.push_back("Internal SW Error this OPC"); + } + if (u_sw_error & 0x04) { + sw_message_vector.push_back("Reset Error"); + } + if (u_sw_error & 0x08) { + sw_message_vector.push_back("Current Nvm Integrity"); + } + if (u_sw_error & 0x10) { + sw_message_vector.push_back("Nvm Integrity Error this OPC"); + } + if (u_sw_error & 0x20) { + sw_message_vector.push_back("RF HW Error this OPC"); + } + if (u_sw_error & 0x40) { + sw_message_vector.push_back("Runtime Error"); + } + if (u_sw_error & 0x80) { + sw_message_vector.push_back("Last Sensor Config Message Rejected Upper"); + } + + key_value.key = "sw_error"; + key_value.value = + sw_message_vector.size() == 0 ? "Ok" : boost::algorithm::join(sw_message_vector, ", "); + diagnostic_values.push_back(key_value); + diagnostic_status.level = (status_packet.u_sw_error.value() & 0x0f) != 0 + ? diagnostic_msgs::msg::DiagnosticStatus::ERROR + : diagnostic_status.level; + + bool aln_driving = status_packet.u_aln_status_azimuth_available & 0b00000001; + bool aln_sensor = (status_packet.u_aln_status_azimuth_available & 0b00000010) >> 1; + + key_value.key = "aln_status_driving"; + key_value.value = aln_driving ? "Driving conditions met" : "Driving conditions NOT met"; + diagnostic_values.push_back(key_value); + + key_value.key = "aln_status_sensor"; + key_value.value = aln_sensor ? "Sensor is adjusted" : "Sensor is not adjusted"; + diagnostic_values.push_back(key_value); + + key_value.key = "aln_azimuth_available"; + key_value.value = + std::to_string((status_packet.u_aln_status_azimuth_available & 0b00000100) >> 2); + diagnostic_values.push_back(key_value); + + key_value.key = "aln_current_azimuth_std"; + key_value.value = std::to_string(1.52593e-05 * status_packet.u_aln_current_azimuth_std.value()); + diagnostic_values.push_back(key_value); + + key_value.key = "aln_current_azimuth"; + key_value.value = + std::to_string(9.58766e-05 * status_packet.u_aln_current_azimuth.value() - 3.14159); + diagnostic_values.push_back(key_value); + + key_value.key = "aln_current_delta"; + key_value.value = + std::to_string(9.58766e-05 * status_packet.u_aln_current_delta.value() - 3.14159); + diagnostic_values.push_back(key_value); + + uint16_t computed_crc = crc16_packet(packet_msg->data.begin() + 4, packet_msg->data.end() - 3); + key_value.key = "crc_check"; + key_value.value = + std::to_string(status_packet.u_crc.value()) + "|" + std::to_string(computed_crc); + diagnostic_values.push_back(key_value); + + key_value.key = "sequence_counter"; + key_value.value = std::to_string(status_packet.u_sequence_counter); + diagnostic_values.push_back(key_value); + + if (status_callback_) { + status_callback_(std::move(diagnostic_array_msg_ptr)); + } + + auto nebula_packets = std::make_unique(); + nebula_packets->header.stamp = packet_msg->stamp; + nebula_packets->header.frame_id = sensor_configuration_->frame_id; + nebula_packets->packets.emplace_back(std::move(*packet_msg)); + + if (nebula_packets_callback_) { + nebula_packets_callback_(std::move(nebula_packets)); + } +} + +void ContinentalSrr520Decoder::ProcessSyncFupPacket( + std::unique_ptr packet_msg) +{ + if (sync_fup_callback_) { + sync_fup_callback_(); + } + + auto nebula_packets = std::make_unique(); + nebula_packets->header.stamp = packet_msg->stamp; + nebula_packets->header.frame_id = sensor_configuration_->frame_id; + nebula_packets->packets.emplace_back(std::move(*packet_msg)); + + if (nebula_packets_callback_) { + nebula_packets_callback_(std::move(nebula_packets)); + } +} + +bool ContinentalSrr520Decoder::ParseStatusPacket( + const nebula_msgs::msg::NebulaPackets & nebula_packets) +{ + assert(false); + return true; +} + +void ContinentalSrr520Decoder::SetLogger(std::shared_ptr logger) +{ + parent_node_logger = logger; +} + +void ContinentalSrr520Decoder::PrintInfo(std::string info) +{ + if (parent_node_logger) { + RCLCPP_INFO_STREAM((*parent_node_logger), info); + } else { + std::cout << info << std::endl; + } +} + +void ContinentalSrr520Decoder::PrintError(std::string error) +{ + if (parent_node_logger) { + RCLCPP_ERROR_STREAM((*parent_node_logger), error); + } else { + std::cerr << error << std::endl; + } +} + +void ContinentalSrr520Decoder::PrintDebug(std::string debug) +{ + if (parent_node_logger) { + RCLCPP_DEBUG_STREAM((*parent_node_logger), debug); + } else { + std::cout << debug << std::endl; + } +} + +} // namespace continental_srr520 +} // namespace drivers +} // namespace nebula diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp index c9389079c..1ffdb2c05 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp @@ -49,7 +49,7 @@ namespace drivers namespace continental_ars548 { /// @brief Hardware interface of the Continental ARS548 radar -class ContinentalArs548HwInterface /* : NebulaHwInterfaceBase */ +class ContinentalArs548HwInterface { private: std::unique_ptr<::drivers::common::IoContext> sensor_io_context_; @@ -109,11 +109,6 @@ class ContinentalArs548HwInterface /* : NebulaHwInterfaceBase */ /// @return Resulting status Status SensorInterfaceStop(); - /// @brief Printing sensor configuration - /// @param sensor_configuration SensorConfiguration for this interface - /// @return Resulting status - Status GetSensorConfiguration(SensorConfigurationBase & sensor_configuration); - /// @brief Setting sensor configuration /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp new file mode 100644 index 000000000..ff1724c31 --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp @@ -0,0 +1,187 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NEBULA_CONTINENTAL_SRR520_HW_INTERFACE_H +#define NEBULA_CONTINENTAL_SRR520_HW_INTERFACE_H +// Have to define macros to silence warnings about deprecated headers being used by +// boost/property_tree/ in some versions of boost. +// See: https://github.com/boostorg/property_tree/issues/51 +#include +#if (BOOST_VERSION / 100 >= 1073 && BOOST_VERSION / 100 <= 1076) // Boost 1.73 - 1.76 +#define BOOST_BIND_GLOBAL_PLACEHOLDERS +#endif +#if (BOOST_VERSION / 100 == 1074) // Boost 1.74 +#define BOOST_ALLOW_DEPRECATED_HEADERS +#endif +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +namespace continental_srr520 +{ +/// @brief Hardware interface of the Continental SRR520 radar +class ContinentalSrr520HwInterface +{ +private: + // std::unique_ptr<::drivers::common::IoContext> sensor_io_context_; + // std::unique_ptr<::drivers::udp_driver::UdpDriver> sensor_udp_driver_; + + std::unique_ptr<::drivers::socketcan::SocketCanReceiver> can_receiver_; + std::unique_ptr<::drivers::socketcan::SocketCanSender> can_sender_; + std::unique_ptr receiver_thread_; + + std::shared_ptr sensor_configuration_; + std::unique_ptr nebula_packets_ptr_; + std::function buffer)> + nebula_packet_callback_; + + std::mutex sensor_status_mutex_; + std::mutex receiver_mutex_; + bool sensor_interface_active_{}; + + uint8_t sync_counter_{0}; + bool sync_fup_sent_{true}; + builtin_interfaces::msg::Time last_sync_stamp_; + + std::shared_ptr parent_node_logger; + + /// @brief Send a Fd frame + /// @param data a buffer containing the data to send + template + bool SendFrame(const std::array & data, int can_frame_id); + + /// @brief Printing the string to RCLCPP_INFO_STREAM + /// @param info Target string + void PrintInfo(std::string info); + + /// @brief Printing the string to RCLCPP_ERROR_STREAM + /// @param error Target string + void PrintError(std::string error); + + /// @brief Printing the string to RCLCPP_DEBUG_STREAM + /// @param debug Target string + void PrintDebug(std::string debug); + + /// @brief Printing the bytes to RCLCPP_DEBUG_STREAM + /// @param bytes Target byte vector + void PrintDebug(const std::vector & bytes); + + /// @brief Main loop of the CAN receiver thread + void ReceiveLoop(); + + /// @brief Process a new filter status packet + /// @param buffer The buffer containing the status packet + void ProcessFilterStatusPacket(const std::vector & buffer); + + /// @brief Process a new data packet + /// @param buffer The buffer containing the data packet + void ProcessDataPacket(const std::vector & buffer); + + /// @brief Callback function to receive the Cloud Packet data from the UDP Driver + /// @param buffer Buffer containing the data received from the UDP socket + void ReceiveSensorPacketCallback(const std::vector & buffer, int id, uint64_t stamp); + +public: + /// @brief Constructor + ContinentalSrr520HwInterface(); + + /// @brief Starting the interface that handles UDP streams + /// @return Resulting status + Status SensorInterfaceStart(); + + /// @brief Function for stopping the interface that handles UDP streams + /// @return Resulting status + Status SensorInterfaceStop(); + + /// @brief Setting sensor configuration + /// @param sensor_configuration SensorConfiguration for this interface + /// @return Resulting status + Status SetSensorConfiguration( + std::shared_ptr< + const nebula::drivers::continental_srr520::ContinentalSrr520SensorConfiguration>); + + /// @brief Registering callback for PandarScan + /// @param scan_callback Callback function + /// @return Resulting status + Status RegisterPacketCallback( + std::function)> packet_callback); + + /// @brief Sensor synchronization routine + void SensorSync(); + + /// @brief Process a new Sync Fup request + void SensorSyncFup(); + + /// @brief Configure the sensor + /// @param sensor_id Desired sensor id + /// @param longitudinal_autosar Desired longitudinal value in autosar coordinates + /// @param lateral_autosar Desired lateral value in autosar coordinates + /// @param vertical_autosar Desired vertical value in autosar coordinates + /// @param yaw_autosar Desired yaw value in autosar coordinates + /// @param longitudinal_cog Desired longitudinal cog + /// @param wheelbase Desired wheelbase + /// @param cover_damping Desired cover damping + /// @param plug_bottom Desired plug bottom + /// @param reset Rest the sensor to its default values + /// @return Resulting status + Status ConfigureSensor( + uint8_t sensor_id, float longitudinal_autosar, float lateral_autosar, float vertical_autosar, + float yaw_autosar, float longitudinal_cog, float wheelbase, float cover_damping, + bool plug_bottom, bool reset); + + /// @brief Set the current vehicle dynamics + /// @param longitudinal_acceleration Longitudinal acceleration + /// @param lateral_acceleration Lateral acceleration + /// @param yaw_rate Yaw rate + /// @param longitudinal_velocity Longitudinal velocity + /// @param driving_direction Driving direction + /// @return Resulting status + Status SetVehicleDynamics( + float longitudinal_acceleration, float lateral_acceleration, float yaw_rate, + float longitudinal_velocity, bool standstill); + + /// @brief Checking the current settings and changing the difference point + /// @return Resulting status + Status CheckAndSetConfig(); + + /// @brief Setting rclcpp::Logger + /// @param node Logger + void SetLogger(std::shared_ptr node); +}; +} // namespace continental_srr520 +} // namespace drivers +} // namespace nebula + +#endif // NEBULA_CONTINENTAL_SRR520_HW_INTERFACE_H diff --git a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp index 151b06066..f4932e0eb 100644 --- a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp @@ -163,15 +163,6 @@ Status ContinentalArs548HwInterface::SensorInterfaceStop() return Status::ERROR_1; } -Status ContinentalArs548HwInterface::GetSensorConfiguration( - SensorConfigurationBase & sensor_configuration) -{ - std::stringstream ss; - ss << sensor_configuration; - PrintDebug(ss.str()); - return Status::ERROR_1; -} - Status ContinentalArs548HwInterface::SetSensorMounting( float longitudinal_autosar, float lateral_autosar, float vertical_autosar, float yaw_autosar, float pitch_autosar, uint8_t plug_orientation) diff --git a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp new file mode 100644 index 000000000..ea364a20d --- /dev/null +++ b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp @@ -0,0 +1,435 @@ +// 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. +// You may obtain a copy of the License at +// + +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include +#include +namespace nebula +{ +namespace drivers +{ +namespace continental_srr520 +{ +ContinentalSrr520HwInterface::ContinentalSrr520HwInterface() +: nebula_packets_ptr_{std::make_unique()} +{ +} + +Status ContinentalSrr520HwInterface::SetSensorConfiguration( + std::shared_ptr) +{ + sensor_configuration_ = sensor_configuration_; + + return Status::OK; +} + +Status ContinentalSrr520HwInterface::SensorInterfaceStart() +{ + std::lock_guard lock(receiver_mutex_); + + try { + can_sender_ = std::make_unique<::drivers::socketcan::SocketCanSender>( + sensor_configuration_->interface, true); + can_receiver_ = std::make_unique<::drivers::socketcan::SocketCanReceiver>( + sensor_configuration_->interface, true); + + can_receiver_->SetCanFilters( + ::drivers::socketcan::SocketCanReceiver::CanFilterList(sensor_configuration_->filters)); + PrintInfo(std::string("applied filters: ") + sensor_configuration_->filters); + + sensor_interface_active_ = true; + receiver_thread_ = + std::make_unique(&ContinentalSrr520HwInterface::ReceiveLoop, this); + } catch (const std::exception & ex) { + Status status = Status::CAN_CONNECTION_ERROR; + std::cerr << status << sensor_configuration_->interface << std::endl; + return status; + } + return Status::OK; +} + +template +bool ContinentalSrr520HwInterface::SendFrame(const std::array & data, int can_frame_id) +{ + ::drivers::socketcan::CanId send_id( + can_frame_id, 0, ::drivers::socketcan::FrameType::DATA, ::drivers::socketcan::StandardFrame); + + try { + can_sender_->send_fd( + data.data(), data.size(), send_id, + std::chrono::duration_cast( + std::chrono::duration(sensor_configuration_->sender_timeout_sec))); + return true; + } catch (const std::exception & ex) { + PrintError(std::string("Error sending CAN message: ") + ex.what()); + return false; + } +} + +void ContinentalSrr520HwInterface::ReceiveLoop() +{ + ::drivers::socketcan::CanId receive_id{}; + std::chrono::nanoseconds receiver_timeout_nsec; + bool use_bus_time; + + while (true) { + auto packet_msg_ptr = std::make_unique(); + + { + std::lock_guard lock(receiver_mutex_); + receiver_timeout_nsec = std::chrono::duration_cast( + std::chrono::duration(sensor_configuration_->receiver_timeout_sec)); + use_bus_time = sensor_configuration_->use_bus_time; + + if (!sensor_interface_active_) { + break; + } + } + + try { + packet_msg_ptr->data.resize(68); // 64 bytes of data + 4 bytes of ID + receive_id = can_receiver_->receive_fd( + packet_msg_ptr->data.data() + 4 * sizeof(uint8_t), receiver_timeout_nsec); + } catch (const std::exception & ex) { + PrintError(std::string("Error receiving CAN FD message: ") + ex.what()); + continue; + } + + packet_msg_ptr->data.resize(receive_id.length() + 4); + + uint32_t id = receive_id.identifier(); + packet_msg_ptr->data[0] = (id & 0xFF000000) >> 24; + packet_msg_ptr->data[1] = (id & 0x00FF0000) >> 16; + packet_msg_ptr->data[2] = (id & 0x0000FF00) >> 8; + packet_msg_ptr->data[3] = (id & 0x000000FF) >> 0; + + int64_t stamp = use_bus_time + ? static_cast(receive_id.get_bus_time() * 1000U) + : static_cast(std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()) + .count()); + + packet_msg_ptr->stamp.sec = stamp / 1'000'000'000; + packet_msg_ptr->stamp.nanosec = stamp % 1'000'000'000; + + if (receive_id.frame_type() == ::drivers::socketcan::FrameType::ERROR) { + PrintError("CAN FD message is an error frame"); + continue; + } + + nebula_packet_callback_(std::move(packet_msg_ptr)); + + // ReceiveSensorPacketCallback(buffer, receive_id.identifier(), stamp); + } +} + +Status ContinentalSrr520HwInterface::RegisterPacketCallback( + std::function)> callback) +{ + nebula_packet_callback_ = std::move(callback); + return Status::OK; +} + +void ContinentalSrr520HwInterface::SensorSyncFup() +{ + if (!can_sender_) { + PrintError("Can sender is invalid so can not do follow up"); + } + + if (!sensor_configuration_->sync_use_bus_time || sync_fup_sent_) { + return; + } + + auto t0s = last_sync_stamp_; + t0s.nanosec = 0; + const auto t1r = stamp; + + builtin_interfaces::msg::Time t4r = + rclcpp::Time(rclcpp::Time() + (rclcpp::Time(t1r) - rclcpp::Time(t0s))); + uint8_t t4r_seconds = static_cast(t4r.sec); + uint32_t t4r_nanoseconds = t4r.nanosec; + std::array data; + data[0] = 0x28; // mode 0x18 is without CRC + data[2] = (((static_cast(TIME_DOMAIN_ID) << 4) & 0xF0)) | + (sync_counter_ & 0x0F); // Domain and counter + data[3] = t4r_seconds & 0x3; // SGW and OVS + data[4] = (t4r_nanoseconds & 0xFF000000) >> 24; + data[5] = (t4r_nanoseconds & 0x00FF0000) >> 16; + data[6] = (t4r_nanoseconds & 0x0000FF00) >> 8; + data[7] = (t4r_nanoseconds & 0x000000FF) >> 0; + + std::array fup_crc_array{data[2], data[3], data[4], data[5], data[6], data[7], 0x00}; + uint8_t fup_crc = crc8h2f(fup_crc_array.begin(), fup_crc_array.end()); + data[1] = fup_crc; + + SendFrame(data, SYNC_FUP_CAN_MESSAGE_ID); + + sync_fup_sent_ = true; + sync_counter_ = sync_counter_ == 15 ? 0 : sync_counter_ + 1; +} + +void ContinentalSrr520HwInterface::SensorSync() +{ + if (!can_sender_) { + PrintError("Can sender is invalid so can not do sync up"); + } + + if (!sync_fup_sent_) { + PrintError("We will send a SYNC message without having sent a FUP message first!"); + } + + auto now = std::chrono::system_clock::now(); + auto now_secs = std::chrono::duration_cast(now.time_since_epoch()).count(); + auto now_nanosecs = + std::chrono::duration_cast(now.time_since_epoch()).count(); + + builtin_interfaces::msg::Time stamp; + stamp.sec = static_cast(now_secs); + stamp.nanosec = static_cast(now_nanosecs % 1'000'000'000); + last_sync_stamp_ = stamp; + + std::array data; + data[0] = 0x20; // mode 0x10 is without CRC + data[2] = (((static_cast(TIME_DOMAIN_ID) << 4) & 0xF0)) | + (sync_counter_ & 0x0F); // Domain and counter + data[3] = 0; // use data + data[4] = (stamp.sec & 0xFF000000) >> 24; + data[5] = (stamp.sec & 0x00FF0000) >> 16; + data[6] = (stamp.sec & 0x0000FF00) >> 8; + data[7] = (stamp.sec & 0x000000FF) >> 0; + + std::array sync_crc_array{data[2], data[3], data[4], data[5], data[6], data[7], 0x00}; + uint8_t sync_crc = crc8h2f(sync_crc_array.begin(), sync_crc_array.end()); + data[1] = sync_crc; + + SendFrame(data, SYNC_FUP_CAN_MESSAGE_ID); + + if (sensor_configuration_->sync_use_bus_time) { + sync_fup_sent_ = false; + return; + } + + data[0] = 0x28; // mode 0x18 is without CRC + data[2] = (((static_cast(TIME_DOMAIN_ID) << 4) & 0xF0)) | + (sync_counter_ & 0x0F); // Domain and counter + data[3] = 0; // SGW and OVS + data[4] = (stamp.nanosec & 0xFF000000) >> 24; + data[5] = (stamp.nanosec & 0x00FF0000) >> 16; + data[6] = (stamp.nanosec & 0x0000FF00) >> 8; + data[7] = (stamp.nanosec & 0x000000FF) >> 0; + + std::array fup_crc_array{data[2], data[3], data[4], data[5], data[6], data[7], 0x00}; + uint8_t fup_crc = crc8h2f(fup_crc_array.begin(), fup_crc_array.end()); + data[1] = fup_crc; + + SendFrame(data, SYNC_FUP_CAN_MESSAGE_ID); + + sync_counter_ = sync_counter_ == 15 ? 0 : sync_counter_ + 1; + sync_fup_sent_ = true; +} + +void ContinentalSrr520HwInterface::ProcessDataPacket(const std::vector & buffer) +{ + nebula_msgs::msg::NebulaPacket nebula_packet; + nebula_packet.data = buffer; + auto now = std::chrono::system_clock::now(); + auto now_secs = std::chrono::duration_cast(now.time_since_epoch()).count(); + auto now_nanosecs = + std::chrono::duration_cast(now.time_since_epoch()).count(); + nebula_packet.stamp.sec = static_cast(now_secs); + nebula_packet.stamp.nanosec = + static_cast((now_nanosecs / 1000000000.0 - static_cast(now_secs)) * 1000000000); + nebula_packets_ptr_->packets.emplace_back(nebula_packet); + + nebula_packets_ptr_->header.stamp = nebula_packets_ptr_->packets.front().stamp; + + { + std::lock_guard lock(receiver_mutex_); + nebula_packets_ptr_->header.frame_id = sensor_configuration_->frame_id; + } + + nebula_packets_reception_callback_(std::move(nebula_packets_ptr_)); + nebula_packets_ptr_ = std::make_unique(); +} + +Status ContinentalSrr520HwInterface::SensorInterfaceStop() +{ + { + std::lock_guard l(receiver_mutex_); + sensor_interface_active_ = true; + } + + receiver_thread_->join(); + return Status::ERROR_1; +} + +Status ContinentalSrr520HwInterface::ConfigureSensor( + uint8_t sensor_id, float longitudinal_autosar, float lateral_autosar, float vertical_autosar, + float yaw_autosar, float longitudinal_cog, float wheelbase, float cover_damping, bool plug_bottom, + bool reset) +{ + std::cout << "longitudinal_autosar=" << longitudinal_autosar << std::endl; + std::cout << "lateral_autosar=" << lateral_autosar << std::endl; + std::cout << "vertical_autosar=" << vertical_autosar << std::endl; + std::cout << "longitudinal_cog=" << longitudinal_cog << std::endl; + std::cout << "wheelbase=" << wheelbase << std::endl; + std::cout << "yaw_autosar=" << yaw_autosar << std::endl; + std::cout << "sensor_id=" << static_cast(sensor_id) << std::endl << std::flush; + std::cout << "plug_bottom=" << plug_bottom << std::endl; + + if ( + longitudinal_autosar < -32.767f || longitudinal_autosar > 32.767f || + lateral_autosar < -32.767f || lateral_autosar > 32.767f || vertical_autosar < -32.767f || + vertical_autosar > 32.767f || longitudinal_cog < -32.767f || longitudinal_cog > 32.767f || + wheelbase < 0.f || wheelbase > 65.534f || yaw_autosar < -3.14159f || yaw_autosar > 3.14159f || + cover_damping < -32.767f || cover_damping > 32.767f) { + PrintError("Sensor configuration values out of range!"); + return Status::SENSOR_CONFIG_ERROR; + } + + const uint16_t u_long_pos = static_cast((longitudinal_autosar + 32.767f) / 0.001f); + const uint16_t u_lat_pos = static_cast((lateral_autosar + 32.767f) / 0.001f); + const uint16_t u_vert_pos = static_cast((vertical_autosar + 32.767f) / 0.001f); + const uint16_t u_long_pos_cog = static_cast((longitudinal_cog + 32.767f) / 0.001f); + const uint16_t u_wheelbase = static_cast(wheelbase / 0.001f); + const uint16_t u_yaw_angle = static_cast((yaw_autosar + 3.14159f) / 9.5877e-05); + const uint16_t u_cover_damping = static_cast((cover_damping + 32.767f) / 0.001f); + + std::array data; + data[0] = sensor_id; + data[1] = static_cast((u_long_pos & 0xff00) >> 8); + data[2] = static_cast((u_long_pos & 0x00ff)); + + data[3] = static_cast((u_lat_pos & 0xff00) >> 8); + data[4] = static_cast((u_lat_pos & 0x00ff)); + + data[5] = static_cast((u_vert_pos & 0xff00) >> 8); + data[6] = static_cast((u_vert_pos & 0x00ff)); + + data[7] = static_cast((u_long_pos_cog & 0xff00) >> 8); + data[8] = static_cast((u_long_pos_cog & 0x00ff)); + + data[9] = static_cast((u_wheelbase & 0xff00) >> 8); + data[10] = static_cast((u_wheelbase & 0x00ff)); + + data[11] = static_cast((u_yaw_angle & 0xff00) >> 8); + data[12] = static_cast((u_yaw_angle & 0x00ff)); + + data[13] = static_cast((u_cover_damping & 0xff00) >> 8); + data[14] = static_cast((u_cover_damping & 0x00ff)); + + uint8_t plug_value = plug_bottom ? 0x00 : 0x01; + uint8_t reset_value = reset ? 0x80 : 0x00; + data[15] = plug_value | reset_value; + + if (SendFrame(data, SENSOR_CONFIG_CAN_MESSAGE_ID)) { + return Status::OK; + } else { + return Status::CAN_CONNECTION_ERROR; + } +} + +Status ContinentalSrr520HwInterface::SetVehicleDynamics( + float longitudinal_acceleration, float lateral_acceleration, float yaw_rate, + float longitudinal_velocity, bool standstill) +{ + if ( + longitudinal_acceleration < -12.7 || longitudinal_acceleration > 12.7 || + lateral_acceleration < -12.7 || lateral_acceleration > 12.7 || yaw_rate < -3.14159 || + yaw_rate > 3.14159 || abs(longitudinal_velocity) > 100.0) { + PrintError("Vehicle dynamics out of range!"); + return Status::SENSOR_CONFIG_ERROR; + } + + const uint8_t u_long_accel = static_cast((longitudinal_acceleration + 12.7) / 0.1); + const uint8_t u_lat_accel = static_cast((lateral_acceleration + 12.7) / 0.1); + const uint16_t u_yaw_rate = static_cast((yaw_rate + 3.14159) / 0.001534729); + const uint16_t u_long_vel = static_cast(std::abs(longitudinal_velocity) / 0.024425989); + uint8_t u_long_dir; + + if (standstill) { + u_long_dir = 0; + } else if (longitudinal_velocity > 0) { + u_long_dir = 1; + } else { + u_long_dir = 2; + } + + std::array data; + data[0] = u_long_accel; + data[1] = u_lat_accel; + data[2] = static_cast((u_yaw_rate & 0xff0) >> 4); + data[3] = (static_cast(u_yaw_rate & 0x0f) << 4) | + static_cast((u_long_vel & 0xf00) >> 8); + data[4] = static_cast(u_long_vel & 0xff); + data[5] = u_long_dir; + data[6] = 0x00; + data[7] = 0x00; + + if (SendFrame(data, VEH_DYN_CAN_MESSAGE_ID)) { + return Status::OK; + } else { + return Status::CAN_CONNECTION_ERROR; + } +} + +void ContinentalSrr520HwInterface::SetLogger(std::shared_ptr logger) +{ + parent_node_logger = logger; +} + +void ContinentalSrr520HwInterface::PrintInfo(std::string info) +{ + if (parent_node_logger) { + RCLCPP_INFO_STREAM((*parent_node_logger), info); + } else { + std::cout << info << std::endl; + } +} + +void ContinentalSrr520HwInterface::PrintError(std::string error) +{ + if (parent_node_logger) { + RCLCPP_ERROR_STREAM((*parent_node_logger), error); + } else { + std::cerr << error << std::endl; + } +} + +void ContinentalSrr520HwInterface::PrintDebug(std::string debug) +{ + if (parent_node_logger) { + RCLCPP_DEBUG_STREAM((*parent_node_logger), debug); + } else { + std::cout << debug << std::endl; + } +} + +void ContinentalSrr520HwInterface::PrintDebug(const std::vector & bytes) +{ + std::stringstream ss; + for (const auto & b : bytes) { + ss << static_cast(b) << ", "; + } + ss << std::endl; + PrintDebug(ss.str()); +} + +} // namespace continental_srr520 +} // namespace drivers +} // namespace nebula diff --git a/nebula_ros/continental_ars548_decoder_ros_wrapper.cpp b/nebula_ros/continental_ars548_decoder_ros_wrapper.cpp new file mode 100644 index 000000000..d45d256de --- /dev/null +++ b/nebula_ros/continental_ars548_decoder_ros_wrapper.cpp @@ -0,0 +1,802 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +namespace nebula +{ +namespace ros +{ +ContinentalArs548DriverRosWrapper::ContinentalArs548DriverRosWrapper( + const rclcpp::NodeOptions & options) +: rclcpp::Node("continental_ars548_driver_ros_wrapper", options), hw_interface_() +{ + drivers::continental_ars548::ContinentalArs548SensorConfiguration sensor_configuration; + + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + + hw_interface_.SetLogger(std::make_shared(this->get_logger())); + + wrapper_status_ = GetParameters(sensor_configuration); + if (Status::OK != wrapper_status_) { + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); + return; + } + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); + + sensor_cfg_ptr_ = + std::make_shared( + sensor_configuration); + + wrapper_status_ = InitializeDriver( + std::const_pointer_cast( + sensor_cfg_ptr_)); + + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); + packets_sub_ = create_subscription( + "nebula_packets", rclcpp::SensorDataQoS(), + std::bind( + &ContinentalArs548DriverRosWrapper::ReceivePacketsMsgCallback, this, std::placeholders::_1)); + + detection_list_pub_ = + this->create_publisher( + "continental_detections", rclcpp::SensorDataQoS()); + object_list_pub_ = this->create_publisher( + "continental_objects", rclcpp::SensorDataQoS()); + + detection_pointcloud_pub_ = this->create_publisher( + "detection_points", rclcpp::SensorDataQoS()); + object_pointcloud_pub_ = + this->create_publisher("object_points", rclcpp::SensorDataQoS()); + + scan_raw_pub_ = + this->create_publisher("scan_raw", rclcpp::SensorDataQoS()); + + objects_raw_pub_ = + this->create_publisher("objects_raw", rclcpp::SensorDataQoS()); + + objects_markers_pub_ = + this->create_publisher("marker_array", 10); + + diagnostics_pub_ = + this->create_publisher("diagnostics", 10); +} + +void ContinentalArs548DriverRosWrapper::ReceivePacketsMsgCallback( + const nebula_msgs::msg::NebulaPackets::SharedPtr scan_msg) +{ + decoder_ptr_->ProcessPackets(*scan_msg); +} + +Status ContinentalArs548DriverRosWrapper::InitializeDriver( + std::shared_ptr sensor_configuration) +{ + decoder_ptr_ = std::make_shared( + std::static_pointer_cast( + sensor_configuration)); + + decoder_ptr_->RegisterDetectionListCallback(std::bind( + &ContinentalArs548DriverRosWrapper::DetectionListCallback, this, std::placeholders::_1)); + decoder_ptr_->RegisterObjectListCallback( + std::bind(&ContinentalArs548DriverRosWrapper::ObjectListCallback, this, std::placeholders::_1)); + decoder_ptr_->RegisterSensorStatusCallback(std::bind( + &ContinentalArs548DriverRosWrapper::SensorStatusCallback, this, std::placeholders::_1)); + + return Status::OK; +} + +Status ContinentalArs548DriverRosWrapper::GetStatus() +{ + return wrapper_status_; +} + +Status ContinentalArs548DriverRosWrapper::GetParameters( + drivers::continental_ars548::ContinentalArs548SensorConfiguration & sensor_configuration) +{ + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("host_ip", descriptor); + sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("sensor_ip", descriptor); + sensor_configuration.sensor_ip = this->get_parameter("sensor_ip").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("data_port", descriptor); + sensor_configuration.data_port = this->get_parameter("data_port").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("sensor_model"); + sensor_configuration.sensor_model = + nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("frame_id", descriptor); + sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("base_frame", descriptor); + sensor_configuration.base_frame = this->get_parameter("base_frame").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("object_frame", descriptor); + sensor_configuration.object_frame = this->get_parameter("object_frame").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("use_sensor_time", descriptor); + sensor_configuration.use_sensor_time = this->get_parameter("use_sensor_time").as_bool(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_vehicle_length", descriptor); + sensor_configuration.configuration_vehicle_length = + static_cast(this->get_parameter("configuration_vehicle_length").as_double()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_vehicle_width", descriptor); + sensor_configuration.configuration_vehicle_width = + static_cast(this->get_parameter("configuration_vehicle_width").as_double()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_vehicle_height", descriptor); + sensor_configuration.configuration_vehicle_height = + static_cast(this->get_parameter("configuration_vehicle_height").as_double()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_vehicle_wheelbase", descriptor); + sensor_configuration.configuration_vehicle_wheelbase = + static_cast(this->get_parameter("configuration_vehicle_wheelbase").as_double()); + } + + if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + + std::shared_ptr sensor_cfg_ptr = + std::make_shared( + sensor_configuration); + + hw_interface_.SetSensorConfiguration( + std::static_pointer_cast(sensor_cfg_ptr)); + + RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); + return Status::OK; +} + +void ContinentalArs548DriverRosWrapper::DetectionListCallback( + std::unique_ptr msg) +{ + if ( + detection_pointcloud_pub_->get_subscription_count() > 0 || + detection_pointcloud_pub_->get_intra_process_subscription_count() > 0) { + const auto detection_pointcloud_ptr = ConvertToPointcloud(*msg); + auto detection_pointcloud_msg_ptr = std::make_unique(); + pcl::toROSMsg(*detection_pointcloud_ptr, *detection_pointcloud_msg_ptr); + + detection_pointcloud_msg_ptr->header = msg->header; + detection_pointcloud_pub_->publish(std::move(detection_pointcloud_msg_ptr)); + } + + if ( + scan_raw_pub_->get_subscription_count() > 0 || + scan_raw_pub_->get_intra_process_subscription_count() > 0) { + auto radar_scan_msg = ConvertToRadarScan(*msg); + radar_scan_msg.header = msg->header; + scan_raw_pub_->publish(std::move(radar_scan_msg)); + } + + if ( + detection_list_pub_->get_subscription_count() > 0 || + detection_list_pub_->get_intra_process_subscription_count() > 0) { + detection_list_pub_->publish(std::move(msg)); + } +} + +void ContinentalArs548DriverRosWrapper::ObjectListCallback( + std::unique_ptr msg) +{ + if ( + object_pointcloud_pub_->get_subscription_count() > 0 || + object_pointcloud_pub_->get_intra_process_subscription_count() > 0) { + const auto object_pointcloud_ptr = ConvertToPointcloud(*msg); + auto object_pointcloud_msg_ptr = std::make_unique(); + pcl::toROSMsg(*object_pointcloud_ptr, *object_pointcloud_msg_ptr); + + object_pointcloud_msg_ptr->header = msg->header; + object_pointcloud_pub_->publish(std::move(object_pointcloud_msg_ptr)); + } + + if ( + objects_raw_pub_->get_subscription_count() > 0 || + objects_raw_pub_->get_intra_process_subscription_count() > 0) { + auto objects_raw_msg = ConvertToRadarTracks(*msg); + objects_raw_msg.header = msg->header; + objects_raw_pub_->publish(std::move(objects_raw_msg)); + } + + if ( + objects_markers_pub_->get_subscription_count() > 0 || + objects_markers_pub_->get_intra_process_subscription_count() > 0) { + auto marker_array_msg = ConvertToMarkers(*msg); + objects_markers_pub_->publish(std::move(marker_array_msg)); + } + + if ( + object_list_pub_->get_subscription_count() > 0 || + object_list_pub_->get_intra_process_subscription_count() > 0) { + object_list_pub_->publish(std::move(msg)); + } +} + +void ContinentalArs548DriverRosWrapper::SensorStatusCallback( + const drivers::continental_ars548::ContinentalArs548Status & sensor_status) +{ + diagnostic_msgs::msg::DiagnosticArray diagnostic_array_msg; + diagnostic_array_msg.header.stamp.sec = sensor_status.timestamp_seconds; + diagnostic_array_msg.header.stamp.nanosec = sensor_status.timestamp_nanoseconds; + diagnostic_array_msg.header.frame_id = sensor_cfg_ptr_->frame_id; + + diagnostic_array_msg.status.resize(1); + auto & status = diagnostic_array_msg.status[0]; + status.values.reserve(36); + status.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + status.hardware_id = sensor_cfg_ptr_->frame_id; + status.name = sensor_cfg_ptr_->frame_id; + status.message = "Diagnostic messages from ARS548"; + + auto add_diagnostic = [&status](const std::string & key, const std::string & value) { + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = key; + key_value.value = value; + status.values.push_back(key_value); + }; + + add_diagnostic("timestamp_nanoseconds", std::to_string(sensor_status.timestamp_nanoseconds)); + add_diagnostic("timestamp_seconds", std::to_string(sensor_status.timestamp_seconds)); + add_diagnostic("timestamp_sync_status", sensor_status.timestamp_sync_status); + add_diagnostic("sw_version_major", std::to_string(sensor_status.sw_version_major)); + add_diagnostic("sw_version_minor", std::to_string(sensor_status.sw_version_minor)); + add_diagnostic("sw_version_patch", std::to_string(sensor_status.sw_version_patch)); + add_diagnostic("longitudinal", std::to_string(sensor_status.longitudinal)); + add_diagnostic("lateral", std::to_string(sensor_status.lateral)); + add_diagnostic("vertical", std::to_string(sensor_status.vertical)); + add_diagnostic("yaw", std::to_string(sensor_status.yaw)); + add_diagnostic("pitch", std::to_string(sensor_status.pitch)); + add_diagnostic("plug_orientation", sensor_status.plug_orientation); + add_diagnostic("length", std::to_string(sensor_status.length)); + add_diagnostic("width", std::to_string(sensor_status.width)); + add_diagnostic("height", std::to_string(sensor_status.height)); + add_diagnostic("wheel_base", std::to_string(sensor_status.wheel_base)); + add_diagnostic("max_distance", std::to_string(sensor_status.max_distance)); + add_diagnostic("frequency_slot", sensor_status.frequency_slot); + add_diagnostic("cycle_time", std::to_string(sensor_status.cycle_time)); + add_diagnostic("time_slot", std::to_string(sensor_status.time_slot)); + add_diagnostic("hcc", sensor_status.hcc); + add_diagnostic("power_save_standstill", sensor_status.power_save_standstill); + add_diagnostic("sensor_ip_address0", sensor_status.sensor_ip_address0); + add_diagnostic("sensor_ip_address1", sensor_status.sensor_ip_address1); + add_diagnostic("configuration_counter", std::to_string(sensor_status.configuration_counter)); + add_diagnostic("longitudinal_velocity_status", sensor_status.longitudinal_velocity_status); + add_diagnostic( + "longitudinal_acceleration_status", sensor_status.longitudinal_acceleration_status); + add_diagnostic("lateral_acceleration_status", sensor_status.lateral_acceleration_status); + add_diagnostic("yaw_rate_status", sensor_status.yaw_rate_status); + add_diagnostic("steering_angle_status", sensor_status.steering_angle_status); + add_diagnostic("driving_direction_status", sensor_status.driving_direction_status); + add_diagnostic("characteristic_speed_status", sensor_status.characteristic_speed_status); + add_diagnostic("radar_status", sensor_status.radar_status); + add_diagnostic("voltage_status", sensor_status.voltage_status); + add_diagnostic("temperature_status", sensor_status.temperature_status); + add_diagnostic("blockage_status", sensor_status.blockage_status); + + double detection_total_time_sec = + (sensor_status.detection_last_stamp - sensor_status.detection_first_stamp) * 1e-9; + uint64_t expected_total_detection = + static_cast(detection_total_time_sec / (sensor_status.cycle_time * 1e-3)); + uint64_t detection_count_diff = + expected_total_detection > sensor_status.detection_total_count + ? expected_total_detection - sensor_status.detection_total_count + : sensor_status.detection_total_count - expected_total_detection; + double detection_dropped_rate = + 100.0 * std::abs(detection_count_diff) / expected_total_detection; + double detection_dropped_rate_dt = + 100.0 * sensor_status.detection_dropped_dt_count / sensor_status.detection_total_count; + double detection_empty_rate = + 100.0 * sensor_status.detection_empty_count / sensor_status.detection_total_count; + + add_diagnostic("detection_total_time", std::to_string(detection_total_time_sec)); + add_diagnostic("detection_dropped_rate", std::to_string(detection_dropped_rate)); + add_diagnostic("detection_dropped_rate_dt", std::to_string(detection_dropped_rate_dt)); + add_diagnostic("detection_empty_rate", std::to_string(detection_empty_rate)); + add_diagnostic( + "detection_dropped_dt_count", std::to_string(sensor_status.detection_dropped_dt_count)); + add_diagnostic("detection_empty_count", std::to_string(sensor_status.detection_empty_count)); + + double object_total_time_sec = + (sensor_status.object_last_stamp - sensor_status.object_first_stamp) * 1e-9; + uint64_t expected_total_object = + static_cast(object_total_time_sec / (sensor_status.cycle_time * 1e-3)); + uint64_t object_count_diff = expected_total_object > sensor_status.object_total_count + ? expected_total_object - sensor_status.object_total_count + : sensor_status.object_total_count - expected_total_object; + double object_dropped_rate = 100.0 * std::abs(object_count_diff) / expected_total_object; + double object_dropped_rate_dt = + 100.0 * sensor_status.object_dropped_dt_count / sensor_status.object_total_count; + double object_empty_rate = + 100.0 * sensor_status.object_empty_count / sensor_status.object_total_count; + + add_diagnostic("sensor_status.expected_total_object", std::to_string(expected_total_object)); + add_diagnostic( + "sensor_status.detection_total_count", std::to_string(sensor_status.detection_total_count)); + + add_diagnostic("object_total_time", std::to_string(object_total_time_sec)); + add_diagnostic("object_dropped_rate", std::to_string(object_dropped_rate)); + add_diagnostic("object_dropped_rate_dt", std::to_string(object_dropped_rate_dt)); + add_diagnostic("object_empty_rate", std::to_string(object_empty_rate)); + add_diagnostic("object_dropped_dt_count", std::to_string(sensor_status.object_dropped_dt_count)); + add_diagnostic("object_empty_count", std::to_string(sensor_status.object_empty_count)); + + add_diagnostic("status_total_count", std::to_string(sensor_status.status_total_count)); + add_diagnostic("radar_invalid_count", std::to_string(sensor_status.radar_invalid_count)); + + diagnostics_pub_->publish(diagnostic_array_msg); +} + +pcl::PointCloud::Ptr +ContinentalArs548DriverRosWrapper::ConvertToPointcloud( + const continental_msgs::msg::ContinentalArs548DetectionList & msg) +{ + pcl::PointCloud::Ptr output_pointcloud( + new pcl::PointCloud); + output_pointcloud->reserve(msg.detections.size()); + + nebula::drivers::continental_ars548::PointArs548Detection point{}; + for (const auto & detection : msg.detections) { + point.x = + std::cos(detection.elevation_angle) * std::cos(detection.azimuth_angle) * detection.range; + point.y = + std::cos(detection.elevation_angle) * std::sin(detection.azimuth_angle) * detection.range; + point.z = std::sin(detection.elevation_angle) * detection.range; + + point.azimuth = detection.azimuth_angle; + point.azimuth_std = detection.azimuth_angle_std; + point.elevation = detection.elevation_angle; + point.elevation_std = detection.elevation_angle_std; + point.range = detection.range; + point.range_std = detection.range_std; + point.range_rate = detection.range_rate; + point.range_rate_std = detection.range_rate_std; + point.rcs = detection.rcs; + point.measurement_id = detection.measurement_id; + point.positive_predictive_value = detection.positive_predictive_value; + point.classification = detection.classification; + point.multi_target_probability = detection.multi_target_probability; + point.object_id = detection.object_id; + point.ambiguity_flag = detection.ambiguity_flag; + + output_pointcloud->points.emplace_back(point); + } + + output_pointcloud->height = 1; + output_pointcloud->width = output_pointcloud->points.size(); + return output_pointcloud; +} + +pcl::PointCloud::Ptr +ContinentalArs548DriverRosWrapper::ConvertToPointcloud( + const continental_msgs::msg::ContinentalArs548ObjectList & msg) +{ + pcl::PointCloud::Ptr output_pointcloud( + new pcl::PointCloud); + output_pointcloud->reserve(msg.objects.size()); + + nebula::drivers::continental_ars548::PointArs548Object point{}; + for (const auto & object : msg.objects) { + point.x = static_cast(object.position.x); + point.y = static_cast(object.position.y); + point.z = static_cast(object.position.z); + + point.id = object.object_id; + point.age = object.age; + point.status_measurement = object.status_measurement; + point.status_movement = object.status_movement; + point.position_reference = object.position_reference; + point.classification_car = object.classification_car; + point.classification_truck = object.classification_truck; + point.classification_motorcycle = object.classification_motorcycle; + point.classification_bicycle = object.classification_bicycle; + point.classification_pedestrian = object.classification_pedestrian; + point.dynamics_abs_vel_x = static_cast(object.absolute_velocity.x); + point.dynamics_abs_vel_y = static_cast(object.absolute_velocity.y); + point.dynamics_rel_vel_x = static_cast(object.relative_velocity.x); + point.dynamics_rel_vel_y = static_cast(object.relative_velocity.y); + point.shape_length_edge_mean = object.shape_length_edge_mean; + point.shape_width_edge_mean = object.shape_width_edge_mean; + point.dynamics_orientation_rate_mean = object.orientation_rate_mean; + + output_pointcloud->points.emplace_back(point); + } + + output_pointcloud->height = 1; + output_pointcloud->width = output_pointcloud->points.size(); + return output_pointcloud; +} + +radar_msgs::msg::RadarScan ContinentalArs548DriverRosWrapper::ConvertToRadarScan( + const continental_msgs::msg::ContinentalArs548DetectionList & msg) +{ + radar_msgs::msg::RadarScan output_msg; + output_msg.header = msg.header; + output_msg.returns.reserve(msg.detections.size()); + + radar_msgs::msg::RadarReturn return_msg; + for (const auto & detection : msg.detections) { + if ( + detection.invalid_azimuth || detection.invalid_distance || detection.invalid_elevation || + detection.invalid_range_rate) { + continue; + } + + return_msg.range = detection.range; + return_msg.azimuth = detection.azimuth_angle; + return_msg.elevation = detection.elevation_angle; + return_msg.doppler_velocity = detection.range_rate; + return_msg.amplitude = detection.rcs; + output_msg.returns.emplace_back(return_msg); + } + + return output_msg; +} + +radar_msgs::msg::RadarTracks ContinentalArs548DriverRosWrapper::ConvertToRadarTracks( + const continental_msgs::msg::ContinentalArs548ObjectList & msg) +{ + radar_msgs::msg::RadarTracks output_msg; + output_msg.tracks.reserve(msg.objects.size()); + output_msg.header = msg.header; + + constexpr int16_t UNKNOWN_ID = 32000; + constexpr int16_t CAR_ID = 32001; + constexpr int16_t TRUCK_ID = 32002; + constexpr int16_t MOTORCYCLE_ID = 32005; + constexpr int16_t BICYCLE_ID = 32006; + constexpr int16_t PEDESTRIAN_ID = 32007; + constexpr float INVALID_COVARIANCE = 1e6; + + radar_msgs::msg::RadarTrack track_msg; + for (const auto & object : msg.objects) { + track_msg.uuid.uuid[0] = static_cast(object.object_id & 0xff); + track_msg.uuid.uuid[1] = static_cast((object.object_id >> 8) & 0xff); + track_msg.uuid.uuid[2] = static_cast((object.object_id >> 16) & 0xff); + track_msg.uuid.uuid[3] = static_cast((object.object_id >> 24) & 0xff); + + const double half_length = 0.5 * object.shape_length_edge_mean; + const double half_width = 0.5 * object.shape_width_edge_mean; + // There are 9 possible reference points. In the case of an invalid reference point, we fall + // back to the center + const int reference_index = std::min(object.position_reference, 8); + const double & yaw = object.orientation; + track_msg.position.x = object.position.x + + std::cos(yaw) * half_length * reference_to_center_[reference_index][0] - + std::sin(yaw) * half_width * reference_to_center_[reference_index][1]; + track_msg.position.y = object.position.y + + std::sin(yaw) * half_length * reference_to_center_[reference_index][0] + + std::cos(yaw) * half_width * reference_to_center_[reference_index][1]; + track_msg.position.z = object.position.z; + + track_msg.velocity = object.absolute_velocity; + track_msg.acceleration = object.absolute_acceleration; + track_msg.size.x = object.shape_length_edge_mean; + track_msg.size.y = object.shape_width_edge_mean; + track_msg.size.z = 1.f; + + uint8_t max_score = object.classification_unknown; + track_msg.classification = UNKNOWN_ID; + + if (object.classification_car > max_score) { + max_score = object.classification_car; + track_msg.classification = CAR_ID; + } + if (object.classification_truck > max_score) { + max_score = object.classification_truck; + track_msg.classification = TRUCK_ID; + } + if (object.classification_motorcycle > max_score) { + max_score = object.classification_motorcycle; + track_msg.classification = MOTORCYCLE_ID; + } + if (object.classification_bicycle > max_score) { + max_score = object.classification_bicycle; + track_msg.classification = BICYCLE_ID; + } + if (object.classification_pedestrian > max_score) { + max_score = object.classification_pedestrian; + track_msg.classification = PEDESTRIAN_ID; + } + + track_msg.position_covariance[0] = + static_cast(object.position_std.x * object.position_std.x); + track_msg.position_covariance[1] = object.position_covariance_xy; + track_msg.position_covariance[2] = 0.f; + track_msg.position_covariance[3] = + static_cast(object.position_std.y * object.position_std.y); + track_msg.position_covariance[4] = 0.f; + track_msg.position_covariance[5] = + static_cast(object.position_std.z * object.position_std.z); + + track_msg.velocity_covariance[0] = + static_cast(object.absolute_velocity_std.x * object.absolute_velocity_std.x); + track_msg.velocity_covariance[1] = object.absolute_velocity_covariance_xy; + track_msg.velocity_covariance[2] = 0.f; + track_msg.velocity_covariance[3] = + static_cast(object.absolute_velocity_std.y * object.absolute_velocity_std.y); + track_msg.velocity_covariance[4] = 0.f; + track_msg.velocity_covariance[5] = + static_cast(object.absolute_velocity_std.z * object.absolute_velocity_std.z); + + track_msg.acceleration_covariance[0] = + static_cast(object.absolute_acceleration_std.x * object.absolute_acceleration_std.x); + track_msg.acceleration_covariance[1] = object.absolute_acceleration_covariance_xy; + track_msg.acceleration_covariance[2] = 0.f; + track_msg.acceleration_covariance[3] = + static_cast(object.absolute_acceleration_std.y * object.absolute_acceleration_std.y); + track_msg.acceleration_covariance[4] = 0.f; + track_msg.acceleration_covariance[5] = + static_cast(object.absolute_acceleration_std.z * object.absolute_acceleration_std.z); + + track_msg.size_covariance[0] = INVALID_COVARIANCE; + track_msg.size_covariance[1] = 0.f; + track_msg.size_covariance[2] = 0.f; + track_msg.size_covariance[3] = INVALID_COVARIANCE; + track_msg.size_covariance[4] = 0.f; + track_msg.size_covariance[5] = INVALID_COVARIANCE; + + output_msg.tracks.emplace_back(track_msg); + } + + return output_msg; +} + +visualization_msgs::msg::MarkerArray ContinentalArs548DriverRosWrapper::ConvertToMarkers( + const continental_msgs::msg::ContinentalArs548ObjectList & msg) +{ + visualization_msgs::msg::MarkerArray marker_array; + marker_array.markers.reserve(4 * msg.objects.size()); + + constexpr int LINE_STRIP_CORNERS_NUM = 17; + constexpr std::array, LINE_STRIP_CORNERS_NUM> cube_corners = { + {{{-1.0, -1.0, -1.0}}, + {{-1.0, -1.0, 1.0}}, + {{-1.0, 1.0, 1.0}}, + {{-1.0, 1.0, -1.0}}, + {{-1.0, -1.0, -1.0}}, + {{1.0, -1.0, -1.0}}, + {{1.0, -1.0, 1.0}}, + {{1.0, 1.0, 1.0}}, + {{1.0, 1.0, -1.0}}, + {{1.0, -1.0, -1.0}}, + {{-1.0, -1.0, -1.0}}, + {{-1.0, -1.0, 1.0}}, + {{1.0, -1.0, 1.0}}, + {{1.0, 1.0, 1.0}}, + {{-1.0, 1.0, 1.0}}, + {{-1.0, 1.0, -1.0}}, + {{1.0, 1.0, -1.0}}}}; + + constexpr int PALETTE_SIZE = 32; + constexpr std::array, PALETTE_SIZE> color_array = {{ + {{1.0, 0.0, 0.0}}, {{0.0, 1.0, 0.0}}, + {{0.0, 0.0, 1.0}}, // Red, Green, Blue + {{1.0, 1.0, 0.0}}, {{0.0, 1.0, 1.0}}, + {{1.0, 0.0, 1.0}}, // Yellow, Cyan, Magenta + {{1.0, 0.647, 0.0}}, {{0.749, 1.0, 0.0}}, + {{0.0, 0.502, 0.502}}, // Orange, Lime, Teal + {{0.502, 0.0, 0.502}}, {{1.0, 0.753, 0.796}}, + {{0.647, 0.165, 0.165}}, // Purple, Pink, Brown + {{0.502, 0.0, 0.0}}, {{0.502, 0.502, 0.0}}, + {{0.0, 0.0, 0.502}}, // Maroon, Olive, Navy + {{0.502, 0.502, 0.502}}, {{1.0, 0.4, 0.4}}, + {{0.4, 1.0, 0.4}}, // Grey, Light Red, Light Green + {{0.4, 0.4, 1.0}}, {{1.0, 1.0, 0.4}}, + {{0.4, 1.0, 1.0}}, // Light Blue, Light Yellow, Light Cyan + {{1.0, 0.4, 1.0}}, {{1.0, 0.698, 0.4}}, + {{0.698, 0.4, 1.0}}, // Light Magenta, Light Orange, Light Purple + {{1.0, 0.6, 0.8}}, {{0.71, 0.396, 0.114}}, + {{0.545, 0.0, 0.0}}, // Light Pink, Light Brown, Dark Red + {{0.0, 0.392, 0.0}}, {{0.0, 0.0, 0.545}}, + {{0.545, 0.545, 0.0}}, // Dark Green, Dark Blue, Dark Yellow + {{0.0, 0.545, 0.545}}, {{0.545, 0.0, 0.545}} // Dark Cyan, Dark Magenta + }}; + + std::unordered_set current_ids; + + radar_msgs::msg::RadarTrack track_msg; + for (const auto & object : msg.objects) { + const double half_length = 0.5 * object.shape_length_edge_mean; + const double half_width = 0.5 * object.shape_width_edge_mean; + constexpr double DEFAULT_HALF_SIZE = 1.0; + const int reference_index = std::min(object.position_reference, 8); + const double & yaw = object.orientation; + current_ids.emplace(object.object_id); + + visualization_msgs::msg::Marker box_marker; + box_marker.header.frame_id = sensor_cfg_ptr_->object_frame; + box_marker.header.stamp = msg.header.stamp; + box_marker.ns = "boxes"; + box_marker.id = object.object_id; + box_marker.action = visualization_msgs::msg::Marker::ADD; + box_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + box_marker.lifetime = rclcpp::Duration::from_seconds(0); + box_marker.color.r = color_array[object.object_id % PALETTE_SIZE][0]; + box_marker.color.g = color_array[object.object_id % PALETTE_SIZE][1]; + box_marker.color.b = color_array[object.object_id % PALETTE_SIZE][2]; + box_marker.color.a = 1.0; + box_marker.scale.x = 0.1; + + box_marker.pose.position.x = + object.position.x + std::cos(yaw) * half_length * reference_to_center_[reference_index][0] - + std::sin(yaw) * half_width * reference_to_center_[reference_index][1]; + box_marker.pose.position.y = + object.position.y + std::sin(yaw) * half_length * reference_to_center_[reference_index][0] + + std::cos(yaw) * half_width * reference_to_center_[reference_index][1]; + box_marker.pose.position.z = object.position.z; + box_marker.pose.orientation.w = std::cos(0.5 * yaw); + box_marker.pose.orientation.z = std::sin(0.5 * yaw); + + for (const auto & corner : cube_corners) { + geometry_msgs::msg::Point p; + p.x = half_length * corner[0]; + p.y = half_width * corner[1]; + p.z = DEFAULT_HALF_SIZE * corner[2]; + box_marker.points.emplace_back(p); + } + + marker_array.markers.emplace_back(box_marker); + + visualization_msgs::msg::Marker text_marker = box_marker; + text_marker.ns = "object_age"; + text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + text_marker.color.r = 1.0; + text_marker.color.g = 1.0; + text_marker.color.b = 1.0; + text_marker.color.a = 1.0; + text_marker.scale.x = 0.3; + text_marker.scale.y = 0.3; + text_marker.scale.z = 0.3; + text_marker.pose.position.z += 0.5; + text_marker.text = + "ID=" + std::to_string(object.object_id) + " Age=" + std::to_string(object.age) + "ms"; + + marker_array.markers.emplace_back(text_marker); + + std::stringstream object_status_ss; + object_status_ss << std::fixed << std::setprecision(3) << "ID=" << object.object_id << "\n" + << static_cast(object.status_measurement) << "/" + << static_cast(object.status_movement) << "/" + << static_cast(object.position_reference); + + text_marker.ns = "object_status"; + text_marker.text = object_status_ss.str(); + + marker_array.markers.emplace_back(text_marker); + + std::stringstream object_dynamics_ss; + object_dynamics_ss << std::fixed << std::setprecision(3) << "ID=" << object.object_id + << "\nyaw=" << object.orientation + << "\nyaw_rate=" << object.orientation_rate_mean + << "\nvx=" << object.absolute_velocity.x + << "\nvy=" << object.absolute_velocity.y + << "\nax=" << object.absolute_acceleration.x + << "\nay=" << object.absolute_acceleration.y; + + text_marker.ns = "object_dynamics"; + text_marker.text = object_dynamics_ss.str(); + + marker_array.markers.emplace_back(text_marker); + } + + for (const auto & previous_id : previous_ids_) { + if (current_ids.find(previous_id) != current_ids.end()) { + continue; + } + + visualization_msgs::msg::Marker delete_marker; + delete_marker.header.frame_id = sensor_cfg_ptr_->object_frame; + delete_marker.header.stamp = msg.header.stamp; + delete_marker.ns = "boxes"; + delete_marker.id = previous_id; + delete_marker.action = visualization_msgs::msg::Marker::DELETE; + + marker_array.markers.push_back(delete_marker); + + delete_marker.ns = "object_age"; + marker_array.markers.push_back(delete_marker); + + delete_marker.ns = "object_status"; + marker_array.markers.push_back(delete_marker); + + delete_marker.ns = "object_dynamics"; + marker_array.markers.push_back(delete_marker); + } + + previous_ids_.clear(); + previous_ids_ = current_ids; + + return marker_array; +} + +RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalArs548DriverRosWrapper) +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/continental_ars548_decoder_ros_wrapper.hpp b/nebula_ros/continental_ars548_decoder_ros_wrapper.hpp new file mode 100644 index 000000000..7cd43df44 --- /dev/null +++ b/nebula_ros/continental_ars548_decoder_ros_wrapper.hpp @@ -0,0 +1,166 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NEBULA_ContinentalArs548DriverRosWrapper_H +#define NEBULA_ContinentalArs548DriverRosWrapper_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace nebula +{ +namespace ros +{ +/// @brief Ros wrapper of continental radar ethernet driver +class ContinentalArs548DriverRosWrapper final : public rclcpp::Node, NebulaDriverRosWrapperBase +{ + std::shared_ptr decoder_ptr_; + Status wrapper_status_; + + rclcpp::Subscription::SharedPtr packets_sub_; + + rclcpp::Publisher::SharedPtr + detection_list_pub_; + rclcpp::Publisher::SharedPtr object_list_pub_; + rclcpp::Publisher::SharedPtr object_pointcloud_pub_; + rclcpp::Publisher::SharedPtr detection_pointcloud_pub_; + rclcpp::Publisher::SharedPtr scan_raw_pub_; + rclcpp::Publisher::SharedPtr objects_raw_pub_; + rclcpp::Publisher::SharedPtr objects_markers_pub_; + rclcpp::Publisher::SharedPtr diagnostics_pub_; + + std::unordered_set previous_ids_; + + constexpr static int REFERENCE_POINTS_NUM = 9; + constexpr static std::array, 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 + sensor_cfg_ptr_; + + drivers::continental_ars548::ContinentalArs548HwInterface hw_interface_; + + /// @brief Initializing ros wrapper + /// @param sensor_configuration SensorConfiguration for this driver + /// @param calibration_configuration CalibrationConfiguration for this driver + /// @return Resulting status + Status InitializeDriver(std::shared_ptr sensor_configuration); + + /// @brief Get configurations from ros parameters + /// @param sensor_configuration Output of SensorConfiguration + /// @param calibration_configuration Output of CalibrationConfiguration + /// @param correction_configuration Output of CorrectionConfiguration (for AT) + /// @return Resulting status + Status GetParameters( + drivers::continental_ars548::ContinentalArs548SensorConfiguration & sensor_configuration); + + /// @brief Convert seconds to chrono::nanoseconds + /// @param seconds + /// @return chrono::nanoseconds + static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) + { + return std::chrono::duration_cast( + std::chrono::duration(seconds)); + } + + /// @brief Callback to process new ContinentalArs548DetectionList from the driver + /// @param msg The new ContinentalArs548DetectionList from the driver + void DetectionListCallback( + std::unique_ptr msg); + + /// @brief Callback to process new ContinentalArs548ObjectList from the driver + /// @param msg The new ContinentalArs548ObjectList from the driver + void ObjectListCallback(std::unique_ptr msg); + + /// @brief Callback to process new ContinentalArs548Status from the driver + /// @param msg The new ContinentalArs548ObjectList from the driver + void SensorStatusCallback( + const drivers::continental_ars548::ContinentalArs548Status & sensor_status); + +public: + explicit ContinentalArs548DriverRosWrapper(const rclcpp::NodeOptions & options); + + /// @brief Callback for NebulaPackets subscriber + /// @param scan_msg Received NebulaPackets + void ReceivePacketsMsgCallback(const nebula_msgs::msg::NebulaPackets::SharedPtr scan_msg); + + /// @brief Get current status of this driver + /// @return Current status + Status GetStatus(); + + /// @brief Convert ARS548 detections to a pointcloud + /// @param msg The ARS548 detection list msg + /// @return Resulting detection pointcloud + pcl::PointCloud::Ptr + ConvertToPointcloud(const continental_msgs::msg::ContinentalArs548DetectionList & msg); + + /// @brief Convert ARS548 objects to a pointcloud + /// @param msg The ARS548 object list msg + /// @return Resulting object pointcloud + pcl::PointCloud::Ptr ConvertToPointcloud( + const continental_msgs::msg::ContinentalArs548ObjectList & msg); + + /// @brief Convert ARS548 detections to a standard RadarScan msg + /// @param msg The ARS548 detection list msg + /// @return Resulting RadarScan msg + radar_msgs::msg::RadarScan ConvertToRadarScan( + const continental_msgs::msg::ContinentalArs548DetectionList & msg); + + /// @brief Convert ARS548 objects to a standard RadarTracks msg + /// @param msg The ARS548 object list msg + /// @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 +} // namespace nebula + +#endif // NEBULA_ContinentalArs548DriverRosWrapper_H diff --git a/nebula_ros/continental_ars548_hw_interface_ros_wrapper.cpp b/nebula_ros/continental_ars548_hw_interface_ros_wrapper.cpp new file mode 100644 index 000000000..40ea3b1e6 --- /dev/null +++ b/nebula_ros/continental_ars548_hw_interface_ros_wrapper.cpp @@ -0,0 +1,561 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include +#include + +namespace nebula +{ +namespace ros +{ +ContinentalArs548HwInterfaceRosWrapper::ContinentalArs548HwInterfaceRosWrapper( + const rclcpp::NodeOptions & options) +: rclcpp::Node("continental_ars548_hw_interface_ros_wrapper", options), hw_interface_() +{ + if (mtx_config_.try_lock()) { + interface_status_ = GetParameters(sensor_configuration_); + mtx_config_.unlock(); + } + if (Status::OK != interface_status_) { + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); + return; + } + hw_interface_.SetLogger(std::make_shared(this->get_logger())); + std::shared_ptr + sensor_cfg_ptr = + std::make_shared( + sensor_configuration_); + hw_interface_.SetSensorConfiguration( + std::static_pointer_cast(sensor_cfg_ptr)); + + hw_interface_.RegisterScanCallback(std::bind( + &ContinentalArs548HwInterfaceRosWrapper::ReceivePacketsDataCallback, this, + std::placeholders::_1)); + packets_pub_ = this->create_publisher( + "nebula_packets", rclcpp::SensorDataQoS()); + + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&ContinentalArs548HwInterfaceRosWrapper::paramCallback, this, std::placeholders::_1)); + + StreamStart(); +} + +ContinentalArs548HwInterfaceRosWrapper::~ContinentalArs548HwInterfaceRosWrapper() +{ +} + +Status ContinentalArs548HwInterfaceRosWrapper::StreamStart() +{ + if (Status::OK == interface_status_) { + interface_status_ = hw_interface_.SensorInterfaceStart(); + } + + if (Status::OK == interface_status_) { + odometry_sub_ = this->create_subscription( + "odometry_input", rclcpp::QoS{1}, + std::bind( + &ContinentalArs548HwInterfaceRosWrapper::OdometryCallback, this, std::placeholders::_1)); + + acceleration_sub_ = create_subscription( + "acceleration_input", rclcpp::QoS{1}, + std::bind( + &ContinentalArs548HwInterfaceRosWrapper::AccelerationCallback, this, + std::placeholders::_1)); + + steering_angle_sub_ = create_subscription( + "steering_angle_input", rclcpp::SensorDataQoS(), + std::bind( + &ContinentalArs548HwInterfaceRosWrapper::SteeringAngleCallback, this, + std::placeholders::_1)); + + set_network_configuration_service_server_ = + this->create_service( + "set_network_configuration", + std::bind( + &ContinentalArs548HwInterfaceRosWrapper::SetNetworkConfigurationRequestCallback, this, + std::placeholders::_1, std::placeholders::_2)); + + set_sensor_mounting_service_server_ = + this->create_service( + "set_sensor_mounting", + std::bind( + &ContinentalArs548HwInterfaceRosWrapper::SetSensorMountingRequestCallback, this, + std::placeholders::_1, std::placeholders::_2)); + + set_vehicle_parameters_service_server_ = + this->create_service( + "set_vehicle_parameters", + std::bind( + &ContinentalArs548HwInterfaceRosWrapper::SetVehicleParametersRequestCallback, this, + std::placeholders::_1, std::placeholders::_2)); + + set_radar_parameters_service_server_ = + this->create_service( + "set_radar_parameters", + std::bind( + &ContinentalArs548HwInterfaceRosWrapper::SetRadarParametersRequestCallback, this, + std::placeholders::_1, std::placeholders::_2)); + } + + return interface_status_; +} + +Status ContinentalArs548HwInterfaceRosWrapper::StreamStop() +{ + return Status::OK; +} +Status ContinentalArs548HwInterfaceRosWrapper::Shutdown() +{ + return Status::OK; +} + +Status ContinentalArs548HwInterfaceRosWrapper::InitializeHwInterface( // todo: don't think + // this is needed + const drivers::SensorConfigurationBase & sensor_configuration) +{ + std::stringstream ss; + ss << sensor_configuration; + RCLCPP_DEBUG_STREAM(this->get_logger(), ss.str()); + return Status::OK; +} + +Status ContinentalArs548HwInterfaceRosWrapper::GetParameters( + drivers::continental_ars548::ContinentalArs548SensorConfiguration & sensor_configuration) +{ + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("sensor_model", descriptor); + sensor_configuration.sensor_model = + nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("host_ip", descriptor); + sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("sensor_ip", descriptor); + sensor_configuration.sensor_ip = this->get_parameter("sensor_ip").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("multicast_ip", descriptor); + sensor_configuration.multicast_ip = this->get_parameter("multicast_ip").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("frame_id", descriptor); + sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("base_frame", descriptor); + sensor_configuration.base_frame = this->get_parameter("base_frame").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("object_frame", descriptor); + sensor_configuration.object_frame = this->get_parameter("object_frame").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("data_port", descriptor); + sensor_configuration.data_port = this->get_parameter("data_port").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_host_port", descriptor); + sensor_configuration.configuration_host_port = + this->get_parameter("configuration_host_port").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_sensor_port", descriptor); + sensor_configuration.configuration_sensor_port = + this->get_parameter("configuration_sensor_port").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("use_sensor_time", descriptor); + sensor_configuration.use_sensor_time = this->get_parameter("use_sensor_time").as_bool(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_vehicle_length", descriptor); + sensor_configuration.configuration_vehicle_length = + static_cast(this->get_parameter("configuration_vehicle_length").as_double()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_vehicle_width", descriptor); + sensor_configuration.configuration_vehicle_width = + static_cast(this->get_parameter("configuration_vehicle_width").as_double()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_vehicle_height", descriptor); + sensor_configuration.configuration_vehicle_height = + static_cast(this->get_parameter("configuration_vehicle_height").as_double()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_vehicle_wheelbase", descriptor); + sensor_configuration.configuration_vehicle_wheelbase = + static_cast(this->get_parameter("configuration_vehicle_wheelbase").as_double()); + } + + if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + + RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); + return Status::OK; +} + +void ContinentalArs548HwInterfaceRosWrapper::ReceivePacketsDataCallback( + std::unique_ptr scan_buffer) +{ + packets_pub_->publish(std::move(scan_buffer)); +} + +rcl_interfaces::msg::SetParametersResult ContinentalArs548HwInterfaceRosWrapper::paramCallback( + const std::vector & p) +{ + std::scoped_lock lock(mtx_config_); + RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback"); + RCLCPP_DEBUG_STREAM(this->get_logger(), p); + RCLCPP_DEBUG_STREAM(this->get_logger(), sensor_configuration_); + RCLCPP_INFO_STREAM(this->get_logger(), p); + + drivers::continental_ars548::ContinentalArs548SensorConfiguration new_param{ + sensor_configuration_}; + RCLCPP_INFO_STREAM(this->get_logger(), new_param); + std::string sensor_model_str; + + if ( + get_param(p, "sensor_model", sensor_model_str) | get_param(p, "host_ip", new_param.host_ip) | + get_param(p, "sensor_ip", new_param.sensor_ip) | get_param(p, "frame_id", new_param.frame_id) | + get_param(p, "data_port", new_param.data_port) | + get_param(p, "multicast_ip", new_param.multicast_ip) | + get_param(p, "base_frame", new_param.base_frame) | + get_param(p, "object_frame", new_param.object_frame) | + get_param(p, "configuration_host_port", new_param.configuration_host_port) | + get_param(p, "configuration_sensor_port", new_param.configuration_sensor_port) | + get_param(p, "configuration_vehicle_length", new_param.configuration_vehicle_length) | + get_param(p, "configuration_vehicle_width", new_param.configuration_vehicle_width) | + get_param(p, "configuration_vehicle_height", new_param.configuration_vehicle_height) | + get_param(p, "configuration_vehicle_wheelbase", new_param.configuration_vehicle_wheelbase) | + get_param(p, "configuration_host_port", new_param.configuration_host_port) | + get_param(p, "configuration_sensor_port", new_param.configuration_sensor_port)) { + if (0 < sensor_model_str.length()) + new_param.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_str); + + sensor_configuration_ = new_param; + RCLCPP_INFO_STREAM(this->get_logger(), "Update sensor_configuration"); + std::shared_ptr sensor_cfg_ptr = + std::make_shared( + sensor_configuration_); + RCLCPP_DEBUG_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); + hw_interface_.SetSensorConfiguration( + std::static_pointer_cast(sensor_cfg_ptr)); + hw_interface_.CheckAndSetConfig(); + } + + auto result = std::make_shared(); + result->successful = true; + result->reason = "success"; + + RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback success"); + + return *result; +} + +void ContinentalArs548HwInterfaceRosWrapper::OdometryCallback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) +{ + std::scoped_lock lock(mtx_config_); + + constexpr float speed_to_standstill = 0.5f; + constexpr float speed_to_moving = 2.f; + + if (standstill_ && std::abs(msg->twist.twist.linear.x) > speed_to_moving) { + standstill_ = false; + } else if (!standstill_ && std::abs(msg->twist.twist.linear.x) < speed_to_standstill) { + standstill_ = true; + } + + if (standstill_) { + hw_interface_.SetDrivingDirection(0); + } else { + hw_interface_.SetDrivingDirection(msg->twist.twist.linear.x > 0.f ? 1 : -1); + } + + constexpr float ms_to_kmh = 3.6f; + hw_interface_.SetVelocityVehicle(ms_to_kmh * std::abs(msg->twist.twist.linear.x)); + + constexpr float rad_to_deg = 180.f / M_PI; + hw_interface_.SetYawRate(rad_to_deg * msg->twist.twist.angular.z); +} + +void ContinentalArs548HwInterfaceRosWrapper::AccelerationCallback( + const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg) +{ + std::scoped_lock lock(mtx_config_); + hw_interface_.SetAccelerationLateralCog(msg->accel.accel.linear.y); + hw_interface_.SetAccelerationLongitudinalCog(msg->accel.accel.linear.x); +} + +void ContinentalArs548HwInterfaceRosWrapper::SteeringAngleCallback( + const std_msgs::msg::Float32::SharedPtr msg) +{ + std::scoped_lock lock(mtx_config_); + constexpr float rad_to_deg = 180.f / M_PI; + hw_interface_.SetSteeringAngleFrontAxle(rad_to_deg * msg->data); +} + +void ContinentalArs548HwInterfaceRosWrapper::SetNetworkConfigurationRequestCallback( + const std::shared_ptr + request, + const std::shared_ptr + response) +{ + std::scoped_lock lock(mtx_config_); + auto result = hw_interface_.SetSensorIPAddress(request->sensor_ip.data); + response->success = result == Status::OK; + response->message = (std::stringstream() << result).str(); +} + +void ContinentalArs548HwInterfaceRosWrapper::SetSensorMountingRequestCallback( + const std::shared_ptr request, + const std::shared_ptr + response) +{ + std::scoped_lock lock(mtx_config_); + + auto tf_buffer = std::make_unique(this->get_clock()); + auto tf_listener = std::make_unique(*tf_buffer); + + float longitudinal = request->longitudinal; + float lateral = request->lateral; + float vertical = request->vertical; + float yaw = request->yaw; + float pitch = request->pitch; + + if (request->autoconfigure_extrinsics) { + RCLCPP_INFO( + this->get_logger(), + "autoconfigure_extrinsics was set to true, so the mounting position will be derived from the " + "tfs"); + + geometry_msgs::msg::TransformStamped base_to_sensor_tf; + try { + base_to_sensor_tf = tf_buffer->lookupTransform( + sensor_configuration_.base_frame, sensor_configuration_.frame_id, rclcpp::Time(0), + rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR( + this->get_logger(), "Could not obtain the transform from the base frame to %s (%s)", + sensor_configuration_.frame_id.c_str(), ex.what()); + response->success = false; + response->message = ex.what(); + return; + } + + const auto & quat = base_to_sensor_tf.transform.rotation; + geometry_msgs::msg::Vector3 rpy; + tf2::Matrix3x3(tf2::Quaternion(quat.x, quat.y, quat.z, quat.w)).getRPY(rpy.x, rpy.y, rpy.z); + + longitudinal = base_to_sensor_tf.transform.translation.x - + sensor_configuration_.configuration_vehicle_wheelbase; + lateral = base_to_sensor_tf.transform.translation.y; + vertical = base_to_sensor_tf.transform.translation.z; + yaw = rpy.z; + pitch = rpy.y; + } + + auto result = hw_interface_.SetSensorMounting( + longitudinal, lateral, vertical, yaw, pitch, request->plug_orientation); + + response->success = result == Status::OK; + response->message = (std::stringstream() << result).str(); +} + +void ContinentalArs548HwInterfaceRosWrapper::SetVehicleParametersRequestCallback( + [[maybe_unused]] const std::shared_ptr< + continental_srvs::srv::ContinentalArs548SetVehicleParameters::Request> + request, + const std::shared_ptr + response) +{ + float vehicle_length = request->vehicle_length; + float vehicle_width = request->vehicle_width; + float vehicle_height = request->vehicle_height; + float vehicle_wheelbase = request->vehicle_wheelbase; + + if (vehicle_length < 0.f) { + RCLCPP_INFO( + this->get_logger(), + "Service vehicle_length is invalid. Falling back to configuration value (%.2f)", + sensor_configuration_.configuration_vehicle_length); + vehicle_length = sensor_configuration_.configuration_vehicle_length; + } + + if (vehicle_width < 0.f) { + RCLCPP_INFO( + this->get_logger(), + "Service vehicle_width is invalid. Falling back to configuration value (%.2f)", + sensor_configuration_.configuration_vehicle_width); + vehicle_width = sensor_configuration_.configuration_vehicle_width; + } + + if (vehicle_height < 0.f) { + RCLCPP_INFO( + this->get_logger(), + "Service vehicle_height is invalid. Falling back to configuration value (%.2f)", + sensor_configuration_.configuration_vehicle_height); + vehicle_height = sensor_configuration_.configuration_vehicle_height; + } + + if (vehicle_wheelbase < 0.f) { + RCLCPP_INFO( + this->get_logger(), + "Service vehicle_wheelbase is invalid. Falling back to configuration value (%.2f)", + sensor_configuration_.configuration_vehicle_wheelbase); + vehicle_wheelbase = sensor_configuration_.configuration_vehicle_wheelbase; + } + + std::scoped_lock lock(mtx_config_); + auto result = hw_interface_.SetVehicleParameters( + vehicle_length, vehicle_width, vehicle_height, vehicle_wheelbase); + + response->success = result == Status::OK; + response->message = (std::stringstream() << result).str(); +} + +void ContinentalArs548HwInterfaceRosWrapper::SetRadarParametersRequestCallback( + const std::shared_ptr + request, + const std::shared_ptr + response) +{ + std::scoped_lock lock(mtx_config_); + auto result = hw_interface_.SetRadarParameters( + request->maximum_distance, request->frequency_slot, request->cycle_time, request->time_slot, + request->country_code, request->powersave_standstill); + + response->success = result == Status::OK; + response->message = (std::stringstream() << result).str(); +} + +std::vector +ContinentalArs548HwInterfaceRosWrapper::updateParameters() +{ + std::scoped_lock lock(mtx_config_); + RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters start"); + std::ostringstream os_sensor_model; + os_sensor_model << sensor_configuration_.sensor_model; + RCLCPP_INFO_STREAM(this->get_logger(), "set_parameters"); + auto results = set_parameters( + {rclcpp::Parameter("sensor_model", os_sensor_model.str()), + rclcpp::Parameter("host_ip", sensor_configuration_.host_ip), + rclcpp::Parameter("sensor_ip", sensor_configuration_.sensor_ip), + rclcpp::Parameter("frame_id", sensor_configuration_.frame_id), + rclcpp::Parameter("data_port", sensor_configuration_.data_port), + rclcpp::Parameter("multicast_ip", sensor_configuration_.multicast_ip), + rclcpp::Parameter("base_frame", sensor_configuration_.base_frame), + rclcpp::Parameter("object_frame", sensor_configuration_.object_frame), + rclcpp::Parameter("configuration_host_port", sensor_configuration_.configuration_host_port), + rclcpp::Parameter( + "configuration_sensor_port", sensor_configuration_.configuration_sensor_port), + rclcpp::Parameter( + "configuration_vehicle_length", sensor_configuration_.configuration_vehicle_length), + rclcpp::Parameter( + "configuration_vehicle_width", sensor_configuration_.configuration_vehicle_width), + rclcpp::Parameter( + "configuration_vehicle_height", sensor_configuration_.configuration_vehicle_height), + rclcpp::Parameter( + "configuration_vehicle_wheelbase", sensor_configuration_.configuration_vehicle_wheelbase)}); + RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters end"); + return results; +} + +RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalArs548HwInterfaceRosWrapper) +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/continental_ars548_hw_interface_ros_wrapper.hpp b/nebula_ros/continental_ars548_hw_interface_ros_wrapper.hpp new file mode 100644 index 000000000..e85309efd --- /dev/null +++ b/nebula_ros/continental_ars548_hw_interface_ros_wrapper.hpp @@ -0,0 +1,188 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NEBULA_ContinentalArs548HwInterfaceRosWrapper_H +#define NEBULA_ContinentalArs548HwInterfaceRosWrapper_H + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +namespace nebula +{ +namespace ros +{ +/// @brief Get parameter from rclcpp::Parameter +/// @tparam T +/// @param p Parameter from rclcpp parameter callback +/// @param name Target parameter name +/// @param value Corresponding value +/// @return Whether the target name existed +template +bool get_param(const std::vector & p, const std::string & name, T & value) +{ + auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { + return parameter.get_name() == name; + }); + if (it != p.cend()) { + value = it->template get_value(); + return true; + } + return false; +} + +/// @brief Hardware interface ros wrapper of continental radar ethernet driver +class ContinentalArs548HwInterfaceRosWrapper final : public rclcpp::Node, + NebulaHwInterfaceWrapperBase +{ + drivers::continental_ars548::ContinentalArs548HwInterface hw_interface_; + Status interface_status_; + + drivers::continental_ars548::ContinentalArs548SensorConfiguration sensor_configuration_; + + /// @brief Received Continental Radar message publisher + rclcpp::Publisher::SharedPtr packets_pub_; + rclcpp::Subscription::SharedPtr odometry_sub_; + rclcpp::Subscription::SharedPtr acceleration_sub_; + rclcpp::Subscription::SharedPtr steering_angle_sub_; + + bool standstill_{true}; + + rclcpp::Service::SharedPtr + set_network_configuration_service_server_; + rclcpp::Service::SharedPtr + set_sensor_mounting_service_server_; + rclcpp::Service::SharedPtr + set_vehicle_parameters_service_server_; + rclcpp::Service::SharedPtr + set_radar_parameters_service_server_; + + /// @brief Initializing hardware interface ros wrapper + /// @param sensor_configuration SensorConfiguration for this driver + /// @return Resulting status + Status InitializeHwInterface( + const drivers::SensorConfigurationBase & sensor_configuration) override; + /// @brief Callback for receiving NebulaPackets + /// @param packets_buffer Received NebulaPackets + void ReceivePacketsDataCallback(std::unique_ptr packets_buffer); + + /// @brief Callback to send the odometry information to the radar device + /// @param msg The odometry message + void OdometryCallback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); + + /// @brief Callback to send the acceleration information to the radar device + /// @param msg The acceleration message + void AccelerationCallback(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg); + + /// @brief Callback to send the steering angle information to the radar device + /// @param msg The steering angle message + void SteeringAngleCallback(const std_msgs::msg::Float32::SharedPtr msg); + + /// @brief Service callback to set the new sensor ip + /// @param request service request + /// @param response service response + void SetNetworkConfigurationRequestCallback( + const std::shared_ptr + request, + const std::shared_ptr + response); + + /// @brief Service callback to set the new sensor mounting position + /// @param request service request + /// @param response service response + void SetSensorMountingRequestCallback( + const std::shared_ptr + request, + const std::shared_ptr + response); + + /// @brief Service callback to set the new vehicle parameters + /// @param request service request + /// @param response service response + void SetVehicleParametersRequestCallback( + const std::shared_ptr + request, + const std::shared_ptr + response); + + /// @brief Service callback to set the new radar parameters + /// @param request service request + /// @param response service response + void SetRadarParametersRequestCallback( + const std::shared_ptr + request, + const std::shared_ptr + response); + +public: + explicit ContinentalArs548HwInterfaceRosWrapper(const rclcpp::NodeOptions & options); + ~ContinentalArs548HwInterfaceRosWrapper() noexcept override; + + /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) + /// @return Resulting status + Status StreamStart() override; + /// @brief Stop point cloud streaming (not used) + /// @return Resulting status + Status StreamStop() override; + /// @brief Shutdown (not used) + /// @return Resulting status + Status Shutdown() override; + /// @brief Get configurations from ros parameters + /// @param sensor_configuration Output of SensorConfiguration + /// @return Resulting status + Status GetParameters( + drivers::continental_ars548::ContinentalArs548SensorConfiguration & sensor_configuration); + +private: + std::mutex mtx_config_; + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + + /// @brief rclcpp parameter callback + /// @param parameters Received parameters + /// @return SetParametersResult + rcl_interfaces::msg::SetParametersResult paramCallback( + const std::vector & parameters); + + /// @brief Updating rclcpp parameter + /// @return SetParametersResult + std::vector updateParameters(); +}; + +} // namespace ros +} // namespace nebula + +#endif // NEBULA_ContinentalArs548HwInterfaceRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp index 0b68c03b9..5f56aa669 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp @@ -79,6 +79,10 @@ class ContinentalArs548DecoderWrapper void SensorStatusCallback( const drivers::continental_ars548::ContinentalArs548Status & sensor_status); + /// @brief Callback to process new ContinentalArs548Status from the driver + /// @param msg The new ContinentalArs548ObjectList from the driver + void PacketsCallback(std::unique_ptr msg); + private: nebula::Status InitializeDriver( const std::shared_ptr< diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_ros_wrapper.hpp new file mode 100644 index 000000000..54f4ddc81 --- /dev/null +++ b/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_ros_wrapper.hpp @@ -0,0 +1,162 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NEBULA_ContinentalSrr520DriverRosWrapper_H +#define NEBULA_ContinentalSrr520DriverRosWrapper_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace nebula +{ +namespace ros +{ +/// @brief Ros wrapper of continental radar ethernet driver +class ContinentalSrr520DriverRosWrapper final : public rclcpp::Node, NebulaDriverRosWrapperBase +{ + std::shared_ptr decoder_ptr_; + Status wrapper_status_; + + rclcpp::Subscription::SharedPtr packets_sub_; + + rclcpp::Publisher::SharedPtr + near_detection_list_pub_; + rclcpp::Publisher::SharedPtr + hrr_detection_list_pub_; + rclcpp::Publisher::SharedPtr object_list_pub_; + rclcpp::Publisher::SharedPtr status_pub_; + rclcpp::Publisher::SharedPtr near_detection_pointcloud_pub_; + rclcpp::Publisher::SharedPtr hrr_detection_pointcloud_pub_; + rclcpp::Publisher::SharedPtr object_pointcloud_pub_; + rclcpp::Publisher::SharedPtr near_scan_raw_pub_; + rclcpp::Publisher::SharedPtr hrr_scan_raw_pub_; + rclcpp::Publisher::SharedPtr objects_raw_pub_; + rclcpp::Publisher::SharedPtr objects_markers_pub_; + + std::unordered_set previous_ids_; + + std::shared_ptr + sensor_cfg_ptr_; + + drivers::continental_srr520::ContinentalSrr520HwInterface hw_interface_; + + /// @brief Initializing ros wrapper + /// @param sensor_configuration SensorConfiguration for this driver + /// @param calibration_configuration CalibrationConfiguration for this driver + /// @return Resulting status + Status InitializeDriver(std::shared_ptr sensor_configuration); + + /// @brief Get configurations from ros parameters + /// @param sensor_configuration Output of SensorConfiguration + /// @param calibration_configuration Output of CalibrationConfiguration + /// @param correction_configuration Output of CorrectionConfiguration (for AT) + /// @return Resulting status + Status GetParameters( + drivers::continental_srr520::ContinentalSrr520SensorConfiguration & sensor_configuration); + + /// @brief Convert seconds to chrono::nanoseconds + /// @param seconds + /// @return chrono::nanoseconds + static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) + { + return std::chrono::duration_cast( + std::chrono::duration(seconds)); + } + + /// @brief Callback to process new Near ContinentalSrr520DetectionList from the driver + /// @param msg The new ContinentalSrr520DetectionList from the driver + void NearDetectionListCallback( + std::unique_ptr msg); + + /// @brief Callback to process new HRR ContinentalSrr520DetectionList from the driver + /// @param msg The new ContinentalSrr520DetectionList from the driver + void HRRDetectionListCallback( + std::unique_ptr msg); + + /// @brief Callback to process new ContinentalSrr520ObjectList from the driver + /// @param msg The new ContinentalSrr520ObjectList from the driver + void ObjectListCallback(std::unique_ptr msg); + + /// @brief Callback to process new DiagnosticArray from the driver + /// @param msg The new DiagnosticArray from the driver + void StatusCallback(std::unique_ptr msg); + +public: + explicit ContinentalSrr520DriverRosWrapper(const rclcpp::NodeOptions & options); + + /// @brief Callback for NebulaPackets subscriber + /// @param scan_msg Received NebulaPackets + void ReceivePacketsMsgCallback(const nebula_msgs::msg::NebulaPackets::SharedPtr scan_msg); + + /// @brief Get current status of this driver + /// @return Current status + Status GetStatus(); + + /// @brief Convert SRR520 detections to a pointcloud + /// @param msg The SRR520 detection list msg + /// @return Resulting detection pointcloud + pcl::PointCloud::Ptr + ConvertToPointcloud(const continental_msgs::msg::ContinentalSrr520DetectionList & msg); + + /// @brief Convert SRR520 objects to a pointcloud + /// @param msg The SRR520 object list msg + /// @return Resulting object pointcloud + pcl::PointCloud::Ptr ConvertToPointcloud( + const continental_msgs::msg::ContinentalSrr520ObjectList & msg); + + /// @brief Convert SRR520 detections to a standard RadarScan msg + /// @param msg The SRR520 detection list msg + /// @return Resulting RadarScan msg + radar_msgs::msg::RadarScan ConvertToRadarScan( + const continental_msgs::msg::ContinentalSrr520DetectionList & msg); + + /// @brief Convert SRR520 objects to a standard RadarTracks msg + /// @param msg The SRR520 object list msg + /// @return Resulting RadarTracks msg + radar_msgs::msg::RadarTracks ConvertToRadarTracks( + const continental_msgs::msg::ContinentalSrr520ObjectList & msg); + + /// @brief Convert SRR520 objects to a standard MarkerArray msg + /// @param msg The SRR520 object list msg + /// @return Resulting MarkerArray msg + visualization_msgs::msg::MarkerArray ConvertToMarkers( + const continental_msgs::msg::ContinentalSrr520ObjectList & msg); +}; + +} // namespace ros +} // namespace nebula + +#endif // NEBULA_ContinentalSrr520DriverRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp new file mode 100644 index 000000000..d2dc3d095 --- /dev/null +++ b/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp @@ -0,0 +1,183 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +//#include +#include +#include +#include +#include + +namespace nebula +{ +namespace ros +{ +class ContinentalSrr520DecoderWrapper +{ +public: + ContinentalSrr520DecoderWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr & + config, + std::shared_ptr hw_interface_ptr); + + void ProcessPacket(std::unique_ptr packet_msg); + + void OnConfigChange( + const std::shared_ptr< + const nebula::drivers::continental_srr520::ContinentalSrr520SensorConfiguration> & + new_config); + + rcl_interfaces::msg::SetParametersResult OnParameterChange( + const std::vector & p); + + nebula::Status Status(); + + /// @brief Callback to process a new Near ContinentalSrr520DetectionList from the driver + /// @param msg The new ContinentalSrr520DetectionList from the driver + void NearDetectionListCallback( + std::unique_ptr msg); + + /// @brief Callback to process a new HRR ContinentalSrr520DetectionList from the driver + /// @param msg The new ContinentalSrr520DetectionList from the driver + void HRRDetectionListCallback( + std::unique_ptr msg); + + /// @brief Callback to process a new ContinentalSrr520ObjectList from the driver + /// @param msg The new ContinentalSrr520ObjectList from the driver + void ObjectListCallback(std::unique_ptr msg); + + /// @brief Callback to process a new DiagnosticArray from the driver + /// @param msg The new DiagnosticArray from the driver + void StatusCallback(std::unique_ptr msg); + + /// @brief Callback to process a new SyncFup message from the driver + void SyncFupCallback(); + + /// @brief Callback to process a new NebulaPackets message from the driver + /// @param msg The new NebulaPackets from the driver + void PacketsCallback(std::unique_ptr msg); + +private: + nebula::Status InitializeDriver( + const std::shared_ptr< + const nebula::drivers::continental_srr520::ContinentalSrr520SensorConfiguration> & config); + + /// @brief Convert SRR520 detections to a pointcloud + /// @param msg The SRR520 detection list msg + /// @return Resulting detection pointcloud + pcl::PointCloud::Ptr + ConvertToPointcloud(const continental_msgs::msg::ContinentalSrr520DetectionList & msg); + + /// @brief Convert SRR520 objects to a pointcloud + /// @param msg The SRR520 object list msg + /// @return Resulting object pointcloud + pcl::PointCloud::Ptr ConvertToPointcloud( + const continental_msgs::msg::ContinentalSrr520ObjectList & msg); + + /// @brief Convert SRR520 detections to a standard RadarScan msg + /// @param msg The SRR520 detection list msg + /// @return Resulting RadarScan msg + radar_msgs::msg::RadarScan ConvertToRadarScan( + const continental_msgs::msg::ContinentalSrr520DetectionList & msg); + + /// @brief Convert SRR520 objects to a standard RadarTracks msg + /// @param msg The SRR520 object list msg + /// @return Resulting RadarTracks msg + radar_msgs::msg::RadarTracks ConvertToRadarTracks( + const continental_msgs::msg::ContinentalSrr520ObjectList & msg); + + /// @brief Convert SRR520 objects to a standard MarkerArray msg + /// @param msg The SRR520 object list msg + /// @return Resulting MarkerArray msg + visualization_msgs::msg::MarkerArray ConvertToMarkers( + const continental_msgs::msg::ContinentalSrr520ObjectList & msg); + + /// @brief Convert seconds to chrono::nanoseconds + /// @param seconds + /// @return chrono::nanoseconds + static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) + { + return std::chrono::duration_cast( + std::chrono::duration(seconds)); + } + + const rclcpp::Node * const parent_node_; + nebula::Status status_; + rclcpp::Logger logger_; + + std::shared_ptr + sensor_cfg_{}; + + std::shared_ptr driver_ptr_{}; + std::shared_ptr hw_interface_ptr_{}; + std::mutex mtx_driver_ptr_; + + rclcpp::Publisher::SharedPtr packets_pub_{}; + + rclcpp::Publisher::SharedPtr + near_detection_list_pub_{}; + rclcpp::Publisher::SharedPtr + hrr_detection_list_pub_{}; + rclcpp::Publisher::SharedPtr + object_list_pub_{}; + rclcpp::Publisher::SharedPtr status_pub_{}; + rclcpp::Publisher::SharedPtr near_detection_pointcloud_pub_{}; + rclcpp::Publisher::SharedPtr hrr_detection_pointcloud_pub_{}; + rclcpp::Publisher::SharedPtr object_pointcloud_pub_{}; + rclcpp::Publisher::SharedPtr near_scan_raw_pub_{}; + rclcpp::Publisher::SharedPtr hrr_scan_raw_pub_{}; + rclcpp::Publisher::SharedPtr objects_raw_pub_{}; + rclcpp::Publisher::SharedPtr objects_markers_pub_{}; + + std::unordered_set previous_ids_; + + /* constexpr static int REFERENCE_POINTS_NUM = 9; + constexpr static std::array, 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 cloud_watchdog_; +}; +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_ros_wrapper.hpp new file mode 100644 index 000000000..1ef242cdb --- /dev/null +++ b/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_ros_wrapper.hpp @@ -0,0 +1,161 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NEBULA_ContinentalSrr520HwInterfaceRosWrapper_H +#define NEBULA_ContinentalSrr520HwInterfaceRosWrapper_H + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace nebula +{ +namespace ros +{ +/// @brief Get parameter from rclcpp::Parameter +/// @tparam T +/// @param p Parameter from rclcpp parameter callback +/// @param name Target parameter name +/// @param value Corresponding value +/// @return Whether the target name existed +template +bool get_param(const std::vector & p, const std::string & name, T & value) +{ + auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { + return parameter.get_name() == name; + }); + if (it != p.cend()) { + value = it->template get_value(); + return true; + } + return false; +} + +/// @brief Hardware interface ros wrapper of continental radar ethernet driver +class ContinentalSrr520HwInterfaceRosWrapper final : public rclcpp::Node, + NebulaHwInterfaceWrapperBase +{ + drivers::continental_srr520::ContinentalSrr520HwInterface hw_interface_; + Status interface_status_; + + drivers::continental_srr520::ContinentalSrr520SensorConfiguration sensor_configuration_; + + /// @brief Received Continental Radar message publisher + rclcpp::Publisher::SharedPtr packets_pub_; + + message_filters::Subscriber odometry_sub_; + message_filters::Subscriber acceleration_sub_; + + using ExactTimeSyncPolicy = message_filters::sync_policies::ExactTime< + geometry_msgs::msg::TwistWithCovarianceStamped, geometry_msgs::msg::AccelWithCovarianceStamped>; + using ExactTimeSync = message_filters::Synchronizer; + std::shared_ptr sync_ptr_; + rclcpp::TimerBase::SharedPtr sync_timer_; + + bool standstill_{true}; + + rclcpp::Service::SharedPtr + configure_sensor_service_server_; + + /// @brief Initializing hardware interface ros wrapper + /// @param sensor_configuration SensorConfiguration for this driver + /// @return Resulting status + Status InitializeHwInterface( + const drivers::SensorConfigurationBase & sensor_configuration) override; + /// @brief Callback for receiving NebulaPackets + /// @param packets_buffer Received NebulaPackets + void ReceivePacketsDataCallback(std::unique_ptr packets_buffer); + + /// @brief Callback to send the odometry information to the radar device + /// @param odometry_msg The odometry message + /// @param acceleration_msg The acceleration message + void dynamicsCallback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr & odometry_msg, + const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr & acceleration_msg); + + /// @brief Service callback to set the new sensor mounting position + /// @param request Empty service request + /// @param response Empty service response + void ConfigureSensorRequestCallback( + const std::shared_ptr + request, + const std::shared_ptr + response); + + /// @brief Method periodically called to initiate the sensor synchronization mechanism + void syncTimerCallback(); + +public: + explicit ContinentalSrr520HwInterfaceRosWrapper(const rclcpp::NodeOptions & options); + ~ContinentalSrr520HwInterfaceRosWrapper() noexcept override; + + /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) + /// @return Resulting status + Status StreamStart() override; + /// @brief Stop point cloud streaming (not used) + /// @return Resulting status + Status StreamStop() override; + /// @brief Shutdown (not used) + /// @return Resulting status + Status Shutdown() override; + /// @brief Get configurations from ros parameters + /// @param sensor_configuration Output of SensorConfiguration + /// @return Resulting status + Status GetParameters( + drivers::continental_srr520::ContinentalSrr520SensorConfiguration & sensor_configuration); + +private: + std::mutex mtx_config_; + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + + /// @brief rclcpp parameter callback + /// @param parameters Received parameters + /// @return SetParametersResult + rcl_interfaces::msg::SetParametersResult paramCallback( + const std::vector & parameters); + + /// @brief Updating rclcpp parameter + /// @return SetParametersResult + std::vector updateParameters(); +}; + +} // namespace ros +} // namespace nebula + +#endif // NEBULA_ContinentalSrr520HwInterfaceRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp new file mode 100644 index 000000000..e4e50487d --- /dev/null +++ b/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp @@ -0,0 +1,99 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include + +namespace nebula +{ +namespace ros +{ +class ContinentalSrr520HwInterfaceWrapper +{ +public: + ContinentalSrr520HwInterfaceWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr & + config); + + /// @brief Starts the hw interface and subscribes to the input topics + void SensorInterfaceStart(); + + void OnConfigChange( + const std::shared_ptr & + new_config); + + /// @brief Get current status of the hw interface + /// @return Current status + nebula::Status Status(); + + std::shared_ptr HwInterface() const; + +private: + /// @brief Callback to send the odometry information to the radar device + /// @param odometry_msg The odometry message + /// @param acceleration_msg The acceleration message + void dynamicsCallback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr & odometry_msg, + const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr & acceleration_msg); + + /// @brief Service callback to set the new sensor mounting position + /// @param request Empty service request + /// @param response Empty service response + void ConfigureSensorRequestCallback( + const std::shared_ptr + request, + const std::shared_ptr + response); + + /// @brief Method periodically called to initiate the sensor synchronization mechanism + void syncTimerCallback(); + + rclcpp::Node * const parent_node_; + std::shared_ptr hw_interface_; + rclcpp::Logger logger_; + nebula::Status status_; + std::shared_ptr + config_ptr_; + + message_filters::Subscriber odometry_sub_; + message_filters::Subscriber acceleration_sub_; + + using ExactTimeSyncPolicy = message_filters::sync_policies::ExactTime< + geometry_msgs::msg::TwistWithCovarianceStamped, geometry_msgs::msg::AccelWithCovarianceStamped>; + using ExactTimeSync = message_filters::Synchronizer; + std::shared_ptr sync_ptr_; + rclcpp::TimerBase::SharedPtr sync_timer_; + + bool standstill_{true}; + + rclcpp::Service::SharedPtr + configure_sensor_service_server_; +}; +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp new file mode 100644 index 000000000..217564775 --- /dev/null +++ b/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp @@ -0,0 +1,106 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* #include +#include +#include */ +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace nebula +{ +namespace ros +{ + +/// @brief Ros wrapper of continental srr520 driver +class ContinentalSrr520RosWrapper final : public rclcpp::Node +{ +public: + explicit ContinentalSrr520RosWrapper(const rclcpp::NodeOptions & options); + ~ContinentalSrr520RosWrapper() noexcept {}; + + /// @brief Get current status of this driver + /// @return Current status + Status GetStatus(); + + /// @brief Start data streaming (Call SensorInterfaceStart of HwInterface) + /// @return Resulting status + Status StreamStart(); + +private: + /// @brief Callback from the hw interface's raw data + void ReceivePacketCallback(std::vector & packet); + + /// @brief Callback from replayed NebulaPackets + void ReceivePacketsMessageCallback(std::unique_ptr packets_msg); + + /// @brief Retrieve the parameters from ROS and set the driver and hw interface + /// @return Resulting status + Status DeclareAndGetSensorConfigParams(); + + /// @brief rclcpp parameter callback + /// @param parameters Received parameters + /// @return SetParametersResult + rcl_interfaces::msg::SetParametersResult OnParameterChange( + const std::vector & p); + + Status ValidateAndSetConfig( + std::shared_ptr & + new_config); + + Status wrapper_status_; + + std::shared_ptr + sensor_cfg_ptr_{}; + + /// @brief Stores received packets that have not been processed yet by the decoder thread + mt_queue> packet_queue_; + /// @brief Thread to isolate decoding from receiving + std::thread decoder_thread_; + + rclcpp::Subscription::SharedPtr packets_sub_{}; + + bool launch_hw_{}; + + std::optional hw_interface_wrapper_{}; + std::optional decoder_wrapper_{}; + + std::mutex mtx_config_; + + OnSetParametersCallbackHandle::SharedPtr parameter_event_cb_; +}; + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/multi_continental_ars548_hw_interface_ros_wrapper.cpp b/nebula_ros/multi_continental_ars548_hw_interface_ros_wrapper.cpp new file mode 100644 index 000000000..965ff9457 --- /dev/null +++ b/nebula_ros/multi_continental_ars548_hw_interface_ros_wrapper.cpp @@ -0,0 +1,356 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include +#include + +namespace nebula +{ +namespace ros +{ +MultiContinentalArs548HwInterfaceRosWrapper::MultiContinentalArs548HwInterfaceRosWrapper( + const rclcpp::NodeOptions & options) +: rclcpp::Node("multi_continental_ars548_hw_interface_ros_wrapper", options), hw_interface_() +{ + if (mtx_config_.try_lock()) { + interface_status_ = GetParameters(sensor_configuration_); + mtx_config_.unlock(); + } + if (Status::OK != interface_status_) { + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); + return; + } + hw_interface_.SetLogger(std::make_shared(this->get_logger())); + std::shared_ptr + sensor_cfg_ptr = + std::make_shared( + sensor_configuration_); + hw_interface_.SetSensorConfiguration( + std::static_pointer_cast(sensor_cfg_ptr)); + + assert(sensor_configuration_.sensor_ips.size() == sensor_configuration_.frame_ids.size()); + hw_interface_.RegisterScanCallback(std::bind( + &MultiContinentalArs548HwInterfaceRosWrapper::ReceivePacketsDataCallback, this, + std::placeholders::_1, std::placeholders::_2)); + + for (std::size_t sensor_id = 0; sensor_id < sensor_configuration_.sensor_ips.size(); + sensor_id++) { + const std::string sensor_ip = sensor_configuration_.sensor_ips[sensor_id]; + const std::string frame_id = sensor_configuration_.frame_ids[sensor_id]; + packets_pub_map_[sensor_ip] = this->create_publisher( + frame_id + "/nebula_packets", rclcpp::SensorDataQoS()); + } + + set_param_res_ = this->add_on_set_parameters_callback(std::bind( + &MultiContinentalArs548HwInterfaceRosWrapper::paramCallback, this, std::placeholders::_1)); + + StreamStart(); +} + +MultiContinentalArs548HwInterfaceRosWrapper::~MultiContinentalArs548HwInterfaceRosWrapper() +{ +} + +Status MultiContinentalArs548HwInterfaceRosWrapper::StreamStart() +{ + if (Status::OK == interface_status_) { + interface_status_ = hw_interface_.SensorInterfaceStart(); + } + + if (Status::OK == interface_status_) { + odometry_sub_ = this->create_subscription( + "odometry_input", rclcpp::QoS{1}, + std::bind( + &MultiContinentalArs548HwInterfaceRosWrapper::OdometryCallback, this, + std::placeholders::_1)); + + acceleration_sub_ = create_subscription( + "acceleration_input", rclcpp::QoS{1}, + std::bind( + &MultiContinentalArs548HwInterfaceRosWrapper::AccelerationCallback, this, + std::placeholders::_1)); + + steering_angle_sub_ = create_subscription( + "steering_angle_input", rclcpp::QoS{1}, + std::bind( + &MultiContinentalArs548HwInterfaceRosWrapper::SteeringAngleCallback, this, + std::placeholders::_1)); + } + + return interface_status_; +} + +Status MultiContinentalArs548HwInterfaceRosWrapper::StreamStop() +{ + return Status::OK; +} +Status MultiContinentalArs548HwInterfaceRosWrapper::Shutdown() +{ + return Status::OK; +} + +Status MultiContinentalArs548HwInterfaceRosWrapper::InitializeHwInterface( // todo: don't think + // this is needed + const drivers::SensorConfigurationBase & sensor_configuration) +{ + std::stringstream ss; + ss << sensor_configuration; + RCLCPP_DEBUG_STREAM(this->get_logger(), ss.str()); + return Status::OK; +} + +Status MultiContinentalArs548HwInterfaceRosWrapper::GetParameters( + drivers::continental_ars548::MultiContinentalArs548SensorConfiguration & sensor_configuration) +{ + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("sensor_model", descriptor); + sensor_configuration.sensor_model = + nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("host_ip", descriptor); + sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter>("sensor_ips", descriptor); + sensor_configuration.sensor_ips = this->get_parameter("sensor_ips").as_string_array(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("multicast_ip", descriptor); + sensor_configuration.multicast_ip = this->get_parameter("multicast_ip").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter>("frame_ids", descriptor); + sensor_configuration.frame_ids = this->get_parameter("frame_ids").as_string_array(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("base_frame", descriptor); + sensor_configuration.base_frame = this->get_parameter("base_frame").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("object_frame", descriptor); + sensor_configuration.object_frame = this->get_parameter("object_frame").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("data_port", descriptor); + sensor_configuration.data_port = this->get_parameter("data_port").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_host_port", descriptor); + sensor_configuration.configuration_host_port = + this->get_parameter("configuration_host_port").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_sensor_port", descriptor); + sensor_configuration.configuration_sensor_port = + this->get_parameter("configuration_sensor_port").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("use_sensor_time", descriptor); + sensor_configuration.use_sensor_time = this->get_parameter("use_sensor_time").as_bool(); + } + + if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + + RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); + return Status::OK; +} + +void MultiContinentalArs548HwInterfaceRosWrapper::ReceivePacketsDataCallback( + std::unique_ptr scan_buffer, const std::string & sensor_ip) +{ + packets_pub_map_[sensor_ip]->publish(std::move(scan_buffer)); +} + +rcl_interfaces::msg::SetParametersResult MultiContinentalArs548HwInterfaceRosWrapper::paramCallback( + const std::vector & p) +{ + std::scoped_lock lock(mtx_config_); + RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback"); + RCLCPP_DEBUG_STREAM(this->get_logger(), p); + RCLCPP_DEBUG_STREAM(this->get_logger(), sensor_configuration_); + RCLCPP_INFO_STREAM(this->get_logger(), p); + + drivers::continental_ars548::MultiContinentalArs548SensorConfiguration new_param{ + sensor_configuration_}; + RCLCPP_INFO_STREAM(this->get_logger(), new_param); + std::string sensor_model_str; + + if ( + get_param(p, "sensor_model", sensor_model_str) | get_param(p, "host_ip", new_param.host_ip) | + get_param(p, "sensor_ips", new_param.sensor_ips) | + get_param(p, "frame_ids", new_param.frame_ids) | + get_param(p, "data_port", new_param.data_port) | + get_param(p, "multicast_ip", new_param.multicast_ip) | + get_param(p, "base_frame", new_param.base_frame) | + get_param(p, "object_frame", new_param.object_frame) | + get_param(p, "configuration_host_port", new_param.configuration_host_port) | + get_param(p, "configuration_sensor_port", new_param.configuration_sensor_port) | + get_param(p, "configuration_host_port", new_param.configuration_host_port) | + get_param(p, "configuration_sensor_port", new_param.configuration_sensor_port)) { + if (0 < sensor_model_str.length()) + new_param.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_str); + + sensor_configuration_ = new_param; + RCLCPP_INFO_STREAM(this->get_logger(), "Update sensor_configuration"); + std::shared_ptr sensor_cfg_ptr = + std::make_shared( + sensor_configuration_); + RCLCPP_DEBUG_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); + hw_interface_.SetSensorConfiguration( + std::static_pointer_cast(sensor_cfg_ptr)); + hw_interface_.CheckAndSetConfig(); + } + + auto result = std::make_shared(); + result->successful = true; + result->reason = "success"; + + RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback success"); + + return *result; +} + +void MultiContinentalArs548HwInterfaceRosWrapper::OdometryCallback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) +{ + std::scoped_lock lock(mtx_config_); + + constexpr float speed_to_standstill = 0.5f; + constexpr float speed_to_moving = 2.f; + + if (standstill_ && std::abs(msg->twist.twist.linear.x) > speed_to_moving) { + standstill_ = false; + } else if (!standstill_ && std::abs(msg->twist.twist.linear.x) < speed_to_standstill) { + standstill_ = true; + } + + if (standstill_) { + hw_interface_.SetDrivingDirection(0); + } else { + hw_interface_.SetDrivingDirection(msg->twist.twist.linear.x > 0.f ? 1 : -1); + } + + constexpr float ms_to_kmh = 3.6f; + hw_interface_.SetVelocityVehicle(ms_to_kmh * std::abs(msg->twist.twist.linear.x)); + + constexpr float rad_to_deg = 180.f / M_PI; + hw_interface_.SetYawRate(rad_to_deg * msg->twist.twist.angular.z); +} + +void MultiContinentalArs548HwInterfaceRosWrapper::AccelerationCallback( + const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg) +{ + std::scoped_lock lock(mtx_config_); + hw_interface_.SetAccelerationLateralCog(msg->accel.accel.linear.y); + hw_interface_.SetAccelerationLongitudinalCog(msg->accel.accel.linear.x); +} + +void MultiContinentalArs548HwInterfaceRosWrapper::SteeringAngleCallback( + const std_msgs::msg::Float32::SharedPtr msg) +{ + std::scoped_lock lock(mtx_config_); + constexpr float rad_to_deg = 180.f / M_PI; + hw_interface_.SetSteeringAngleFrontAxle(rad_to_deg * msg->data); +} + +std::vector +MultiContinentalArs548HwInterfaceRosWrapper::updateParameters() +{ + std::scoped_lock lock(mtx_config_); + RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters start"); + std::ostringstream os_sensor_model; + os_sensor_model << sensor_configuration_.sensor_model; + RCLCPP_INFO_STREAM(this->get_logger(), "set_parameters"); + auto results = set_parameters( + {rclcpp::Parameter("sensor_model", os_sensor_model.str()), + rclcpp::Parameter("host_ip", sensor_configuration_.host_ip), + rclcpp::Parameter("sensor_ips", sensor_configuration_.sensor_ips), + rclcpp::Parameter("frame_ids", sensor_configuration_.frame_ids), + rclcpp::Parameter("data_port", sensor_configuration_.data_port), + rclcpp::Parameter("multicast_ip", sensor_configuration_.multicast_ip), + rclcpp::Parameter("base_frame", sensor_configuration_.base_frame), + rclcpp::Parameter("object_frame", sensor_configuration_.object_frame), + rclcpp::Parameter("configuration_host_port", sensor_configuration_.configuration_host_port), + rclcpp::Parameter( + "configuration_sensor_port", sensor_configuration_.configuration_sensor_port)}); + RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters end"); + return results; +} + +RCLCPP_COMPONENTS_REGISTER_NODE(MultiContinentalArs548HwInterfaceRosWrapper) +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/multi_continental_ars548_hw_interface_ros_wrapper.hpp b/nebula_ros/multi_continental_ars548_hw_interface_ros_wrapper.hpp new file mode 100644 index 000000000..ef7a42452 --- /dev/null +++ b/nebula_ros/multi_continental_ars548_hw_interface_ros_wrapper.hpp @@ -0,0 +1,146 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NEBULA_ContinentalArs548HwInterfaceRosWrapper_H +#define NEBULA_ContinentalArs548HwInterfaceRosWrapper_H + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace nebula +{ +namespace ros +{ +/// @brief Get parameter from rclcpp::Parameter +/// @tparam T +/// @param p Parameter from rclcpp parameter callback +/// @param name Target parameter name +/// @param value Corresponding value +/// @return Whether the target name existed +template +bool get_param(const std::vector & p, const std::string & name, T & value) +{ + auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { + return parameter.get_name() == name; + }); + if (it != p.cend()) { + value = it->template get_value(); + return true; + } + return false; +} + +/// @brief Hardware interface ros wrapper of continental radar ethernet driver +/// NOTE: this is a temporary class that acts as a single hw interface for multiple devices +/// The reason behind this is a not-so-efficient multicasting and package processing when having N +/// interfaces for N devices If we end up having problems because of that we may switch to this +/// implementation. Otherwise this implementation will be removed at a later date +class MultiContinentalArs548HwInterfaceRosWrapper final : public rclcpp::Node, + NebulaHwInterfaceWrapperBase +{ + drivers::continental_ars548::MultiContinentalArs548HwInterface hw_interface_; + Status interface_status_; + + drivers::continental_ars548::MultiContinentalArs548SensorConfiguration sensor_configuration_; + + /// @brief Received Continental Radar message publisher + std::unordered_map::SharedPtr> + packets_pub_map_; + rclcpp::Subscription::SharedPtr odometry_sub_; + rclcpp::Subscription::SharedPtr acceleration_sub_; + rclcpp::Subscription::SharedPtr steering_angle_sub_; + + bool standstill_{true}; + + /// @brief Initializing hardware interface ros wrapper + /// @param sensor_configuration SensorConfiguration for this driver + /// @return Resulting status + Status InitializeHwInterface( + const drivers::SensorConfigurationBase & sensor_configuration) override; + /// @brief Callback for receiving NebulaPackets + /// @param packets_buffer Received NebulaPackets + void ReceivePacketsDataCallback( + std::unique_ptr packets_buffer, const std::string & sensor_ip); + + /// @brief Callback to send the odometry information to the radar device + /// @param msg The odometry message + void OdometryCallback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); + + /// @brief Callback to send the acceleration information to the radar device + /// @param msg The acceleration message + void AccelerationCallback(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg); + + /// @brief Callback to send the steering angle information to the radar device + /// @param msg The steering angle message + void SteeringAngleCallback(const std_msgs::msg::Float32::SharedPtr msg); + +public: + explicit MultiContinentalArs548HwInterfaceRosWrapper(const rclcpp::NodeOptions & options); + ~MultiContinentalArs548HwInterfaceRosWrapper() noexcept override; + + /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) + /// @return Resulting status + Status StreamStart() override; + /// @brief Stop point cloud streaming (not used) + /// @return Resulting status + Status StreamStop() override; + /// @brief Shutdown (not used) + /// @return Resulting status + Status Shutdown() override; + /// @brief Get configurations from ros parameters + /// @param sensor_configuration Output of SensorConfiguration + /// @return Resulting status + Status GetParameters( + drivers::continental_ars548::MultiContinentalArs548SensorConfiguration & sensor_configuration); + +private: + std::mutex mtx_config_; + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + + /// @brief rclcpp parameter callback + /// @param parameters Received parameters + /// @return SetParametersResult + rcl_interfaces::msg::SetParametersResult paramCallback( + const std::vector & parameters); + + /// @brief Updating rclcpp parameter + /// @return SetParametersResult + std::vector updateParameters(); +}; + +} // namespace ros +} // namespace nebula + +#endif // NEBULA_ContinentalArs548HwInterfaceRosWrapper_H diff --git a/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp index f6d4e69ba..0f73db190 100644 --- a/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp @@ -102,6 +102,8 @@ Status ContinentalArs548DecoderWrapper::InitializeDriver( std::bind(&ContinentalArs548DecoderWrapper::ObjectListCallback, this, std::placeholders::_1)); driver_ptr_->RegisterSensorStatusCallback( std::bind(&ContinentalArs548DecoderWrapper::SensorStatusCallback, this, std::placeholders::_1)); + driver_ptr_->RegisterPacketsCallback( + std::bind(&ContinentalArs548DecoderWrapper::PacketsCallback, this, std::placeholders::_1)); return Status::OK; } @@ -118,17 +120,7 @@ void ContinentalArs548DecoderWrapper::OnConfigChange( void ContinentalArs548DecoderWrapper::ProcessPacket( std::unique_ptr packet_msg) { - driver_ptr_->ProcessPacket(*packet_msg); - - if ( - packets_pub_ && (packets_pub_->get_subscription_count() > 0 || - packets_pub_->get_intra_process_subscription_count() > 0)) { - auto nebula_packets_msg = std::make_unique(); - nebula_packets_msg->header.stamp = packet_msg->stamp; - nebula_packets_msg->header.frame_id = sensor_cfg_->frame_id; - nebula_packets_msg->packets.emplace_back(std::move(*packet_msg)); - packets_pub_->publish(std::move(nebula_packets_msg)); - } + driver_ptr_->ProcessPacket(std::move(packet_msg)); cloud_watchdog_->update(); } @@ -312,6 +304,16 @@ void ContinentalArs548DecoderWrapper::SensorStatusCallback( diagnostics_pub_->publish(diagnostic_array_msg); } +void ContinentalArs548DecoderWrapper::PacketsCallback( + std::unique_ptr msg) +{ + if ( + packets_pub_ && (packets_pub_->get_subscription_count() > 0 || + packets_pub_->get_intra_process_subscription_count() > 0)) { + packets_pub_->publish(std::move(msg)); + } +} + pcl::PointCloud::Ptr ContinentalArs548DecoderWrapper::ConvertToPointcloud( const continental_msgs::msg::ContinentalArs548DetectionList & msg) diff --git a/nebula_ros/src/continental/continental_srr520_decoder_ros_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_decoder_ros_wrapper.cpp new file mode 100644 index 000000000..9184a0cfd --- /dev/null +++ b/nebula_ros/src/continental/continental_srr520_decoder_ros_wrapper.cpp @@ -0,0 +1,646 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +namespace nebula +{ +namespace ros +{ +ContinentalSrr520DriverRosWrapper::ContinentalSrr520DriverRosWrapper( + const rclcpp::NodeOptions & options) +: rclcpp::Node("continental_srr520_driver_ros_wrapper", options), hw_interface_() +{ + drivers::continental_srr520::ContinentalSrr520SensorConfiguration sensor_configuration; + + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + + hw_interface_.SetLogger(std::make_shared(this->get_logger())); + + wrapper_status_ = GetParameters(sensor_configuration); + if (Status::OK != wrapper_status_) { + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); + return; + } + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); + + sensor_cfg_ptr_ = + std::make_shared( + sensor_configuration); + + wrapper_status_ = InitializeDriver( + std::const_pointer_cast( + sensor_cfg_ptr_)); + + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); + packets_sub_ = create_subscription( + "nebula_packets", rclcpp::SensorDataQoS(), + std::bind( + &ContinentalSrr520DriverRosWrapper::ReceivePacketsMsgCallback, this, std::placeholders::_1)); + + near_detection_list_pub_ = + this->create_publisher( + "near_continental_detections", rclcpp::SensorDataQoS()); + hrr_detection_list_pub_ = + this->create_publisher( + "hrr_continental_detections", rclcpp::SensorDataQoS()); + object_list_pub_ = this->create_publisher( + "continental_objects", rclcpp::SensorDataQoS()); + status_pub_ = this->create_publisher("diagnostics", 10); + + near_detection_pointcloud_pub_ = this->create_publisher( + "near_detection_points", rclcpp::SensorDataQoS()); + hrr_detection_pointcloud_pub_ = this->create_publisher( + "hrr_detection_points", rclcpp::SensorDataQoS()); + object_pointcloud_pub_ = + this->create_publisher("object_points", rclcpp::SensorDataQoS()); + + near_scan_raw_pub_ = + this->create_publisher("near_scan_raw", rclcpp::SensorDataQoS()); + hrr_scan_raw_pub_ = + this->create_publisher("hrr_scan_raw", rclcpp::SensorDataQoS()); + + objects_raw_pub_ = + this->create_publisher("objects_raw", rclcpp::SensorDataQoS()); + + objects_markers_pub_ = + this->create_publisher("marker_array", 10); +} + +void ContinentalSrr520DriverRosWrapper::ReceivePacketsMsgCallback( + const nebula_msgs::msg::NebulaPackets::SharedPtr scan_msg) +{ + decoder_ptr_->ProcessPackets(*scan_msg); +} + +Status ContinentalSrr520DriverRosWrapper::InitializeDriver( + std::shared_ptr sensor_configuration) +{ + decoder_ptr_ = std::make_shared( + std::static_pointer_cast( + sensor_configuration)); + + decoder_ptr_->RegisterNearDetectionListCallback(std::bind( + &ContinentalSrr520DriverRosWrapper::NearDetectionListCallback, this, std::placeholders::_1)); + decoder_ptr_->RegisterHRRDetectionListCallback(std::bind( + &ContinentalSrr520DriverRosWrapper::HRRDetectionListCallback, this, std::placeholders::_1)); + decoder_ptr_->RegisterObjectListCallback( + std::bind(&ContinentalSrr520DriverRosWrapper::ObjectListCallback, this, std::placeholders::_1)); + decoder_ptr_->RegisterStatusCallback( + std::bind(&ContinentalSrr520DriverRosWrapper::StatusCallback, this, std::placeholders::_1)); + + return Status::OK; +} + +Status ContinentalSrr520DriverRosWrapper::GetStatus() +{ + return wrapper_status_; +} + +Status ContinentalSrr520DriverRosWrapper::GetParameters( + drivers::continental_srr520::ContinentalSrr520SensorConfiguration & sensor_configuration) +{ + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("sensor_model", descriptor); + sensor_configuration.sensor_model = + nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("interface", descriptor); + sensor_configuration.interface = this->get_parameter("interface").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("receiver_timeout_sec", descriptor); + sensor_configuration.receiver_timeout_sec = + this->get_parameter("receiver_timeout_sec").as_double(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("sender_timeout_sec", descriptor); + sensor_configuration.sender_timeout_sec = this->get_parameter("sender_timeout_sec").as_double(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("filters", descriptor); + sensor_configuration.filters = this->get_parameter("filters").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("frame_id", descriptor); + sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("base_frame", descriptor); + sensor_configuration.base_frame = this->get_parameter("base_frame").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("use_bus_time", descriptor); + sensor_configuration.use_bus_time = this->get_parameter("use_bus_time").as_bool(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_vehicle_wheelbase", descriptor); + sensor_configuration.configuration_vehicle_wheelbase = + static_cast(this->get_parameter("configuration_vehicle_wheelbase").as_double()); + } + + if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + + if (sensor_configuration.frame_id.empty()) { + return Status::SENSOR_CONFIG_ERROR; + } + + RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); + return Status::OK; +} + +void ContinentalSrr520DriverRosWrapper::NearDetectionListCallback( + std::unique_ptr msg) +{ + if ( + near_detection_pointcloud_pub_->get_subscription_count() > 0 || + near_detection_pointcloud_pub_->get_intra_process_subscription_count() > 0) { + const auto detection_pointcloud_ptr = ConvertToPointcloud(*msg); + auto detection_pointcloud_msg_ptr = std::make_unique(); + pcl::toROSMsg(*detection_pointcloud_ptr, *detection_pointcloud_msg_ptr); + + detection_pointcloud_msg_ptr->header = msg->header; + near_detection_pointcloud_pub_->publish(std::move(detection_pointcloud_msg_ptr)); + } + + if ( + near_scan_raw_pub_->get_subscription_count() > 0 || + near_scan_raw_pub_->get_intra_process_subscription_count() > 0) { + auto radar_scan_msg = ConvertToRadarScan(*msg); + radar_scan_msg.header = msg->header; + near_scan_raw_pub_->publish(std::move(radar_scan_msg)); + } + + if ( + near_detection_list_pub_->get_subscription_count() > 0 || + near_detection_list_pub_->get_intra_process_subscription_count() > 0) { + near_detection_list_pub_->publish(std::move(msg)); + } +} + +void ContinentalSrr520DriverRosWrapper::HRRDetectionListCallback( + std::unique_ptr msg) +{ + if ( + hrr_detection_pointcloud_pub_->get_subscription_count() > 0 || + hrr_detection_pointcloud_pub_->get_intra_process_subscription_count() > 0) { + const auto detection_pointcloud_ptr = ConvertToPointcloud(*msg); + auto detection_pointcloud_msg_ptr = std::make_unique(); + pcl::toROSMsg(*detection_pointcloud_ptr, *detection_pointcloud_msg_ptr); + + detection_pointcloud_msg_ptr->header = msg->header; + hrr_detection_pointcloud_pub_->publish(std::move(detection_pointcloud_msg_ptr)); + } + + if ( + hrr_scan_raw_pub_->get_subscription_count() > 0 || + hrr_scan_raw_pub_->get_intra_process_subscription_count() > 0) { + auto radar_scan_msg = ConvertToRadarScan(*msg); + radar_scan_msg.header = msg->header; + hrr_scan_raw_pub_->publish(std::move(radar_scan_msg)); + } + + if ( + hrr_detection_list_pub_->get_subscription_count() > 0 || + hrr_detection_list_pub_->get_intra_process_subscription_count() > 0) { + hrr_detection_list_pub_->publish(std::move(msg)); + } +} + +void ContinentalSrr520DriverRosWrapper::ObjectListCallback( + std::unique_ptr msg) +{ + if ( + object_pointcloud_pub_->get_subscription_count() > 0 || + object_pointcloud_pub_->get_intra_process_subscription_count() > 0) { + const auto object_pointcloud_ptr = ConvertToPointcloud(*msg); + auto object_pointcloud_msg_ptr = std::make_unique(); + pcl::toROSMsg(*object_pointcloud_ptr, *object_pointcloud_msg_ptr); + + object_pointcloud_msg_ptr->header = msg->header; + object_pointcloud_pub_->publish(std::move(object_pointcloud_msg_ptr)); + } + + if ( + objects_raw_pub_->get_subscription_count() > 0 || + objects_raw_pub_->get_intra_process_subscription_count() > 0) { + auto objects_raw_msg = ConvertToRadarTracks(*msg); + objects_raw_msg.header = msg->header; + objects_raw_pub_->publish(std::move(objects_raw_msg)); + } + + if ( + objects_markers_pub_->get_subscription_count() > 0 || + objects_markers_pub_->get_intra_process_subscription_count() > 0) { + auto marker_array_msg = ConvertToMarkers(*msg); + objects_markers_pub_->publish(std::move(marker_array_msg)); + } + + if ( + object_list_pub_->get_subscription_count() > 0 || + object_list_pub_->get_intra_process_subscription_count() > 0) { + object_list_pub_->publish(std::move(msg)); + } +} + +void ContinentalSrr520DriverRosWrapper::StatusCallback( + std::unique_ptr status_msg_ptr) +{ + status_pub_->publish(std::move(status_msg_ptr)); +} + +pcl::PointCloud::Ptr +ContinentalSrr520DriverRosWrapper::ConvertToPointcloud( + const continental_msgs::msg::ContinentalSrr520DetectionList & msg) +{ + pcl::PointCloud::Ptr output_pointcloud( + new pcl::PointCloud); + output_pointcloud->reserve(msg.detections.size()); + + nebula::drivers::continental_srr520::PointSrr520Detection point{}; + for (const auto & detection : msg.detections) { + point.x = std::cos(detection.azimuth_angle) * detection.range; + point.y = std::sin(detection.azimuth_angle) * detection.range; + point.z = 0.f; + + point.azimuth = detection.azimuth_angle; + point.range = detection.range; + point.range_rate = detection.range_rate; + point.rcs = detection.rcs; + point.snr = detection.snr; + point.pdh00 = detection.pdh00; + point.pdh01 = detection.pdh01; + point.pdh02 = detection.pdh02; + point.pdh03 = detection.pdh03; + point.pdh04 = detection.pdh04; + point.pdh05 = detection.pdh05; + + output_pointcloud->points.emplace_back(point); + } + + output_pointcloud->height = 1; + output_pointcloud->width = output_pointcloud->points.size(); + return output_pointcloud; +} + +pcl::PointCloud::Ptr +ContinentalSrr520DriverRosWrapper::ConvertToPointcloud( + const continental_msgs::msg::ContinentalSrr520ObjectList & msg) +{ + pcl::PointCloud::Ptr output_pointcloud( + new pcl::PointCloud); + output_pointcloud->reserve(msg.objects.size()); + + nebula::drivers::continental_srr520::PointSrr520Object point{}; + for (const auto & object : msg.objects) { + point.x = object.dist_x; + point.y = object.dist_y; + point.z = 0.f; + + point.id = object.object_id; + point.age = object.life_cycles; + + point.box_length = object.box_length; + point.box_width = object.box_width; + + point.object_status = object.object_status; + point.orientation = object.orientation; + point.rcs = object.rcs; + point.score = object.score; + point.dynamics_abs_vel_x = object.v_abs_x; + point.dynamics_abs_vel_y = object.v_abs_y; + point.dynamics_abs_acc_x = object.a_abs_x; + point.dynamics_abs_acc_y = object.a_abs_y; + + output_pointcloud->points.emplace_back(point); + } + + output_pointcloud->height = 1; + output_pointcloud->width = output_pointcloud->points.size(); + return output_pointcloud; +} + +radar_msgs::msg::RadarScan ContinentalSrr520DriverRosWrapper::ConvertToRadarScan( + const continental_msgs::msg::ContinentalSrr520DetectionList & msg) +{ + radar_msgs::msg::RadarScan output_msg; + output_msg.header = msg.header; + output_msg.returns.reserve(msg.detections.size()); + + radar_msgs::msg::RadarReturn return_msg; + for (const auto & detection : msg.detections) { + if (detection.pdh00 > 0 || detection.pdh00 > 1 || detection.pdh02 > 0) { + continue; + } + + return_msg.range = detection.range; + return_msg.azimuth = detection.azimuth_angle; + return_msg.elevation = 0.f; + return_msg.doppler_velocity = detection.range_rate; + return_msg.amplitude = detection.rcs; + output_msg.returns.emplace_back(return_msg); + } + + return output_msg; +} + +radar_msgs::msg::RadarTracks ContinentalSrr520DriverRosWrapper::ConvertToRadarTracks( + const continental_msgs::msg::ContinentalSrr520ObjectList & msg) +{ + radar_msgs::msg::RadarTracks output_msg; + output_msg.tracks.reserve(msg.objects.size()); + output_msg.header = msg.header; + + constexpr int16_t UNKNOWN_ID = 32000; + constexpr float INVALID_COVARIANCE = 1e6; + + radar_msgs::msg::RadarTrack track_msg; + for (const auto & object : msg.objects) { + if (!object.box_valid || object.object_status == 0) { + continue; + } + + track_msg.uuid.uuid[0] = static_cast(object.object_id & 0xff); + track_msg.uuid.uuid[1] = static_cast((object.object_id >> 8) & 0xff); + track_msg.uuid.uuid[2] = static_cast((object.object_id >> 16) & 0xff); + track_msg.uuid.uuid[3] = static_cast((object.object_id >> 24) & 0xff); + + track_msg.position.x = static_cast(object.dist_x); + track_msg.position.y = static_cast(object.dist_y); + track_msg.position.z = 0.0; + + track_msg.velocity.x = static_cast(object.v_abs_x); + track_msg.velocity.y = static_cast(object.v_abs_y); + track_msg.velocity.z = 0.0; + track_msg.acceleration.x = static_cast(object.a_abs_x); + track_msg.acceleration.y = static_cast(object.a_abs_y); + track_msg.acceleration.z = 0.0; + track_msg.size.x = object.box_length; + track_msg.size.y = object.box_width; + track_msg.size.z = 1.f; + + track_msg.classification = UNKNOWN_ID; + + track_msg.position_covariance[0] = object.dist_x_std * object.dist_x_std; + track_msg.position_covariance[1] = INVALID_COVARIANCE; + track_msg.position_covariance[2] = 0.f; + track_msg.position_covariance[3] = object.dist_y_std * object.dist_y_std; + track_msg.position_covariance[4] = 0.f; + track_msg.position_covariance[5] = INVALID_COVARIANCE; + + track_msg.velocity_covariance[0] = object.v_abs_x_std * object.v_abs_x_std; + track_msg.velocity_covariance[1] = INVALID_COVARIANCE; + track_msg.velocity_covariance[2] = 0.f; + track_msg.velocity_covariance[3] = object.v_abs_y_std * object.v_abs_y_std; + track_msg.velocity_covariance[4] = 0.f; + track_msg.velocity_covariance[5] = INVALID_COVARIANCE; + + track_msg.acceleration_covariance[0] = object.a_abs_x_std * object.a_abs_x_std; + track_msg.acceleration_covariance[1] = INVALID_COVARIANCE; + track_msg.acceleration_covariance[2] = 0.f; + track_msg.acceleration_covariance[3] = object.a_abs_y_std * object.a_abs_y_std; + track_msg.acceleration_covariance[4] = 0.f; + track_msg.acceleration_covariance[5] = INVALID_COVARIANCE; + + track_msg.size_covariance[0] = INVALID_COVARIANCE; + track_msg.size_covariance[1] = 0.f; + track_msg.size_covariance[2] = 0.f; + track_msg.size_covariance[3] = INVALID_COVARIANCE; + track_msg.size_covariance[4] = 0.f; + track_msg.size_covariance[5] = INVALID_COVARIANCE; + + output_msg.tracks.emplace_back(track_msg); + } + + return output_msg; +} + +visualization_msgs::msg::MarkerArray ContinentalSrr520DriverRosWrapper::ConvertToMarkers( + const continental_msgs::msg::ContinentalSrr520ObjectList & msg) +{ + visualization_msgs::msg::MarkerArray marker_array; + marker_array.markers.reserve(4 * msg.objects.size()); + + constexpr int LINE_STRIP_CORNERS_NUM = 17; + constexpr std::array, LINE_STRIP_CORNERS_NUM> cube_corners = { + {{{-1.0, -1.0, -1.0}}, + {{-1.0, -1.0, 1.0}}, + {{-1.0, 1.0, 1.0}}, + {{-1.0, 1.0, -1.0}}, + {{-1.0, -1.0, -1.0}}, + {{1.0, -1.0, -1.0}}, + {{1.0, -1.0, 1.0}}, + {{1.0, 1.0, 1.0}}, + {{1.0, 1.0, -1.0}}, + {{1.0, -1.0, -1.0}}, + {{-1.0, -1.0, -1.0}}, + {{-1.0, -1.0, 1.0}}, + {{1.0, -1.0, 1.0}}, + {{1.0, 1.0, 1.0}}, + {{-1.0, 1.0, 1.0}}, + {{-1.0, 1.0, -1.0}}, + {{1.0, 1.0, -1.0}}}}; + + constexpr int PALETTE_SIZE = 32; + constexpr std::array, PALETTE_SIZE> color_array = {{ + {{1.0, 0.0, 0.0}}, {{0.0, 1.0, 0.0}}, + {{0.0, 0.0, 1.0}}, // Red, Green, Blue + {{1.0, 1.0, 0.0}}, {{0.0, 1.0, 1.0}}, + {{1.0, 0.0, 1.0}}, // Yellow, Cyan, Magenta + {{1.0, 0.647, 0.0}}, {{0.749, 1.0, 0.0}}, + {{0.0, 0.502, 0.502}}, // Orange, Lime, Teal + {{0.502, 0.0, 0.502}}, {{1.0, 0.753, 0.796}}, + {{0.647, 0.165, 0.165}}, // Purple, Pink, Brown + {{0.502, 0.0, 0.0}}, {{0.502, 0.502, 0.0}}, + {{0.0, 0.0, 0.502}}, // Maroon, Olive, Navy + {{0.502, 0.502, 0.502}}, {{1.0, 0.4, 0.4}}, + {{0.4, 1.0, 0.4}}, // Grey, Light Red, Light Green + {{0.4, 0.4, 1.0}}, {{1.0, 1.0, 0.4}}, + {{0.4, 1.0, 1.0}}, // Light Blue, Light Yellow, Light Cyan + {{1.0, 0.4, 1.0}}, {{1.0, 0.698, 0.4}}, + {{0.698, 0.4, 1.0}}, // Light Magenta, Light Orange, Light Purple + {{1.0, 0.6, 0.8}}, {{0.71, 0.396, 0.114}}, + {{0.545, 0.0, 0.0}}, // Light Pink, Light Brown, Dark Red + {{0.0, 0.392, 0.0}}, {{0.0, 0.0, 0.545}}, + {{0.545, 0.545, 0.0}}, // Dark Green, Dark Blue, Dark Yellow + {{0.0, 0.545, 0.545}}, {{0.545, 0.0, 0.545}} // Dark Cyan, Dark Magenta + }}; + + std::unordered_set current_ids; + + for (const auto & object : msg.objects) { + if (!object.box_valid || object.object_status == 0) { + continue; + } + + const double half_length = 0.5 * object.box_length; + const double half_width = 0.5 * object.box_width; + constexpr double DEFAULT_HALF_SIZE = 1.0; + + const double & yaw = object.orientation; + current_ids.emplace(object.object_id); + + visualization_msgs::msg::Marker box_marker; + box_marker.header.frame_id = sensor_cfg_ptr_->base_frame; + box_marker.header.stamp = msg.header.stamp; + box_marker.ns = "boxes"; + box_marker.id = object.object_id; + box_marker.action = visualization_msgs::msg::Marker::ADD; + box_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + box_marker.lifetime = rclcpp::Duration::from_seconds(0); + box_marker.color.r = color_array[object.object_id % PALETTE_SIZE][0]; + box_marker.color.g = color_array[object.object_id % PALETTE_SIZE][1]; + box_marker.color.b = color_array[object.object_id % PALETTE_SIZE][2]; + box_marker.color.a = 1.0; + box_marker.scale.x = 0.1; + + box_marker.pose.position.x = object.dist_x; + box_marker.pose.position.y = object.dist_y; + box_marker.pose.position.z = DEFAULT_HALF_SIZE; + box_marker.pose.orientation.w = std::cos(0.5 * yaw); + box_marker.pose.orientation.z = std::sin(0.5 * yaw); + + for (const auto & corner : cube_corners) { + geometry_msgs::msg::Point p; + p.x = half_length * corner[0]; + p.y = half_width * corner[1]; + p.z = DEFAULT_HALF_SIZE * corner[2]; + box_marker.points.emplace_back(p); + } + + marker_array.markers.emplace_back(box_marker); + + visualization_msgs::msg::Marker text_marker = box_marker; + text_marker.ns = "object_age"; + text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + text_marker.color.r = 1.0; + text_marker.color.g = 1.0; + text_marker.color.b = 1.0; + text_marker.color.a = 1.0; + text_marker.scale.x = 0.3; + text_marker.scale.y = 0.3; + text_marker.scale.z = 0.3; + text_marker.pose.position.z += 0.5; + text_marker.text = "ID=" + std::to_string(object.object_id) + + " Age=" + std::to_string(object.life_cycles) + "ms"; + + marker_array.markers.emplace_back(text_marker); + + std::stringstream object_status_ss; + object_status_ss << std::fixed << std::setprecision(3) + << "ID=" << static_cast(object.object_id) << "\n" + << static_cast(object.box_length) << "/" + << static_cast(object.object_status); + + text_marker.ns = "object_status"; + text_marker.text = object_status_ss.str(); + + marker_array.markers.emplace_back(text_marker); + + std::stringstream object_dynamics_ss; + object_dynamics_ss << std::fixed << std::setprecision(3) + << "ID=" << static_cast(object.object_id) + << "\nyaw=" << object.orientation << "\nvx=" << object.v_abs_x + << "\nvy=" << object.v_abs_y << "\nax=" << object.a_abs_x + << "\nay=" << object.a_abs_y; + + text_marker.ns = "object_dynamics"; + text_marker.text = object_dynamics_ss.str(); + + marker_array.markers.emplace_back(text_marker); + } + + for (const auto & previous_id : previous_ids_) { + if (current_ids.find(previous_id) != current_ids.end()) { + continue; + } + + visualization_msgs::msg::Marker delete_marker; + delete_marker.header.frame_id = sensor_cfg_ptr_->base_frame; + delete_marker.header.stamp = msg.header.stamp; + delete_marker.ns = "boxes"; + delete_marker.id = previous_id; + delete_marker.action = visualization_msgs::msg::Marker::DELETE; + + marker_array.markers.push_back(delete_marker); + + delete_marker.ns = "object_age"; + marker_array.markers.push_back(delete_marker); + + delete_marker.ns = "object_status"; + marker_array.markers.push_back(delete_marker); + + delete_marker.ns = "object_dynamics"; + marker_array.markers.push_back(delete_marker); + } + + previous_ids_.clear(); + previous_ids_ = current_ids; + + return marker_array; +} + +RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalSrr520DriverRosWrapper) +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp new file mode 100644 index 000000000..6222c6aff --- /dev/null +++ b/nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp @@ -0,0 +1,603 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +namespace nebula +{ +namespace ros +{ +ContinentalSrr520DecoderWrapper::ContinentalSrr520DecoderWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr & + config, + std::shared_ptr hw_interface_ptr) +: parent_node_(parent_node), + status_(nebula::Status::NOT_INITIALIZED), + logger_(parent_node->get_logger().get_child("ContinentalSrr520Decoder")), + sensor_cfg_(config), + hw_interface_ptr_(hw_interface_ptr) +{ + using std::chrono_literals::operator""us; + if (!config) { + throw std::runtime_error( + "ContinentalSrr520DecoderWrapper cannot be instantiated without a valid config!"); + } + + RCLCPP_INFO(logger_, "Starting Decoder"); + + InitializeDriver(config); + status_ = driver_ptr_->GetStatus(); + + if (Status::OK != status_) { + throw std::runtime_error( + (std::stringstream() << "Error instantiating decoder: " << status_).str()); + } + + // Publish packets only if HW interface is connected + if (hw_interface_ptr_) { + packets_pub_ = parent_node->create_publisher( + "nebula_packets", rclcpp::SensorDataQoS()); + } + + auto qos_profile = rmw_qos_profile_sensor_data; + auto pointcloud_qos = + rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); + + near_detection_list_pub_ = + parent_node->create_publisher( + "near_continental_detections", pointcloud_qos); + hrr_detection_list_pub_ = + parent_node->create_publisher( + "hrr_continental_detections", pointcloud_qos); + object_list_pub_ = + parent_node->create_publisher( + "continental_objects", pointcloud_qos); + status_pub_ = + parent_node->create_publisher("diagnostics", 10); + + near_detection_pointcloud_pub_ = parent_node->create_publisher( + "near_detection_points", pointcloud_qos); + hrr_detection_pointcloud_pub_ = parent_node->create_publisher( + "hrr_detection_points", pointcloud_qos); + object_pointcloud_pub_ = + parent_node->create_publisher("object_points", pointcloud_qos); + + near_scan_raw_pub_ = + parent_node->create_publisher("near_scan_raw", pointcloud_qos); + hrr_scan_raw_pub_ = + parent_node->create_publisher("hrr_scan_raw", pointcloud_qos); + + objects_raw_pub_ = + parent_node->create_publisher("objects_raw", pointcloud_qos); + + objects_markers_pub_ = + parent_node->create_publisher("marker_array", 10); + + RCLCPP_INFO_STREAM(logger_, ". Wrapper=" << status_); + + cloud_watchdog_ = + std::make_shared(*parent_node, 100'000us, [this, parent_node](bool ok) { + if (ok) return; + RCLCPP_WARN_THROTTLE( + logger_, *parent_node->get_clock(), 5000, "Missed sensor output deadline"); + }); +} + +Status ContinentalSrr520DecoderWrapper::InitializeDriver( + const std::shared_ptr< + const nebula::drivers::continental_srr520::ContinentalSrr520SensorConfiguration> & config) +{ + driver_ptr_.reset(); + driver_ptr_ = std::make_shared(config); + + driver_ptr_->RegisterNearDetectionListCallback(std::bind( + &ContinentalSrr520DecoderWrapper::NearDetectionListCallback, this, std::placeholders::_1)); + driver_ptr_->RegisterHRRDetectionListCallback(std::bind( + &ContinentalSrr520DecoderWrapper::HRRDetectionListCallback, this, std::placeholders::_1)); + driver_ptr_->RegisterObjectListCallback( + std::bind(&ContinentalSrr520DecoderWrapper::ObjectListCallback, this, std::placeholders::_1)); + driver_ptr_->RegisterStatusCallback( + std::bind(&ContinentalSrr520DecoderWrapper::StatusCallback, this, std::placeholders::_1)); + + if (hw_interface_ptr_) { + driver_ptr_->RegisterSyncFupCallback( + std::bind(&ContinentalSrr520DecoderWrapper::SyncFupCallback, this)); + driver_ptr_->RegisterPacketsCallback( + std::bind(&ContinentalSrr520DecoderWrapper::PacketsCallback, this, std::placeholders::_1)); + } + + driver_ptr_->SetLogger( + std::make_shared(parent_node_->get_logger().get_child("Driver"))); + + return Status::OK; +} + +void ContinentalSrr520DecoderWrapper::OnConfigChange( + const std::shared_ptr< + const nebula::drivers::continental_srr520::ContinentalSrr520SensorConfiguration> & new_config) +{ + std::lock_guard lock(mtx_driver_ptr_); + InitializeDriver(new_config); + sensor_cfg_ = new_config; +} + +void ContinentalSrr520DecoderWrapper::ProcessPacket( + std::unique_ptr packet_msg) +{ + driver_ptr_->ProcessPacket(std::move(packet_msg)); + + cloud_watchdog_->update(); +} + +void ContinentalSrr520DecoderWrapper::NearDetectionListCallback( + std::unique_ptr msg) +{ + if ( + near_detection_pointcloud_pub_->get_subscription_count() > 0 || + near_detection_pointcloud_pub_->get_intra_process_subscription_count() > 0) { + const auto detection_pointcloud_ptr = ConvertToPointcloud(*msg); + auto detection_pointcloud_msg_ptr = std::make_unique(); + pcl::toROSMsg(*detection_pointcloud_ptr, *detection_pointcloud_msg_ptr); + + detection_pointcloud_msg_ptr->header = msg->header; + near_detection_pointcloud_pub_->publish(std::move(detection_pointcloud_msg_ptr)); + } + + if ( + near_scan_raw_pub_->get_subscription_count() > 0 || + near_scan_raw_pub_->get_intra_process_subscription_count() > 0) { + auto radar_scan_msg = ConvertToRadarScan(*msg); + radar_scan_msg.header = msg->header; + near_scan_raw_pub_->publish(std::move(radar_scan_msg)); + } + + if ( + near_detection_list_pub_->get_subscription_count() > 0 || + near_detection_list_pub_->get_intra_process_subscription_count() > 0) { + near_detection_list_pub_->publish(std::move(msg)); + } +} + +void ContinentalSrr520DecoderWrapper::HRRDetectionListCallback( + std::unique_ptr msg) +{ + if ( + hrr_detection_pointcloud_pub_->get_subscription_count() > 0 || + hrr_detection_pointcloud_pub_->get_intra_process_subscription_count() > 0) { + const auto detection_pointcloud_ptr = ConvertToPointcloud(*msg); + auto detection_pointcloud_msg_ptr = std::make_unique(); + pcl::toROSMsg(*detection_pointcloud_ptr, *detection_pointcloud_msg_ptr); + + detection_pointcloud_msg_ptr->header = msg->header; + hrr_detection_pointcloud_pub_->publish(std::move(detection_pointcloud_msg_ptr)); + } + + if ( + hrr_scan_raw_pub_->get_subscription_count() > 0 || + hrr_scan_raw_pub_->get_intra_process_subscription_count() > 0) { + auto radar_scan_msg = ConvertToRadarScan(*msg); + radar_scan_msg.header = msg->header; + hrr_scan_raw_pub_->publish(std::move(radar_scan_msg)); + } + + if ( + hrr_detection_list_pub_->get_subscription_count() > 0 || + hrr_detection_list_pub_->get_intra_process_subscription_count() > 0) { + hrr_detection_list_pub_->publish(std::move(msg)); + } +} + +void ContinentalSrr520DecoderWrapper::ObjectListCallback( + std::unique_ptr msg) +{ + if ( + object_pointcloud_pub_->get_subscription_count() > 0 || + object_pointcloud_pub_->get_intra_process_subscription_count() > 0) { + const auto object_pointcloud_ptr = ConvertToPointcloud(*msg); + auto object_pointcloud_msg_ptr = std::make_unique(); + pcl::toROSMsg(*object_pointcloud_ptr, *object_pointcloud_msg_ptr); + + object_pointcloud_msg_ptr->header = msg->header; + object_pointcloud_pub_->publish(std::move(object_pointcloud_msg_ptr)); + } + + if ( + objects_raw_pub_->get_subscription_count() > 0 || + objects_raw_pub_->get_intra_process_subscription_count() > 0) { + auto objects_raw_msg = ConvertToRadarTracks(*msg); + objects_raw_msg.header = msg->header; + objects_raw_pub_->publish(std::move(objects_raw_msg)); + } + + if ( + objects_markers_pub_->get_subscription_count() > 0 || + objects_markers_pub_->get_intra_process_subscription_count() > 0) { + auto marker_array_msg = ConvertToMarkers(*msg); + objects_markers_pub_->publish(std::move(marker_array_msg)); + } + + if ( + object_list_pub_->get_subscription_count() > 0 || + object_list_pub_->get_intra_process_subscription_count() > 0) { + object_list_pub_->publish(std::move(msg)); + } +} + +void ContinentalSrr520DecoderWrapper::StatusCallback( + std::unique_ptr status_msg_ptr) +{ + status_pub_->publish(std::move(status_msg_ptr)); +} + +pcl::PointCloud::Ptr +ContinentalSrr520DecoderWrapper::ConvertToPointcloud( + const continental_msgs::msg::ContinentalSrr520DetectionList & msg) +{ + pcl::PointCloud::Ptr output_pointcloud( + new pcl::PointCloud); + output_pointcloud->reserve(msg.detections.size()); + + nebula::drivers::continental_srr520::PointSrr520Detection point{}; + for (const auto & detection : msg.detections) { + point.x = std::cos(detection.azimuth_angle) * detection.range; + point.y = std::sin(detection.azimuth_angle) * detection.range; + point.z = 0.f; + + point.azimuth = detection.azimuth_angle; + point.range = detection.range; + point.range_rate = detection.range_rate; + point.rcs = detection.rcs; + point.snr = detection.snr; + point.pdh00 = detection.pdh00; + point.pdh01 = detection.pdh01; + point.pdh02 = detection.pdh02; + point.pdh03 = detection.pdh03; + point.pdh04 = detection.pdh04; + point.pdh05 = detection.pdh05; + + output_pointcloud->points.emplace_back(point); + } + + output_pointcloud->height = 1; + output_pointcloud->width = output_pointcloud->points.size(); + return output_pointcloud; +} + +pcl::PointCloud::Ptr +ContinentalSrr520DecoderWrapper::ConvertToPointcloud( + const continental_msgs::msg::ContinentalSrr520ObjectList & msg) +{ + pcl::PointCloud::Ptr output_pointcloud( + new pcl::PointCloud); + output_pointcloud->reserve(msg.objects.size()); + + nebula::drivers::continental_srr520::PointSrr520Object point{}; + for (const auto & object : msg.objects) { + point.x = object.dist_x; + point.y = object.dist_y; + point.z = 0.f; + + point.id = object.object_id; + point.age = object.life_cycles; + + point.box_length = object.box_length; + point.box_width = object.box_width; + + point.object_status = object.object_status; + point.orientation = object.orientation; + point.rcs = object.rcs; + point.score = object.score; + point.dynamics_abs_vel_x = object.v_abs_x; + point.dynamics_abs_vel_y = object.v_abs_y; + point.dynamics_abs_acc_x = object.a_abs_x; + point.dynamics_abs_acc_y = object.a_abs_y; + + output_pointcloud->points.emplace_back(point); + } + + output_pointcloud->height = 1; + output_pointcloud->width = output_pointcloud->points.size(); + return output_pointcloud; +} + +radar_msgs::msg::RadarScan ContinentalSrr520DecoderWrapper::ConvertToRadarScan( + const continental_msgs::msg::ContinentalSrr520DetectionList & msg) +{ + radar_msgs::msg::RadarScan output_msg; + output_msg.header = msg.header; + output_msg.returns.reserve(msg.detections.size()); + + radar_msgs::msg::RadarReturn return_msg; + for (const auto & detection : msg.detections) { + if (detection.pdh00 > 0 || detection.pdh00 > 1 || detection.pdh02 > 0) { + continue; + } + + return_msg.range = detection.range; + return_msg.azimuth = detection.azimuth_angle; + return_msg.elevation = 0.f; + return_msg.doppler_velocity = detection.range_rate; + return_msg.amplitude = detection.rcs; + output_msg.returns.emplace_back(return_msg); + } + + return output_msg; +} + +radar_msgs::msg::RadarTracks ContinentalSrr520DecoderWrapper::ConvertToRadarTracks( + const continental_msgs::msg::ContinentalSrr520ObjectList & msg) +{ + radar_msgs::msg::RadarTracks output_msg; + output_msg.tracks.reserve(msg.objects.size()); + output_msg.header = msg.header; + + constexpr int16_t UNKNOWN_ID = 32000; + constexpr float INVALID_COVARIANCE = 1e6; + + radar_msgs::msg::RadarTrack track_msg; + for (const auto & object : msg.objects) { + if (!object.box_valid || object.object_status == 0) { + continue; + } + + track_msg.uuid.uuid[0] = static_cast(object.object_id & 0xff); + track_msg.uuid.uuid[1] = static_cast((object.object_id >> 8) & 0xff); + track_msg.uuid.uuid[2] = static_cast((object.object_id >> 16) & 0xff); + track_msg.uuid.uuid[3] = static_cast((object.object_id >> 24) & 0xff); + + track_msg.position.x = static_cast(object.dist_x); + track_msg.position.y = static_cast(object.dist_y); + track_msg.position.z = 0.0; + + track_msg.velocity.x = static_cast(object.v_abs_x); + track_msg.velocity.y = static_cast(object.v_abs_y); + track_msg.velocity.z = 0.0; + track_msg.acceleration.x = static_cast(object.a_abs_x); + track_msg.acceleration.y = static_cast(object.a_abs_y); + track_msg.acceleration.z = 0.0; + track_msg.size.x = object.box_length; + track_msg.size.y = object.box_width; + track_msg.size.z = 1.f; + + track_msg.classification = UNKNOWN_ID; + + track_msg.position_covariance[0] = object.dist_x_std * object.dist_x_std; + track_msg.position_covariance[1] = INVALID_COVARIANCE; + track_msg.position_covariance[2] = 0.f; + track_msg.position_covariance[3] = object.dist_y_std * object.dist_y_std; + track_msg.position_covariance[4] = 0.f; + track_msg.position_covariance[5] = INVALID_COVARIANCE; + + track_msg.velocity_covariance[0] = object.v_abs_x_std * object.v_abs_x_std; + track_msg.velocity_covariance[1] = INVALID_COVARIANCE; + track_msg.velocity_covariance[2] = 0.f; + track_msg.velocity_covariance[3] = object.v_abs_y_std * object.v_abs_y_std; + track_msg.velocity_covariance[4] = 0.f; + track_msg.velocity_covariance[5] = INVALID_COVARIANCE; + + track_msg.acceleration_covariance[0] = object.a_abs_x_std * object.a_abs_x_std; + track_msg.acceleration_covariance[1] = INVALID_COVARIANCE; + track_msg.acceleration_covariance[2] = 0.f; + track_msg.acceleration_covariance[3] = object.a_abs_y_std * object.a_abs_y_std; + track_msg.acceleration_covariance[4] = 0.f; + track_msg.acceleration_covariance[5] = INVALID_COVARIANCE; + + track_msg.size_covariance[0] = INVALID_COVARIANCE; + track_msg.size_covariance[1] = 0.f; + track_msg.size_covariance[2] = 0.f; + track_msg.size_covariance[3] = INVALID_COVARIANCE; + track_msg.size_covariance[4] = 0.f; + track_msg.size_covariance[5] = INVALID_COVARIANCE; + + output_msg.tracks.emplace_back(track_msg); + } + + return output_msg; +} + +visualization_msgs::msg::MarkerArray ContinentalSrr520DecoderWrapper::ConvertToMarkers( + const continental_msgs::msg::ContinentalSrr520ObjectList & msg) +{ + visualization_msgs::msg::MarkerArray marker_array; + marker_array.markers.reserve(4 * msg.objects.size()); + + constexpr int LINE_STRIP_CORNERS_NUM = 17; + constexpr std::array, LINE_STRIP_CORNERS_NUM> cube_corners = { + {{{-1.0, -1.0, -1.0}}, + {{-1.0, -1.0, 1.0}}, + {{-1.0, 1.0, 1.0}}, + {{-1.0, 1.0, -1.0}}, + {{-1.0, -1.0, -1.0}}, + {{1.0, -1.0, -1.0}}, + {{1.0, -1.0, 1.0}}, + {{1.0, 1.0, 1.0}}, + {{1.0, 1.0, -1.0}}, + {{1.0, -1.0, -1.0}}, + {{-1.0, -1.0, -1.0}}, + {{-1.0, -1.0, 1.0}}, + {{1.0, -1.0, 1.0}}, + {{1.0, 1.0, 1.0}}, + {{-1.0, 1.0, 1.0}}, + {{-1.0, 1.0, -1.0}}, + {{1.0, 1.0, -1.0}}}}; + + constexpr int PALETTE_SIZE = 32; + constexpr std::array, PALETTE_SIZE> color_array = {{ + {{1.0, 0.0, 0.0}}, {{0.0, 1.0, 0.0}}, + {{0.0, 0.0, 1.0}}, // Red, Green, Blue + {{1.0, 1.0, 0.0}}, {{0.0, 1.0, 1.0}}, + {{1.0, 0.0, 1.0}}, // Yellow, Cyan, Magenta + {{1.0, 0.647, 0.0}}, {{0.749, 1.0, 0.0}}, + {{0.0, 0.502, 0.502}}, // Orange, Lime, Teal + {{0.502, 0.0, 0.502}}, {{1.0, 0.753, 0.796}}, + {{0.647, 0.165, 0.165}}, // Purple, Pink, Brown + {{0.502, 0.0, 0.0}}, {{0.502, 0.502, 0.0}}, + {{0.0, 0.0, 0.502}}, // Maroon, Olive, Navy + {{0.502, 0.502, 0.502}}, {{1.0, 0.4, 0.4}}, + {{0.4, 1.0, 0.4}}, // Grey, Light Red, Light Green + {{0.4, 0.4, 1.0}}, {{1.0, 1.0, 0.4}}, + {{0.4, 1.0, 1.0}}, // Light Blue, Light Yellow, Light Cyan + {{1.0, 0.4, 1.0}}, {{1.0, 0.698, 0.4}}, + {{0.698, 0.4, 1.0}}, // Light Magenta, Light Orange, Light Purple + {{1.0, 0.6, 0.8}}, {{0.71, 0.396, 0.114}}, + {{0.545, 0.0, 0.0}}, // Light Pink, Light Brown, Dark Red + {{0.0, 0.392, 0.0}}, {{0.0, 0.0, 0.545}}, + {{0.545, 0.545, 0.0}}, // Dark Green, Dark Blue, Dark Yellow + {{0.0, 0.545, 0.545}}, {{0.545, 0.0, 0.545}} // Dark Cyan, Dark Magenta + }}; + + std::unordered_set current_ids; + + for (const auto & object : msg.objects) { + if (!object.box_valid || object.object_status == 0) { + continue; + } + + const double half_length = 0.5 * object.box_length; + const double half_width = 0.5 * object.box_width; + constexpr double DEFAULT_HALF_SIZE = 1.0; + + const double & yaw = object.orientation; + current_ids.emplace(object.object_id); + + visualization_msgs::msg::Marker box_marker; + box_marker.header.frame_id = sensor_cfg_->base_frame; + box_marker.header.stamp = msg.header.stamp; + box_marker.ns = "boxes"; + box_marker.id = object.object_id; + box_marker.action = visualization_msgs::msg::Marker::ADD; + box_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + box_marker.lifetime = rclcpp::Duration::from_seconds(0); + box_marker.color.r = color_array[object.object_id % PALETTE_SIZE][0]; + box_marker.color.g = color_array[object.object_id % PALETTE_SIZE][1]; + box_marker.color.b = color_array[object.object_id % PALETTE_SIZE][2]; + box_marker.color.a = 1.0; + box_marker.scale.x = 0.1; + + box_marker.pose.position.x = object.dist_x; + box_marker.pose.position.y = object.dist_y; + box_marker.pose.position.z = DEFAULT_HALF_SIZE; + box_marker.pose.orientation.w = std::cos(0.5 * yaw); + box_marker.pose.orientation.z = std::sin(0.5 * yaw); + + for (const auto & corner : cube_corners) { + geometry_msgs::msg::Point p; + p.x = half_length * corner[0]; + p.y = half_width * corner[1]; + p.z = DEFAULT_HALF_SIZE * corner[2]; + box_marker.points.emplace_back(p); + } + + marker_array.markers.emplace_back(box_marker); + + visualization_msgs::msg::Marker text_marker = box_marker; + text_marker.ns = "object_age"; + text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + text_marker.color.r = 1.0; + text_marker.color.g = 1.0; + text_marker.color.b = 1.0; + text_marker.color.a = 1.0; + text_marker.scale.x = 0.3; + text_marker.scale.y = 0.3; + text_marker.scale.z = 0.3; + text_marker.pose.position.z += 0.5; + text_marker.text = "ID=" + std::to_string(object.object_id) + + " Age=" + std::to_string(object.life_cycles) + "ms"; + + marker_array.markers.emplace_back(text_marker); + + std::stringstream object_status_ss; + object_status_ss << std::fixed << std::setprecision(3) + << "ID=" << static_cast(object.object_id) << "\n" + << static_cast(object.box_length) << "/" + << static_cast(object.object_status); + + text_marker.ns = "object_status"; + text_marker.text = object_status_ss.str(); + + marker_array.markers.emplace_back(text_marker); + + std::stringstream object_dynamics_ss; + object_dynamics_ss << std::fixed << std::setprecision(3) + << "ID=" << static_cast(object.object_id) + << "\nyaw=" << object.orientation << "\nvx=" << object.v_abs_x + << "\nvy=" << object.v_abs_y << "\nax=" << object.a_abs_x + << "\nay=" << object.a_abs_y; + + text_marker.ns = "object_dynamics"; + text_marker.text = object_dynamics_ss.str(); + + marker_array.markers.emplace_back(text_marker); + } + + for (const auto & previous_id : previous_ids_) { + if (current_ids.find(previous_id) != current_ids.end()) { + continue; + } + + visualization_msgs::msg::Marker delete_marker; + delete_marker.header.frame_id = sensor_cfg_->base_frame; + delete_marker.header.stamp = msg.header.stamp; + delete_marker.ns = "boxes"; + delete_marker.id = previous_id; + delete_marker.action = visualization_msgs::msg::Marker::DELETE; + + marker_array.markers.push_back(delete_marker); + + delete_marker.ns = "object_age"; + marker_array.markers.push_back(delete_marker); + + delete_marker.ns = "object_status"; + marker_array.markers.push_back(delete_marker); + + delete_marker.ns = "object_dynamics"; + marker_array.markers.push_back(delete_marker); + } + + previous_ids_.clear(); + previous_ids_ = current_ids; + + return marker_array; +} + +void ContinentalSrr520DecoderWrapper::SyncFupCallback() +{ + hw_interface_ptr_->SensorSyncFup(); +} + +void ContinentalSrr520DecoderWrapper::PacketsCallback( + std::unique_ptr msg) +{ + if ( + packets_pub_ && (packets_pub_->get_subscription_count() > 0 || + packets_pub_->get_intra_process_subscription_count() > 0)) { + packets_pub_->publish(std::move(msg)); + } +} + +nebula::Status ContinentalSrr520DecoderWrapper::Status() +{ + std::lock_guard lock(mtx_driver_ptr_); + + if (!driver_ptr_) { + return nebula::Status::NOT_INITIALIZED; + } + + return driver_ptr_->GetStatus(); +} +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/continental/continental_srr520_hw_interface_ros_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_hw_interface_ros_wrapper.cpp new file mode 100644 index 000000000..78b734e9c --- /dev/null +++ b/nebula_ros/src/continental/continental_srr520_hw_interface_ros_wrapper.cpp @@ -0,0 +1,384 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include +#include + +namespace nebula +{ +namespace ros +{ +ContinentalSrr520HwInterfaceRosWrapper::ContinentalSrr520HwInterfaceRosWrapper( + const rclcpp::NodeOptions & options) +: rclcpp::Node("continental_srr520_hw_interface_ros_wrapper", options), hw_interface_() +{ + if (mtx_config_.try_lock()) { + interface_status_ = GetParameters(sensor_configuration_); + mtx_config_.unlock(); + } + if (Status::OK != interface_status_) { + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); + return; + } + hw_interface_.SetLogger(std::make_shared(this->get_logger())); + std::shared_ptr + sensor_cfg_ptr = + std::make_shared( + sensor_configuration_); + hw_interface_.SetSensorConfiguration( + std::static_pointer_cast(sensor_cfg_ptr)); + + hw_interface_.RegisterScanCallback(std::bind( + &ContinentalSrr520HwInterfaceRosWrapper::ReceivePacketsDataCallback, this, + std::placeholders::_1)); + packets_pub_ = this->create_publisher( + "nebula_packets", rclcpp::SensorDataQoS()); + + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&ContinentalSrr520HwInterfaceRosWrapper::paramCallback, this, std::placeholders::_1)); + + StreamStart(); +} + +ContinentalSrr520HwInterfaceRosWrapper::~ContinentalSrr520HwInterfaceRosWrapper() +{ +} + +Status ContinentalSrr520HwInterfaceRosWrapper::StreamStart() +{ + using std::chrono_literals::operator""ms; + + if (Status::OK == interface_status_) { + interface_status_ = hw_interface_.SensorInterfaceStart(); + } + + if (Status::OK == interface_status_) { + odometry_sub_.subscribe(this, "odometry_input"); + acceleration_sub_.subscribe(this, "acceleration_input"); + + sync_ptr_ = + std::make_shared(ExactTimeSyncPolicy(10), odometry_sub_, acceleration_sub_); + sync_ptr_->registerCallback(&ContinentalSrr520HwInterfaceRosWrapper::dynamicsCallback, this); + + configure_sensor_service_server_ = + this->create_service( + "configure_sensor", + std::bind( + &ContinentalSrr520HwInterfaceRosWrapper::ConfigureSensorRequestCallback, this, + std::placeholders::_1, std::placeholders::_2)); + + sync_timer_ = rclcpp::create_timer( + this, get_clock(), 100ms, + std::bind(&ContinentalSrr520HwInterfaceRosWrapper::syncTimerCallback, this)); + } + + return interface_status_; +} + +Status ContinentalSrr520HwInterfaceRosWrapper::StreamStop() +{ + hw_interface_.SensorInterfaceStop(); + return Status::OK; +} +Status ContinentalSrr520HwInterfaceRosWrapper::Shutdown() +{ + hw_interface_.SensorInterfaceStop(); + return Status::OK; +} + +Status ContinentalSrr520HwInterfaceRosWrapper::InitializeHwInterface( // todo: don't think + // this is needed + const drivers::SensorConfigurationBase & sensor_configuration) +{ + std::stringstream ss; + ss << sensor_configuration; + RCLCPP_DEBUG_STREAM(this->get_logger(), ss.str()); + return Status::OK; +} + +Status ContinentalSrr520HwInterfaceRosWrapper::GetParameters( + drivers::continental_srr520::ContinentalSrr520SensorConfiguration & sensor_configuration) +{ + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("sensor_model", descriptor); + sensor_configuration.sensor_model = + nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("interface", descriptor); + sensor_configuration.interface = this->get_parameter("interface").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("receiver_timeout_sec", descriptor); + sensor_configuration.receiver_timeout_sec = + this->get_parameter("receiver_timeout_sec").as_double(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("sender_timeout_sec", descriptor); + sensor_configuration.sender_timeout_sec = this->get_parameter("sender_timeout_sec").as_double(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("filters", descriptor); + sensor_configuration.filters = this->get_parameter("filters").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("frame_id", descriptor); + sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("base_frame", descriptor); + sensor_configuration.base_frame = this->get_parameter("base_frame").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("use_bus_time", descriptor); + sensor_configuration.use_bus_time = this->get_parameter("use_bus_time").as_bool(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_vehicle_wheelbase", descriptor); + sensor_configuration.configuration_vehicle_wheelbase = + static_cast(this->get_parameter("configuration_vehicle_wheelbase").as_double()); + } + + if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + + if (sensor_configuration.frame_id.empty()) { + return Status::SENSOR_CONFIG_ERROR; + } + + RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); + return Status::OK; +} + +void ContinentalSrr520HwInterfaceRosWrapper::ReceivePacketsDataCallback( + std::unique_ptr scan_buffer) +{ + packets_pub_->publish(std::move(scan_buffer)); +} + +rcl_interfaces::msg::SetParametersResult ContinentalSrr520HwInterfaceRosWrapper::paramCallback( + const std::vector & p) +{ + std::scoped_lock lock(mtx_config_); + RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback"); + RCLCPP_DEBUG_STREAM(this->get_logger(), p); + RCLCPP_DEBUG_STREAM(this->get_logger(), sensor_configuration_); + RCLCPP_INFO_STREAM(this->get_logger(), p); + + drivers::continental_srr520::ContinentalSrr520SensorConfiguration new_param{ + sensor_configuration_}; + RCLCPP_INFO_STREAM(this->get_logger(), new_param); + std::string sensor_model_str; + std::string return_mode_str; + + if ( + get_param(p, "sensor_model", sensor_model_str) | + get_param(p, "interface", new_param.interface) | + get_param(p, "receiver_timeout_sec", new_param.receiver_timeout_sec) | + get_param(p, "sender_timeout_sec", new_param.sender_timeout_sec) | + get_param(p, "frame_id", new_param.frame_id) | + get_param(p, "base_frame", new_param.base_frame) | get_param(p, "filters", new_param.filters) | + get_param(p, "use_bus_time", new_param.use_bus_time) | + get_param(p, "configuration_vehicle_wheelbase", new_param.configuration_vehicle_wheelbase)) { + if (0 < sensor_model_str.length()) + new_param.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_str); + + sensor_configuration_ = new_param; + RCLCPP_INFO_STREAM(this->get_logger(), "Update sensor_configuration"); + std::shared_ptr sensor_cfg_ptr = + std::make_shared( + sensor_configuration_); + RCLCPP_DEBUG_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); + hw_interface_.SetSensorConfiguration( + std::static_pointer_cast(sensor_cfg_ptr)); + hw_interface_.CheckAndSetConfig(); + } + + auto result = std::make_shared(); + result->successful = true; + result->reason = "success"; + + RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback success"); + + return *result; +} + +void ContinentalSrr520HwInterfaceRosWrapper::dynamicsCallback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr & odometry_msg, + const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr & acceleration_msg) +{ + constexpr float speed_to_standstill = 0.5f; + constexpr float speed_to_moving = 2.f; + + if (standstill_ && std::abs(odometry_msg->twist.twist.linear.x) > speed_to_moving) { + standstill_ = false; + } else if (!standstill_ && std::abs(odometry_msg->twist.twist.linear.x) < speed_to_standstill) { + standstill_ = true; + } + + hw_interface_.SetVehicleDynamics( + acceleration_msg->accel.accel.linear.x, acceleration_msg->accel.accel.linear.y, + odometry_msg->twist.twist.angular.z, odometry_msg->twist.twist.linear.x, standstill_); +} + +void ContinentalSrr520HwInterfaceRosWrapper::ConfigureSensorRequestCallback( + const std::shared_ptr + request, + const std::shared_ptr + response) +{ + std::scoped_lock lock(mtx_config_); + + auto tf_buffer = std::make_unique(this->get_clock()); + auto tf_listener = std::make_unique(*tf_buffer); + + float longitudinal = request->longitudinal; + float lateral = request->lateral; + float vertical = request->vertical; + float yaw = request->yaw; + float vehicle_wheelbase = request->vehicle_wheelbase; + + if (vehicle_wheelbase < 0.f) { + RCLCPP_INFO( + this->get_logger(), + "Service vehicle_wheelbase is invalid. Falling back to configuration value (%.2f)", + sensor_configuration_.configuration_vehicle_wheelbase); + vehicle_wheelbase = sensor_configuration_.configuration_vehicle_wheelbase; + } + + if (request->autoconfigure_extrinsics) { + RCLCPP_INFO( + this->get_logger(), + "autoconfigure_extrinsics was set to true, so the mounting position will be derived from the " + "tfs"); + + geometry_msgs::msg::TransformStamped base_to_sensor_tf; + try { + base_to_sensor_tf = tf_buffer->lookupTransform( + sensor_configuration_.base_frame, sensor_configuration_.frame_id, rclcpp::Time(0), + rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR( + this->get_logger(), "Could not obtain the transform from the base frame to %s (%s)", + sensor_configuration_.frame_id.c_str(), ex.what()); + response->success = false; + response->message = ex.what(); + return; + } + + const auto & quat = base_to_sensor_tf.transform.rotation; + geometry_msgs::msg::Vector3 rpy; + tf2::Matrix3x3(tf2::Quaternion(quat.x, quat.y, quat.z, quat.w)).getRPY(rpy.x, rpy.y, rpy.z); + + longitudinal = base_to_sensor_tf.transform.translation.x - vehicle_wheelbase; + lateral = base_to_sensor_tf.transform.translation.y; + vertical = base_to_sensor_tf.transform.translation.z; + yaw = rpy.z; + } + + yaw = std::min(std::max(yaw, -3.14159f), 3.14159f); + + auto result = hw_interface_.ConfigureSensor( + request->sensor_id, longitudinal, lateral, vertical, yaw, + longitudinal + 0.5 * vehicle_wheelbase, vehicle_wheelbase, request->cover_damping, + request->plug_bottom, request->reset_sensor_configuration); + + response->success = result == Status::OK; + response->message = (std::stringstream() << result).str(); +} + +void ContinentalSrr520HwInterfaceRosWrapper::syncTimerCallback() +{ + hw_interface_.SensorSync(); +} + +std::vector +ContinentalSrr520HwInterfaceRosWrapper::updateParameters() +{ + std::scoped_lock lock(mtx_config_); + RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters start"); + std::ostringstream os_sensor_model; + os_sensor_model << sensor_configuration_.sensor_model; + RCLCPP_INFO_STREAM(this->get_logger(), "set_parameters"); + + auto results = set_parameters( + {rclcpp::Parameter("sensor_model", os_sensor_model.str()), + rclcpp::Parameter("interface", sensor_configuration_.interface), + rclcpp::Parameter("receiver_timeout_sec", sensor_configuration_.receiver_timeout_sec), + rclcpp::Parameter("sender_timeout_sec", sensor_configuration_.sender_timeout_sec), + rclcpp::Parameter("frame_id", sensor_configuration_.frame_id), + rclcpp::Parameter("base_frame", sensor_configuration_.base_frame), + rclcpp::Parameter("filters", sensor_configuration_.filters), + rclcpp::Parameter("use_bus_time", sensor_configuration_.use_bus_time), + rclcpp::Parameter( + "configuration_vehicle_wheelbase", sensor_configuration_.configuration_vehicle_wheelbase)}); + + RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters end"); + return results; +} + +RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalSrr520HwInterfaceRosWrapper) +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/continental/continental_srr520_hw_interface_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_hw_interface_wrapper.cpp new file mode 100644 index 000000000..e4841e082 --- /dev/null +++ b/nebula_ros/src/continental/continental_srr520_hw_interface_wrapper.cpp @@ -0,0 +1,183 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +namespace nebula +{ +namespace ros +{ + +ContinentalSrr520HwInterfaceWrapper::ContinentalSrr520HwInterfaceWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr & + config_ptr) +: parent_node_(parent_node), + hw_interface_( + std::make_shared()), + logger_(parent_node->get_logger().get_child("HwInterfaceWrapper")), + status_(Status::NOT_INITIALIZED), + config_ptr_(config_ptr) +{ + hw_interface_->SetLogger( + std::make_shared(parent_node->get_logger().get_child("HwInterface"))); + status_ = hw_interface_->SetSensorConfiguration(config_ptr_); + + if (status_ != Status::OK) { + throw std::runtime_error( + (std::stringstream{} << "Could not initialize HW interface: " << status_).str()); + } + + status_ = Status::OK; +} + +void ContinentalSrr520HwInterfaceWrapper::SensorInterfaceStart() +{ + using std::chrono_literals::operator""ms; + + if (Status::OK == status_) { + hw_interface_->SensorInterfaceStart(); + } + + if (Status::OK == status_) { + odometry_sub_.subscribe(parent_node_, "odometry_input"); + acceleration_sub_.subscribe(parent_node_, "acceleration_input"); + + sync_ptr_ = + std::make_shared(ExactTimeSyncPolicy(10), odometry_sub_, acceleration_sub_); + sync_ptr_->registerCallback(&ContinentalSrr520HwInterfaceWrapper::dynamicsCallback, this); + + configure_sensor_service_server_ = + parent_node_->create_service( + "configure_sensor", std::bind( + &ContinentalSrr520HwInterfaceWrapper::ConfigureSensorRequestCallback, + this, std::placeholders::_1, std::placeholders::_2)); + + sync_timer_ = rclcpp::create_timer( + parent_node_, parent_node_->get_clock(), 100ms, + std::bind(&ContinentalSrr520HwInterfaceWrapper::syncTimerCallback, this)); + } +} + +void ContinentalSrr520HwInterfaceWrapper::OnConfigChange( + const std::shared_ptr< + const nebula::drivers::continental_srr520::ContinentalSrr520SensorConfiguration> & + new_config_ptr) +{ + hw_interface_->SetSensorConfiguration(new_config_ptr); + config_ptr_ = new_config_ptr; +} + +Status ContinentalSrr520HwInterfaceWrapper::Status() +{ + return status_; +} + +std::shared_ptr +ContinentalSrr520HwInterfaceWrapper::HwInterface() const +{ + return hw_interface_; +} + +void ContinentalSrr520HwInterfaceWrapper::dynamicsCallback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr & odometry_msg, + const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr & acceleration_msg) +{ + constexpr float speed_to_standstill = 0.5f; + constexpr float speed_to_moving = 2.f; + + if (standstill_ && std::abs(odometry_msg->twist.twist.linear.x) > speed_to_moving) { + standstill_ = false; + } else if (!standstill_ && std::abs(odometry_msg->twist.twist.linear.x) < speed_to_standstill) { + standstill_ = true; + } + + hw_interface_->SetVehicleDynamics( + acceleration_msg->accel.accel.linear.x, acceleration_msg->accel.accel.linear.y, + odometry_msg->twist.twist.angular.z, odometry_msg->twist.twist.linear.x, standstill_); +} + +void ContinentalSrr520HwInterfaceWrapper::ConfigureSensorRequestCallback( + const std::shared_ptr + request, + const std::shared_ptr + response) +{ + auto tf_buffer = std::make_unique(parent_node_->get_clock()); + auto tf_listener = std::make_unique(*tf_buffer); + + float longitudinal = request->longitudinal; + float lateral = request->lateral; + float vertical = request->vertical; + float yaw = request->yaw; + float vehicle_wheelbase = request->vehicle_wheelbase; + + if (vehicle_wheelbase < 0.f) { + RCLCPP_INFO( + logger_, "Service vehicle_wheelbase is invalid. Falling back to configuration value (%.2f)", + config_ptr_->configuration_vehicle_wheelbase); + vehicle_wheelbase = config_ptr_->configuration_vehicle_wheelbase; + } + + if (request->autoconfigure_extrinsics) { + RCLCPP_INFO( + logger_, + "autoconfigure_extrinsics was set to true, so the mounting position will be derived from the " + "tfs"); + + geometry_msgs::msg::TransformStamped base_to_sensor_tf; + try { + base_to_sensor_tf = tf_buffer->lookupTransform( + config_ptr_->base_frame, config_ptr_->frame_id, rclcpp::Time(0), + rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR( + logger_, "Could not obtain the transform from the base frame to %s (%s)", + config_ptr_->frame_id.c_str(), ex.what()); + response->success = false; + response->message = ex.what(); + return; + } + + const auto & quat = base_to_sensor_tf.transform.rotation; + geometry_msgs::msg::Vector3 rpy; + tf2::Matrix3x3(tf2::Quaternion(quat.x, quat.y, quat.z, quat.w)).getRPY(rpy.x, rpy.y, rpy.z); + + longitudinal = base_to_sensor_tf.transform.translation.x - vehicle_wheelbase; + lateral = base_to_sensor_tf.transform.translation.y; + vertical = base_to_sensor_tf.transform.translation.z; + yaw = rpy.z; + } + + yaw = std::min(std::max(yaw, -3.14159f), 3.14159f); + + auto result = hw_interface_->ConfigureSensor( + request->sensor_id, longitudinal, lateral, vertical, yaw, + longitudinal + 0.5 * vehicle_wheelbase, vehicle_wheelbase, request->cover_damping, + request->plug_bottom, request->reset_sensor_configuration); + + response->success = result == Status::OK; + response->message = (std::stringstream() << result).str(); +} + +void ContinentalSrr520HwInterfaceWrapper::syncTimerCallback() +{ + hw_interface_->SensorSync(); +} + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp new file mode 100644 index 000000000..74a5217cc --- /dev/null +++ b/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp @@ -0,0 +1,298 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +namespace nebula +{ +namespace ros +{ +ContinentalSrr520RosWrapper::ContinentalSrr520RosWrapper(const rclcpp::NodeOptions & options) +: rclcpp::Node( + "continental_srr520_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)), + wrapper_status_(Status::NOT_INITIALIZED), + sensor_cfg_ptr_(nullptr), + packet_queue_(3000), + hw_interface_wrapper_(), + decoder_wrapper_() +{ + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + + wrapper_status_ = DeclareAndGetSensorConfigParams(); + + if (wrapper_status_ != Status::OK) { + throw std::runtime_error( + (std::stringstream{} << "Sensor configuration invalid: " << wrapper_status_).str()); + } + + RCLCPP_INFO_STREAM(get_logger(), "SensorConfig:" << *sensor_cfg_ptr_); + + launch_hw_ = declare_parameter("launch_hw", param_read_only()); + + if (launch_hw_) { + hw_interface_wrapper_.emplace(this, sensor_cfg_ptr_); + decoder_wrapper_.emplace(this, sensor_cfg_ptr_, hw_interface_wrapper_->HwInterface()); + } else { + decoder_wrapper_.emplace(this, sensor_cfg_ptr_, nullptr); + } + + RCLCPP_DEBUG(get_logger(), "Starting stream"); + + decoder_thread_ = std::thread([this]() { + while (true) { + decoder_wrapper_->ProcessPacket(std::move(packet_queue_.pop())); + } + }); + + if (launch_hw_) { + hw_interface_wrapper_->HwInterface()->RegisterCallback( + std::bind(&ContinentalSrr520RosWrapper::ReceivePacketCallback, this, std::placeholders::_1)); + StreamStart(); + } else { + packets_sub_ = create_subscription( + "nebula_packets", rclcpp::SensorDataQoS(), + std::bind( + &ContinentalSrr520RosWrapper::ReceivePacketsMessageCallback, this, std::placeholders::_1)); + RCLCPP_INFO_STREAM( + get_logger(), + "Hardware connection disabled, listening for packets on " << packets_sub_->get_topic_name()); + } + + // Register parameter callback after all params have been declared. Otherwise it would be called + // once for each declaration + parameter_event_cb_ = add_on_set_parameters_callback( + std::bind(&ContinentalSrr520RosWrapper::OnParameterChange, this, std::placeholders::_1)); +} + +nebula::Status ContinentalSrr520RosWrapper::DeclareAndGetSensorConfigParams() +{ + nebula::drivers::continental_srr520::ContinentalSrr520SensorConfiguration config; + + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("sensor_model", descriptor); + config.sensor_model = + nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("interface", descriptor); + config.interface = this->get_parameter("interface").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("receiver_timeout_sec", descriptor); + config.receiver_timeout_sec = this->get_parameter("receiver_timeout_sec").as_double(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("sender_timeout_sec", descriptor); + config.sender_timeout_sec = this->get_parameter("sender_timeout_sec").as_double(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("filters", descriptor); + config.filters = this->get_parameter("filters").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("frame_id", descriptor); + config.frame_id = this->get_parameter("frame_id").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("base_frame", descriptor); + config.base_frame = this->get_parameter("base_frame").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("use_bus_time", descriptor); + config.use_bus_time = this->get_parameter("use_bus_time").as_bool(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_vehicle_wheelbase", descriptor); + config.configuration_vehicle_wheelbase = + static_cast(this->get_parameter("configuration_vehicle_wheelbase").as_double()); + } + + if (config.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + + auto new_config_ptr = std::make_shared< + const nebula::drivers::continental_srr520::ContinentalSrr520SensorConfiguration>(config); + return ValidateAndSetConfig(new_config_ptr); +} + +Status ContinentalSrr520RosWrapper::ValidateAndSetConfig( + std::shared_ptr & + new_config) +{ + if (new_config->sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + + if (new_config->frame_id.empty()) { + return Status::SENSOR_CONFIG_ERROR; + } + + if (hw_interface_wrapper_) { + hw_interface_wrapper_->OnConfigChange(new_config); + } + if (decoder_wrapper_) { + decoder_wrapper_->OnConfigChange(new_config); + } + + sensor_cfg_ptr_ = new_config; + return Status::OK; +} + +void ContinentalSrr520RosWrapper::ReceivePacketsMessageCallback( + std::unique_ptr packets_msg) +{ + if (hw_interface_wrapper_) { + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 1000, + "Ignoring NebulaPackets PandarScan. Launch with launch_hw:=false to enable NebulaPackets " + "replay."); + return; + } + + for (auto & packet : packets_msg->packets) { + auto nebula_packet_ptr = std::make_unique(); + nebula_packet_ptr->stamp = packet.stamp; + std::copy(packet.data.begin(), packet.data.end(), std::back_inserter(nebula_packet_ptr->data)); + + packet_queue_.push(std::move(nebula_packet_ptr)); + } +} + +Status ContinentalSrr520RosWrapper::GetStatus() +{ + return wrapper_status_; +} + +Status ContinentalSrr520RosWrapper::StreamStart() +{ + if (!hw_interface_wrapper_) { + return Status::UDP_CONNECTION_ERROR; + } + + if (hw_interface_wrapper_->Status() != Status::OK) { + return hw_interface_wrapper_->Status(); + } + + hw_interface_wrapper_->SensorInterfaceStart(); + + return hw_interface_wrapper_->Status(); +} + +rcl_interfaces::msg::SetParametersResult ContinentalSrr520RosWrapper::OnParameterChange( + const std::vector & p) +{ + using rcl_interfaces::msg::SetParametersResult; + + if (p.empty()) { + return rcl_interfaces::build().successful(true).reason(""); + } + + RCLCPP_INFO(get_logger(), "OnParameterChange"); + + drivers::continental_srr520::ContinentalSrr520SensorConfiguration new_config(*sensor_cfg_ptr_); + + bool got_any = + get_param(p, "frame_id", new_config.frame_id) | + get_param(p, "base_frame", new_config.base_frame) | + get_param(p, "use_bus_time", new_config.use_bus_time) | + get_param(p, "configuration_vehicle_wheelbase", new_config.configuration_vehicle_wheelbase); + + if (!got_any) { + return rcl_interfaces::build().successful(true).reason(""); + } + + auto new_config_ptr = std::make_shared< + const nebula::drivers::continental_srr520::ContinentalSrr520SensorConfiguration>(new_config); + auto status = ValidateAndSetConfig(new_config_ptr); + + if (status != Status::OK) { + RCLCPP_WARN_STREAM(get_logger(), "OnParameterChange aborted: " << status); + auto result = SetParametersResult(); + result.successful = false; + result.reason = (std::stringstream() << "Invalid configuration: " << status).str(); + return result; + } + + return rcl_interfaces::build().successful(true).reason(""); +} + +void ContinentalSrr520RosWrapper::ReceivePacketCallback(std::vector & packet) +{ + if (!decoder_wrapper_ || decoder_wrapper_->Status() != Status::OK) { + return; + } + + const auto now = std::chrono::high_resolution_clock::now(); + const auto timestamp_ns = + std::chrono::duration_cast(now.time_since_epoch()).count(); + + auto msg_ptr = std::make_unique(); + msg_ptr->stamp.sec = static_cast(timestamp_ns / 1'000'000'000); + msg_ptr->stamp.nanosec = static_cast(timestamp_ns % 1'000'000'000); + msg_ptr->data.swap(packet); + + if (!packet_queue_.try_push(std::move(msg_ptr))) { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped"); + } +} + +RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalSrr520RosWrapper) +} // namespace ros +} // namespace nebula diff --git a/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp b/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp new file mode 100644 index 000000000..86efaf9c0 --- /dev/null +++ b/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp @@ -0,0 +1,300 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "continental_ros_decoder_test_srr520.hpp" + +#include "rclcpp/serialization.hpp" +#include "rclcpp/serialized_message.hpp" +#include "rcpputils/filesystem_helper.hpp" +#include "rcutils/time.h" +#include "rosbag2_cpp/reader.hpp" +#include "rosbag2_cpp/readers/sequential_reader.hpp" +#include "rosbag2_cpp/writer.hpp" +#include "rosbag2_cpp/writers/sequential_writer.hpp" +#include "rosbag2_storage/storage_options.hpp" + +#include + +#include +#include +#include +#include + +namespace nebula +{ +namespace ros +{ +ContinentalRosDecoderTest::ContinentalRosDecoderTest( + const rclcpp::NodeOptions & options, const std::string & node_name) +: rclcpp::Node(node_name, options) +{ + drivers::continental_srr520::ContinentalSrr520SensorConfiguration sensor_configuration; + + wrapper_status_ = GetParameters(sensor_configuration); + if (Status::OK != wrapper_status_) { + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); + return; + } + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); + + sensor_cfg_ptr_ = + std::make_shared( + sensor_configuration); + + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); + wrapper_status_ = InitializeDriver( + std::const_pointer_cast( + sensor_cfg_ptr_)); + + driver_ptr_->RegisterHRRDetectionListCallback( + std::bind(&ContinentalRosDecoderTest::HRRDetectionListCallback, this, std::placeholders::_1)); + driver_ptr_->RegisterNearDetectionListCallback( + std::bind(&ContinentalRosDecoderTest::NearDetectionListCallback, this, std::placeholders::_1)); + driver_ptr_->RegisterObjectListCallback( + std::bind(&ContinentalRosDecoderTest::ObjectListCallback, this, std::placeholders::_1)); + driver_ptr_->RegisterStatusCallback( + std::bind(&ContinentalRosDecoderTest::StatusCallback, this, std::placeholders::_1)); + + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); +} + +Status ContinentalRosDecoderTest::InitializeDriver( + std::shared_ptr + sensor_configuration) +{ + // driver should be initialized here with proper decoder + driver_ptr_ = std::make_shared( + std::static_pointer_cast( + sensor_configuration)); + return Status::OK; +} + +Status ContinentalRosDecoderTest::GetStatus() +{ + return wrapper_status_; +} + +Status ContinentalRosDecoderTest::GetParameters( + drivers::continental_srr520::ContinentalSrr520SensorConfiguration & sensor_configuration) +{ + std::filesystem::path bag_root_dir = + _SRC_RESOURCES_DIR_PATH; // variable defined in CMakeLists.txt; + bag_root_dir /= "continental"; + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = 4; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("sensor_model", "SRR520"); + sensor_configuration.sensor_model = + nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = 4; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("base_frame", "some_base_frame", descriptor); + sensor_configuration.frame_id = this->get_parameter("base_frame").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = 4; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("frame_id", "some_sensor_frame", descriptor); + sensor_configuration.base_frame = this->get_parameter("frame_id").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = 4; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter( + "bag_path", (bag_root_dir / "srr520" / "1708578209").string(), descriptor); + bag_path = this->get_parameter("bag_path").as_string(); + std::cout << bag_path << std::endl; + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = 4; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("storage_id", "sqlite3", descriptor); + storage_id = this->get_parameter("storage_id").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = 4; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("format", "cdr", descriptor); + format = this->get_parameter("format").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = 4; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter( + "target_topic", "/sensing/radar/front_left/nebula_packets", descriptor); + target_topic = this->get_parameter("target_topic").as_string(); + } + + if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + + RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); + return Status::OK; +} + +void ContinentalRosDecoderTest::CompareNodes(const YAML::Node & node1, const YAML::Node & node2) +{ + ASSERT_EQ(node1.IsDefined(), node2.IsDefined()); + ASSERT_EQ(node1.IsMap(), node2.IsMap()); + ASSERT_EQ(node1.IsNull(), node2.IsNull()); + ASSERT_EQ(node1.IsScalar(), node2.IsScalar()); + ASSERT_EQ(node1.IsSequence(), node2.IsSequence()); + + if (node1.IsMap()) { + for (YAML::const_iterator it = node1.begin(); it != node1.end(); ++it) { + CompareNodes(it->second, node2[it->first.as()]); + } + } else if (node1.IsScalar()) { + ASSERT_EQ(node1.as(), node2.as()); + } else if (node1.IsSequence()) { + ASSERT_EQ(node1.size(), node2.size()); + for (std::size_t i = 0; i < node1.size(); i++) { + CompareNodes(node1[i], node2[i]); + } + } +} + +void ContinentalRosDecoderTest::CheckResult( + const std::string msg_as_string, const std::string & gt_path) +{ + YAML::Node current_node = YAML::Load(msg_as_string); + YAML::Node gt_node = YAML::LoadFile(gt_path); + CompareNodes(gt_node, current_node); + + // To generate the gt + // std::cout << gt_path << std::endl; + // std::ofstream ostream(gt_path); + // ostream << msg_as_string; + // ostream.close(); +} + +void ContinentalRosDecoderTest::HRRDetectionListCallback( + std::unique_ptr msg) +{ + EXPECT_EQ(sensor_cfg_ptr_->frame_id, msg->header.frame_id); + std::string msg_as_string = continental_msgs::msg::to_yaml(*msg); + + std::stringstream detection_path; + detection_path << msg->header.stamp.sec << "_" << msg->header.stamp.nanosec + << "_hrr_detection.yaml"; + + auto gt_path = rcpputils::fs::path(bag_path).parent_path() / detection_path.str(); + ASSERT_TRUE(gt_path.exists()); + + CheckResult(msg_as_string, gt_path.string()); +} + +void ContinentalRosDecoderTest::NearDetectionListCallback( + std::unique_ptr msg) +{ + EXPECT_EQ(sensor_cfg_ptr_->frame_id, msg->header.frame_id); + std::string msg_as_string = continental_msgs::msg::to_yaml(*msg); + + std::stringstream detection_path; + detection_path << msg->header.stamp.sec << "_" << msg->header.stamp.nanosec + << "_near_detection.yaml"; + + auto gt_path = rcpputils::fs::path(bag_path).parent_path() / detection_path.str(); + ASSERT_TRUE(gt_path.exists()); + + CheckResult(msg_as_string, gt_path.string()); +} + +void ContinentalRosDecoderTest::ObjectListCallback( + std::unique_ptr msg) +{ + EXPECT_EQ(sensor_cfg_ptr_->base_frame, msg->header.frame_id); + std::string msg_as_string = continental_msgs::msg::to_yaml(*msg); + + std::stringstream detection_path; + detection_path << msg->header.stamp.sec << "_" << msg->header.stamp.nanosec << "_object.yaml"; + + auto gt_path = rcpputils::fs::path(bag_path).parent_path() / detection_path.str(); + ASSERT_TRUE(gt_path.exists()); + + CheckResult(msg_as_string, gt_path.string()); +} + +void ContinentalRosDecoderTest::ReadBag() +{ + rosbag2_storage::StorageOptions storage_options; + rosbag2_cpp::ConverterOptions converter_options; + + std::cout << bag_path << std::endl; + std::cout << storage_id << std::endl; + std::cout << format << std::endl; + std::cout << target_topic << std::endl; + + auto target_topic_name = target_topic; + if (target_topic_name.substr(0, 1) == "/") { + target_topic_name = target_topic_name.substr(1); + } + target_topic_name = std::regex_replace(target_topic_name, std::regex("/"), "_"); + + rcpputils::fs::path bag_dir(bag_path); + + storage_options.uri = bag_path; + storage_options.storage_id = storage_id; + converter_options.output_serialization_format = format; // "cdr"; + rclcpp::Serialization serialization; + + { + rosbag2_cpp::Reader bag_reader(std::make_unique()); + bag_reader.open(storage_options, converter_options); + while (bag_reader.has_next()) { + auto bag_message = bag_reader.read_next(); + + std::cout << "Found topic name " << bag_message->topic_name << std::endl; + + if (bag_message->topic_name == target_topic) { + nebula_msgs::msg::NebulaPackets extracted_msg; + rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); + serialization.deserialize_message(&extracted_serialized_msg, &extracted_msg); + + std::cout << "Found data in topic " << bag_message->topic_name << ": " + << bag_message->time_stamp << std::endl; + + auto extracted_msg_ptr = std::make_shared(extracted_msg); + driver_ptr_->ProcessPackets(extracted_msg); + } + } + } +} + +} // namespace ros +} // namespace nebula diff --git a/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp b/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp new file mode 100644 index 000000000..c4473e49e --- /dev/null +++ b/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp @@ -0,0 +1,97 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NEBULA_ContinentalRosDecoderTestsrr520_H +#define NEBULA_ContinentalRosDecoderTestsrr520_H + +#include "nebula_common/nebula_common.hpp" +#include "nebula_common/nebula_status.hpp" +#include "nebula_common/velodyne/velodyne_common.hpp" +#include "nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp" +#include "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp" + +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include + +namespace nebula +{ +namespace ros +{ +class ContinentalRosDecoderTest final : public rclcpp::Node, + NebulaDriverRosWrapperBase //, testing::Test +{ + std::shared_ptr driver_ptr_; + Status wrapper_status_; + + std::shared_ptr + sensor_cfg_ptr_; + + Status InitializeDriver( + std::shared_ptr + sensor_configuration); + + Status GetParameters( + drivers::continental_srr520::ContinentalSrr520SensorConfiguration & sensor_configuration); + + void CheckResult(const std::string msg_as_string, const std::string & gt_path); + + void HRRDetectionListCallback( + std::unique_ptr msg); + + void NearDetectionListCallback( + std::unique_ptr msg); + + void StatusCallback([[maybe_unused]] std::unique_ptr msg) + { + } + + void ObjectListCallback(std::unique_ptr msg); + + void CompareNodes(const YAML::Node & node1, const YAML::Node & node2); + + static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) + { + return std::chrono::duration_cast( + std::chrono::duration(seconds)); + } + +public: + explicit ContinentalRosDecoderTest( + const rclcpp::NodeOptions & options, const std::string & node_name); + + void ReceiveScanMsgCallback(const nebula_msgs::msg::NebulaPackets::SharedPtr scan_msg); + Status GetStatus(); + void ReadBag(); + +private: + std::string bag_path; + std::string storage_id; + std::string format; + std::string target_topic; +}; + +} // namespace ros +} // namespace nebula + +#endif // NEBULA_ContinentalRosDecoderTestsrr520_H From 48da73d0c6f72773c3a947caf650c0179995cf91 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 23 May 2024 03:08:01 +0900 Subject: [PATCH 03/47] chore: cleaned code and removed unused code/includes Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental/continental_ars548.hpp | 343 +++++++++--------- .../decoders/continental_ars548_decoder.hpp | 21 +- .../decoders/continental_packets_decoder.hpp | 11 +- .../decoders/continental_ars548_decoder.cpp | 15 +- .../continental_ars548_hw_interface.hpp | 96 ++--- .../continental_ars548_hw_interface.cpp | 163 +++------ .../continental_ars548_decoder_wrapper.hpp | 3 +- ...ontinental_ars548_hw_interface_wrapper.hpp | 23 +- .../continental_ars548_ros_wrapper.hpp | 10 +- .../continental_ars548_decoder_wrapper.cpp | 25 +- ...ontinental_ars548_hw_interface_wrapper.cpp | 35 +- .../continental_ars548_ros_wrapper.cpp | 60 ++- .../continental_ros_decoder_test_ars548.cpp | 7 +- 13 files changed, 353 insertions(+), 459 deletions(-) diff --git a/nebula_common/include/nebula_common/continental/continental_ars548.hpp b/nebula_common/include/nebula_common/continental/continental_ars548.hpp index 3fde9e84b..98f813c9f 100644 --- a/nebula_common/include/nebula_common/continental/continental_ars548.hpp +++ b/nebula_common/include/nebula_common/continental/continental_ars548.hpp @@ -17,19 +17,12 @@ #include #include -#include "boost/endian/buffers.hpp" #include -#include +#include #include #include -#include -#include -#include -#include -#include -#include #include #include #include @@ -327,159 +320,159 @@ struct HeaderPacket struct HeaderSomeIPPacket { - big_uint16_buf_t client_id; - big_uint16_buf_t session_id; - uint8_t protocol_version; - uint8_t interface_version; - uint8_t message_type; - uint8_t return_code; + big_uint16_buf_t client_id{}; + big_uint16_buf_t session_id{}; + uint8_t protocol_version{}; + uint8_t interface_version{}; + uint8_t message_type{}; + uint8_t return_code{}; }; struct HeaderE2EP07Packet { - big_uint64_buf_t crc; - big_uint32_buf_t length; - big_uint32_buf_t sqc; - big_uint32_buf_t data_id; + big_uint64_buf_t crc{}; + big_uint32_buf_t length{}; + big_uint32_buf_t sqc{}; + big_uint32_buf_t data_id{}; }; struct StampSyncStatusPacket { - big_uint32_buf_t timestamp_nanoseconds; - big_uint32_buf_t timestamp_seconds; - uint8_t timestamp_sync_status; + big_uint32_buf_t timestamp_nanoseconds{}; + big_uint32_buf_t timestamp_seconds{}; + uint8_t timestamp_sync_status{}; }; struct DetectionPacket { - big_float32_buf_t azimuth_angle; - big_float32_buf_t azimuth_angle_std; - uint8_t invalid_flags; - big_float32_buf_t elevation_angle; - big_float32_buf_t elevation_angle_std; - big_float32_buf_t range; - big_float32_buf_t range_std; - big_float32_buf_t range_rate; - big_float32_buf_t range_rate_std; - int8_t rcs; - big_uint16_buf_t measurement_id; - uint8_t positive_predictive_value; - uint8_t classification; - uint8_t multi_target_probability; - big_uint16_buf_t object_id; - uint8_t ambiguity_flag; - big_uint16_buf_t sort_index; + big_float32_buf_t azimuth_angle{}; + big_float32_buf_t azimuth_angle_std{}; + uint8_t invalid_flags{}; + big_float32_buf_t elevation_angle{}; + big_float32_buf_t elevation_angle_std{}; + big_float32_buf_t range{}; + big_float32_buf_t range_std{}; + big_float32_buf_t range_rate{}; + big_float32_buf_t range_rate_std{}; + int8_t rcs{}; + big_uint16_buf_t measurement_id{}; + uint8_t positive_predictive_value{}; + uint8_t classification{}; + uint8_t multi_target_probability{}; + big_uint16_buf_t object_id{}; + uint8_t ambiguity_flag{}; + big_uint16_buf_t sort_index{}; }; struct DetectionListPacket { - HeaderPacket header; - HeaderSomeIPPacket header_some_ip; - HeaderE2EP07Packet header_e2ep07; - StampSyncStatusPacket stamp; - big_uint32_buf_t event_data_qualifier; - uint8_t extended_qualifier; - big_uint16_buf_t origin_invalid_flags; - big_float32_buf_t origin_x_pos; - big_float32_buf_t origin_x_std; - big_float32_buf_t origin_y_pos; - big_float32_buf_t origin_y_std; - big_float32_buf_t origin_z_pos; - big_float32_buf_t origin_z_std; - big_float32_buf_t origin_roll; - big_float32_buf_t origin_roll_std; - big_float32_buf_t origin_pitch; - big_float32_buf_t origin_pitch_std; - big_float32_buf_t origin_yaw; - big_float32_buf_t origin_yaw_std; - uint8_t list_invalid_flags; + HeaderPacket header{}; + HeaderSomeIPPacket header_some_ip{}; + HeaderE2EP07Packet header_e2ep07{}; + StampSyncStatusPacket stamp{}; + big_uint32_buf_t event_data_qualifier{}; + uint8_t extended_qualifier{}; + big_uint16_buf_t origin_invalid_flags{}; + big_float32_buf_t origin_x_pos{}; + big_float32_buf_t origin_x_std{}; + big_float32_buf_t origin_y_pos{}; + big_float32_buf_t origin_y_std{}; + big_float32_buf_t origin_z_pos{}; + big_float32_buf_t origin_z_std{}; + big_float32_buf_t origin_roll{}; + big_float32_buf_t origin_roll_std{}; + big_float32_buf_t origin_pitch{}; + big_float32_buf_t origin_pitch_std{}; + big_float32_buf_t origin_yaw{}; + big_float32_buf_t origin_yaw_std{}; + uint8_t list_invalid_flags{}; DetectionPacket detections[MAX_DETECTIONS]; - big_float32_buf_t list_rad_vel_domain_min; - big_float32_buf_t list_rad_vel_domain_max; - big_uint32_buf_t number_of_detections; - big_float32_buf_t alignment_azimuth_correction; - big_float32_buf_t alignment_elevation_correction; - uint8_t alignment_status; + big_float32_buf_t list_rad_vel_domain_min{}; + big_float32_buf_t list_rad_vel_domain_max{}; + big_uint32_buf_t number_of_detections{}; + big_float32_buf_t alignment_azimuth_correction{}; + big_float32_buf_t alignment_elevation_correction{}; + uint8_t alignment_status{}; uint8_t reserved[14]; }; struct ObjectPacket { - big_uint16_buf_t status_sensor; - big_uint32_buf_t id; - big_uint16_buf_t age; - uint8_t status_measurement; - uint8_t status_movement; - big_uint16_buf_t position_invalid_flags; - uint8_t position_reference; - big_float32_buf_t position_x; - big_float32_buf_t position_x_std; - big_float32_buf_t position_y; - big_float32_buf_t position_y_std; - big_float32_buf_t position_z; - big_float32_buf_t position_z_std; - big_float32_buf_t position_covariance_xy; - big_float32_buf_t position_orientation; - big_float32_buf_t position_orientation_std; - uint8_t existence_invalid_flags; - big_float32_buf_t existence_probability; - big_float32_buf_t existence_ppv; - uint8_t classification_car; - uint8_t classification_truck; - uint8_t classification_motorcycle; - uint8_t classification_bicycle; - uint8_t classification_pedestrian; - uint8_t classification_animal; - uint8_t classification_hazard; - uint8_t classification_unknown; - uint8_t classification_overdrivable; - uint8_t classification_underdrivable; - uint8_t dynamics_abs_vel_invalid_flags; - big_float32_buf_t dynamics_abs_vel_x; - big_float32_buf_t dynamics_abs_vel_x_std; - big_float32_buf_t dynamics_abs_vel_y; - big_float32_buf_t dynamics_abs_vel_y_std; - big_float32_buf_t dynamics_abs_vel_covariance_xy; - uint8_t dynamics_rel_vel_invalid_flags; - big_float32_buf_t dynamics_rel_vel_x; - big_float32_buf_t dynamics_rel_vel_x_std; - big_float32_buf_t dynamics_rel_vel_y; - big_float32_buf_t dynamics_rel_vel_y_std; - big_float32_buf_t dynamics_rel_vel_covariance_xy; - uint8_t dynamics_abs_accel_invalid_flags; - big_float32_buf_t dynamics_abs_accel_x; - big_float32_buf_t dynamics_abs_accel_x_std; - big_float32_buf_t dynamics_abs_accel_y; - big_float32_buf_t dynamics_abs_accel_y_std; - big_float32_buf_t dynamics_abs_accel_covariance_xy; - uint8_t dynamics_rel_accel_invalid_flags; - big_float32_buf_t dynamics_rel_accel_x; - big_float32_buf_t dynamics_rel_accel_x_std; - big_float32_buf_t dynamics_rel_accel_y; - big_float32_buf_t dynamics_rel_accel_y_std; - big_float32_buf_t dynamics_rel_accel_covariance_xy; - uint8_t dynamics_orientation_invalid_flags; - big_float32_buf_t dynamics_orientation_rate_mean; - big_float32_buf_t dynamics_orientation_rate_std; - big_uint32_buf_t shape_length_status; - uint8_t shape_length_edge_invalid_flags; - big_float32_buf_t shape_length_edge_mean; - big_float32_buf_t shape_length_edge_std; - big_uint32_buf_t shape_width_status; - uint8_t shape_width_edge_invalid_flags; - big_float32_buf_t shape_width_edge_mean; - big_float32_buf_t shape_width_edge_std; + big_uint16_buf_t status_sensor{}; + big_uint32_buf_t id{}; + big_uint16_buf_t age{}; + uint8_t status_measurement{}; + uint8_t status_movement{}; + big_uint16_buf_t position_invalid_flags{}; + uint8_t position_reference{}; + big_float32_buf_t position_x{}; + big_float32_buf_t position_x_std{}; + big_float32_buf_t position_y{}; + big_float32_buf_t position_y_std{}; + big_float32_buf_t position_z{}; + big_float32_buf_t position_z_std{}; + big_float32_buf_t position_covariance_xy{}; + big_float32_buf_t position_orientation{}; + big_float32_buf_t position_orientation_std{}; + uint8_t existence_invalid_flags{}; + big_float32_buf_t existence_probability{}; + big_float32_buf_t existence_ppv{}; + uint8_t classification_car{}; + uint8_t classification_truck{}; + uint8_t classification_motorcycle{}; + uint8_t classification_bicycle{}; + uint8_t classification_pedestrian{}; + uint8_t classification_animal{}; + uint8_t classification_hazard{}; + uint8_t classification_unknown{}; + uint8_t classification_overdrivable{}; + uint8_t classification_underdrivable{}; + uint8_t dynamics_abs_vel_invalid_flags{}; + big_float32_buf_t dynamics_abs_vel_x{}; + big_float32_buf_t dynamics_abs_vel_x_std{}; + big_float32_buf_t dynamics_abs_vel_y{}; + big_float32_buf_t dynamics_abs_vel_y_std{}; + big_float32_buf_t dynamics_abs_vel_covariance_xy{}; + uint8_t dynamics_rel_vel_invalid_flags{}; + big_float32_buf_t dynamics_rel_vel_x{}; + big_float32_buf_t dynamics_rel_vel_x_std{}; + big_float32_buf_t dynamics_rel_vel_y{}; + big_float32_buf_t dynamics_rel_vel_y_std{}; + big_float32_buf_t dynamics_rel_vel_covariance_xy{}; + uint8_t dynamics_abs_accel_invalid_flags{}; + big_float32_buf_t dynamics_abs_accel_x{}; + big_float32_buf_t dynamics_abs_accel_x_std{}; + big_float32_buf_t dynamics_abs_accel_y{}; + big_float32_buf_t dynamics_abs_accel_y_std{}; + big_float32_buf_t dynamics_abs_accel_covariance_xy{}; + uint8_t dynamics_rel_accel_invalid_flags{}; + big_float32_buf_t dynamics_rel_accel_x{}; + big_float32_buf_t dynamics_rel_accel_x_std{}; + big_float32_buf_t dynamics_rel_accel_y{}; + big_float32_buf_t dynamics_rel_accel_y_std{}; + big_float32_buf_t dynamics_rel_accel_covariance_xy{}; + uint8_t dynamics_orientation_invalid_flags{}; + big_float32_buf_t dynamics_orientation_rate_mean{}; + big_float32_buf_t dynamics_orientation_rate_std{}; + big_uint32_buf_t shape_length_status{}; + uint8_t shape_length_edge_invalid_flags{}; + big_float32_buf_t shape_length_edge_mean{}; + big_float32_buf_t shape_length_edge_std{}; + big_uint32_buf_t shape_width_status{}; + uint8_t shape_width_edge_invalid_flags{}; + big_float32_buf_t shape_width_edge_mean{}; + big_float32_buf_t shape_width_edge_std{}; }; struct ObjectListPacket { - HeaderPacket header; - HeaderSomeIPPacket header_some_ip; - HeaderE2EP07Packet header_e2ep07; - StampSyncStatusPacket stamp; - big_uint32_buf_t event_data_qualifier; - uint8_t extended_qualifier; - uint8_t number_of_objects; + HeaderPacket header{}; + HeaderSomeIPPacket header_some_ip{}; + HeaderE2EP07Packet header_e2ep07{}; + StampSyncStatusPacket stamp{}; + big_uint32_buf_t event_data_qualifier{}; + uint8_t extended_qualifier{}; + uint8_t number_of_objects{}; ObjectPacket objects[MAX_OBJECTS]; }; @@ -513,24 +506,24 @@ struct StatusConfigurationPacket struct SensorStatusPacket { - HeaderPacket header; - StampSyncStatusPacket stamp; - uint8_t sw_version_major; - uint8_t sw_version_minor; - uint8_t sw_version_patch; - StatusConfigurationPacket status; - uint8_t configuration_counter; - uint8_t longitudinal_velocity_status; - uint8_t longitudinal_acceleration_status; - uint8_t lateral_acceleration_status; - uint8_t yaw_rate_status; - uint8_t steering_angle_status; - uint8_t driving_direction_status; - uint8_t characteristic_speed_status; - uint8_t radar_status; - uint8_t voltage_status; - uint8_t temperature_status; - uint8_t blockage_status; + HeaderPacket header{}; + StampSyncStatusPacket stamp{}; + uint8_t sw_version_major{}; + uint8_t sw_version_minor{}; + uint8_t sw_version_patch{}; + StatusConfigurationPacket status{}; + uint8_t configuration_counter{}; + uint8_t longitudinal_velocity_status{}; + uint8_t longitudinal_acceleration_status{}; + uint8_t lateral_acceleration_status{}; + uint8_t yaw_rate_status{}; + uint8_t steering_angle_status{}; + uint8_t driving_direction_status{}; + uint8_t characteristic_speed_status{}; + uint8_t radar_status{}; + uint8_t voltage_status{}; + uint8_t temperature_status{}; + uint8_t blockage_status{}; }; struct ConfigurationPacket @@ -545,17 +538,17 @@ struct ConfigurationPacket struct AccelerationLateralCoGPacket { - HeaderPacket header; + HeaderPacket header{}; uint8_t reserved0[6]; - big_float32_buf_t acceleration_lateral; + big_float32_buf_t acceleration_lateral{}; uint8_t reserved1[22]; }; struct AccelerationLongitudinalCoGPacket { - HeaderPacket header; + HeaderPacket header{}; uint8_t reserved0[6]; - big_float32_buf_t acceleration_lateral; + big_float32_buf_t acceleration_lateral{}; uint8_t reserved1[22]; }; @@ -569,51 +562,51 @@ struct CharacteristicSpeedPacket struct DrivingDirectionPacket { - HeaderPacket header; - uint8_t reserved0; - uint8_t driving_direction; + HeaderPacket header{}; + uint8_t reserved0{}; + uint8_t driving_direction{}; uint8_t reserved1[20]; }; struct SteeringAngleFrontAxlePacket { - HeaderPacket header; + HeaderPacket header{}; uint8_t reserved0[6]; - big_float32_buf_t steering_angle_front_axle; + big_float32_buf_t steering_angle_front_axle{}; uint8_t reserved1[22]; }; struct VelocityVehiclePacket { - HeaderPacket header; + HeaderPacket header{}; uint8_t reserved0[3]; - big_float32_buf_t velocity_vehicle; + big_float32_buf_t velocity_vehicle{}; uint8_t reserved1[21]; }; struct YawRatePacket { - HeaderPacket header; + HeaderPacket header{}; uint8_t reserved0[6]; - big_float32_buf_t yaw_rate; + big_float32_buf_t yaw_rate{}; uint8_t reserved1[22]; }; struct FilterStatusEntryPacket { - uint8_t active; - uint8_t data_index; - big_float32_buf_t min_value; - big_float32_buf_t max_value; + uint8_t active{}; + uint8_t data_index{}; + big_float32_buf_t min_value{}; + big_float32_buf_t max_value{}; }; struct FilterStatusPacket { - HeaderPacket header; - StampSyncStatusPacket stamp; - uint8_t filter_configuration_counter; - uint8_t detection_sort_index; - uint8_t object_sort_index; + HeaderPacket header{}; + StampSyncStatusPacket stamp{}; + uint8_t filter_configuration_counter{}; + uint8_t detection_sort_index{}; + uint8_t object_sort_index{}; FilterStatusEntryPacket detection_filters[DETECTION_FILTER_PROPERTIES_NUM]; FilterStatusEntryPacket object_filters[OBJECT_FILTER_PROPERTIES_NUM]; }; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp index e4d600bc8..b492c0197 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp @@ -22,7 +22,6 @@ #include #include #include -#include #include #include @@ -94,8 +93,23 @@ class ContinentalArs548Decoder : public ContinentalPacketsDecoder std::function)> 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 msg)> - detection_list_callback_; + detection_list_callback_{}; std::function msg)> object_list_callback_; std::function sensor_status_callback_; @@ -105,8 +119,7 @@ class ContinentalArs548Decoder : public ContinentalPacketsDecoder ContinentalArs548Status radar_status_{}; /// @brief SensorConfiguration for this decoder - std::shared_ptr - sensor_configuration_; + std::shared_ptr config_ptr_{}; }; } // namespace continental_ars548 diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp index 2e9518312..0d3c2022c 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp @@ -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 -#include "nebula_msgs/msg/nebula_packet.hpp" -#include "nebula_msgs/msg/nebula_packets.hpp" +#include #include #include @@ -43,10 +42,10 @@ class ContinentalPacketsDecoder /// @return Current status virtual Status GetStatus() = 0; - /// @brief Virtual function for parsing NebulaPackets based on packet structure - /// @param nebula_packets + /// @brief Virtual function for parsing a NebulaPacket + /// @param packet_msg /// @return Resulting flag - virtual bool ProcessPacket(const nebula_msgs::msg::NebulaPacket & nebula_packet) = 0; + virtual bool ProcessPacket(std::unique_ptr packet_msg) = 0; }; } // namespace drivers } // namespace nebula diff --git a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp index 762048de8..07a1dc18a 100644 --- a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp @@ -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 +#include #include #include @@ -29,7 +28,7 @@ ContinentalArs548Decoder::ContinentalArs548Decoder( const std::shared_ptr & sensor_configuration) { - sensor_configuration_ = sensor_configuration; + config_ptr_ = sensor_configuration; } Status ContinentalArs548Decoder::GetStatus() @@ -121,9 +120,9 @@ bool ContinentalArs548Decoder::ParseDetectionsListPacket( 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 { @@ -246,9 +245,9 @@ bool ContinentalArs548Decoder::ParseObjectsListPacket( 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 { diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp index 1ffdb2c05..e341edd8e 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp @@ -14,18 +14,7 @@ #ifndef NEBULA_CONTINENTAL_ARS548_HW_INTERFACE_H #define NEBULA_CONTINENTAL_ARS548_HW_INTERFACE_H -// Have to define macros to silence warnings about deprecated headers being used by -// boost/property_tree/ in some versions of boost. -// See: https://github.com/boostorg/property_tree/issues/51 -#include -#if (BOOST_VERSION / 100 >= 1073 && BOOST_VERSION / 100 <= 1076) // Boost 1.73 - 1.76 -#define BOOST_BIND_GLOBAL_PLACEHOLDERS -#endif -#if (BOOST_VERSION / 100 == 1074) // Boost 1.74 -#define BOOST_ALLOW_DEPRECATED_HEADERS -#endif -#include -#include + #include #include #include @@ -33,12 +22,7 @@ #include -#include -#include -#include - #include -#include #include #include @@ -51,56 +35,10 @@ namespace continental_ars548 /// @brief Hardware interface of the Continental ARS548 radar class ContinentalArs548HwInterface { -private: - std::unique_ptr<::drivers::common::IoContext> sensor_io_context_; - std::unique_ptr<::drivers::udp_driver::UdpDriver> sensor_udp_driver_; - std::shared_ptr sensor_configuration_; - std::function &)> callback_; - - std::mutex sensor_status_mutex_; - - SensorStatusPacket sensor_status_packet_{}; - FilterStatusPacket filter_status_{}; - - std::shared_ptr parent_node_logger; - - /// @brief Printing the string to RCLCPP_INFO_STREAM - /// @param info Target string - void PrintInfo(std::string info); - - /// @brief Printing the string to RCLCPP_ERROR_STREAM - /// @param error Target string - void PrintError(std::string error); - - /// @brief Printing the string to RCLCPP_DEBUG_STREAM - /// @param debug Target string - void PrintDebug(std::string debug); - - /// @brief Printing the bytes to RCLCPP_DEBUG_STREAM - /// @param bytes Target byte vector - void PrintDebug(const std::vector & bytes); - public: /// @brief Constructor ContinentalArs548HwInterface(); - /// @brief Process a new filter status packet - /// @param buffer The buffer containing the status packet - void ProcessFilterStatusPacket(std::vector & buffer); - - /// @brief Process a new data packet - /// @param buffer The buffer containing the data packet - void ProcessDataPacket(std::vector & buffer); - - /// @brief Callback function to receive the Cloud Packet data from the UDP Driver - /// @param buffer Buffer containing the data received from the UDP socket - void ReceiveSensorPacketCallbackWithSender( - std::vector & buffer, const std::string & sender_ip); - - /// @brief Callback function to receive the Cloud Packet data from the UDP Driver - /// @param buffer Buffer containing the data received from the UDP socket - void ReceiveSensorPacketCallback(std::vector & buffer); - /// @brief Starting the interface that handles UDP streams /// @return Resulting status Status SensorInterfaceStart(); @@ -118,7 +56,8 @@ class ContinentalArs548HwInterface /// @brief Registering callback /// @param callback Callback function /// @return Resulting status - Status RegisterCallback(std::function &)> callback); + Status RegisterCallback( + std::function)> packet_callback); /// @brief Set the sensor mounting parameters /// @param longitudinal_autosar Desired longitudinal value in autosar coordinates @@ -201,6 +140,35 @@ class ContinentalArs548HwInterface /// @brief Setting rclcpp::Logger /// @param node Logger void SetLogger(std::shared_ptr node); + +private: + /// @brief Printing the string to RCLCPP_INFO_STREAM + /// @param info Target string + void PrintInfo(std::string info); + + /// @brief Printing the string to RCLCPP_ERROR_STREAM + /// @param error Target string + void PrintError(std::string error); + + /// @brief Printing the string to RCLCPP_DEBUG_STREAM + /// @param debug Target string + void PrintDebug(std::string debug); + + /// @brief Callback function to receive the Cloud Packet data from the UDP Driver + /// @param buffer Buffer containing the data received from the UDP socket + void ReceiveSensorPacketCallbackWithSender( + std::vector & buffer, const std::string & sender_ip); + + /// @brief Callback function to receive the Cloud Packet data from the UDP Driver + /// @param buffer Buffer containing the data received from the UDP socket + void ReceiveSensorPacketCallback(std::vector & buffer); + + std::unique_ptr<::drivers::common::IoContext> sensor_io_context_ptr_; + std::unique_ptr<::drivers::udp_driver::UdpDriver> sensor_udp_driver_ptr_; + std::shared_ptr config_ptr_; + std::function)> packet_callback_; + + std::shared_ptr parent_node_logger_ptr_; }; } // namespace continental_ars548 } // namespace drivers diff --git a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp index f4932e0eb..0a32ad185 100644 --- a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp @@ -16,8 +16,6 @@ #include #include -#include -#include namespace nebula { namespace drivers @@ -25,62 +23,60 @@ namespace drivers namespace continental_ars548 { ContinentalArs548HwInterface::ContinentalArs548HwInterface() -: sensor_io_context_{new ::drivers::common::IoContext(1)}, - sensor_udp_driver_{new ::drivers::udp_driver::UdpDriver(*sensor_io_context_)} +: sensor_io_context_ptr_{new ::drivers::common::IoContext(1)}, + sensor_udp_driver_ptr_{new ::drivers::udp_driver::UdpDriver(*sensor_io_context_ptr_)} { } Status ContinentalArs548HwInterface::SetSensorConfiguration( - std::shared_ptr sensor_configuration) + std::shared_ptr new_config_ptr) { - sensor_configuration_ = sensor_configuration; - + config_ptr_ = new_config_ptr; return Status::OK; } Status ContinentalArs548HwInterface::SensorInterfaceStart() { try { - sensor_udp_driver_->init_receiver( - sensor_configuration_->multicast_ip, sensor_configuration_->data_port, - sensor_configuration_->host_ip, sensor_configuration_->data_port, 2 << 16); - sensor_udp_driver_->receiver()->setMulticast(true); - sensor_udp_driver_->receiver()->open(); - sensor_udp_driver_->receiver()->bind(); - sensor_udp_driver_->receiver()->asyncReceiveWithSender(std::bind( + sensor_udp_driver_ptr_->init_receiver( + config_ptr_->multicast_ip, config_ptr_->data_port, config_ptr_->host_ip, + config_ptr_->data_port, 2 << 16); + sensor_udp_driver_ptr_->receiver()->setMulticast(true); + sensor_udp_driver_ptr_->receiver()->open(); + sensor_udp_driver_ptr_->receiver()->bind(); + sensor_udp_driver_ptr_->receiver()->asyncReceiveWithSender(std::bind( &ContinentalArs548HwInterface::ReceiveSensorPacketCallbackWithSender, this, std::placeholders::_1, std::placeholders::_2)); - sensor_udp_driver_->init_sender( - sensor_configuration_->sensor_ip, sensor_configuration_->configuration_sensor_port, - sensor_configuration_->host_ip, sensor_configuration_->configuration_host_port); + sensor_udp_driver_ptr_->init_sender( + config_ptr_->sensor_ip, config_ptr_->configuration_sensor_port, config_ptr_->host_ip, + config_ptr_->configuration_host_port); - sensor_udp_driver_->sender()->open(); - sensor_udp_driver_->sender()->bind(); + sensor_udp_driver_ptr_->sender()->open(); + sensor_udp_driver_ptr_->sender()->bind(); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } } catch (const std::exception & ex) { Status status = Status::UDP_CONNECTION_ERROR; - std::cerr << status << sensor_configuration_->sensor_ip << "," - << sensor_configuration_->data_port << std::endl; + std::cerr << status << config_ptr_->sensor_ip << "," << config_ptr_->data_port << std::endl; return status; } return Status::OK; } Status ContinentalArs548HwInterface::RegisterCallback( - std::function &)> callback) + std::function)> packet_callback) { - callback_ = std::move(callback); + packet_callback_ = std::move(packet_callback); return Status::OK; } void ContinentalArs548HwInterface::ReceiveSensorPacketCallbackWithSender( std::vector & buffer, const std::string & sender_ip) { - if (sender_ip == sensor_configuration_->sensor_ip) { + if (sender_ip == config_ptr_->sensor_ip) { ReceiveSensorPacketCallback(buffer); } } @@ -91,71 +87,16 @@ void ContinentalArs548HwInterface::ReceiveSensorPacketCallback(std::vector(now.time_since_epoch()).count(); - ProcessDataPacket(buffer); - } else if (header_packet.method_id.value() == OBJECT_LIST_METHOD_ID) { - if ( - buffer.size() != OBJECT_LIST_UDP_PAYLOAD || - header_packet.length.value() != OBJECT_LIST_PDU_LENGTH) { - PrintError("ObjectList message with invalid size"); - return; - } + auto msg_ptr = std::make_unique(); + msg_ptr->stamp.sec = static_cast(timestamp_ns / 1'000'000'000); + msg_ptr->stamp.nanosec = static_cast(timestamp_ns % 1'000'000'000); + msg_ptr->data.swap(buffer); - ProcessDataPacket(buffer); - } -} - -void ContinentalArs548HwInterface::ProcessFilterStatusPacket(std::vector & buffer) -{ - assert(buffer.size() == sizeof(FilterStatusPacket)); - std::memcpy(&filter_status_, buffer.data(), sizeof(FilterStatusPacket)); -} - -void ContinentalArs548HwInterface::ProcessDataPacket(std::vector & buffer) -{ - /* auto nebula_packet_ptr = std::make_unique(); - nebula_packet_ptr->data = buffer; - auto now = std::chrono::system_clock::now(); - auto now_secs = - std::chrono::duration_cast(now.time_since_epoch()).count(); auto - now_nanosecs = - std::chrono::duration_cast(now.time_since_epoch()).count(); - nebula_packet_ptr->stamp.sec = static_cast(now_secs); - nebula_packet_ptr->stamp.nanosec = - static_cast((now_nanosecs / 1000000000.0 - static_cast(now_secs)) * 1000000000); - */ - - callback_(buffer); + packet_callback_(std::move(msg_ptr)); } Status ContinentalArs548HwInterface::SensorInterfaceStop() @@ -198,7 +139,7 @@ Status ContinentalArs548HwInterface::SetSensorMounting( PrintInfo("pitch_autosar = " + std::to_string(pitch_autosar)); PrintInfo("plug_orientation = " + std::to_string(plug_orientation)); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } @@ -237,7 +178,7 @@ Status ContinentalArs548HwInterface::SetVehicleParameters( PrintInfo("height_autosar = " + std::to_string(height_autosar)); PrintInfo("wheel_base_autosar = " + std::to_string(wheel_base_autosar)); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } @@ -281,7 +222,7 @@ Status ContinentalArs548HwInterface::SetRadarParameters( PrintInfo("hcc = " + std::to_string(hcc)); PrintInfo("power_save_standstill = " + std::to_string(power_save_standstill)); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } @@ -322,7 +263,7 @@ Status ContinentalArs548HwInterface::SetSensorIPAddress(const std::string & sens std::vector send_vector(sizeof(ConfigurationPacket)); std::memcpy(send_vector.data(), &configuration, sizeof(ConfigurationPacket)); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } @@ -353,7 +294,7 @@ Status ContinentalArs548HwInterface::SetAccelerationLateralCog(float lateral_acc std::vector send_vector(sizeof(AccelerationLateralCoGPacket)); std::memcpy(send_vector.data(), &acceleration_lateral_cog, sizeof(AccelerationLateralCoGPacket)); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } @@ -386,7 +327,7 @@ Status ContinentalArs548HwInterface::SetAccelerationLongitudinalCog(float longit std::memcpy( send_vector.data(), &acceleration_longitudinal_cog, sizeof(AccelerationLongitudinalCoGPacket)); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } @@ -417,7 +358,7 @@ Status ContinentalArs548HwInterface::SetCharacteristicSpeed(float characteristic std::vector send_vector(sizeof(CharacteristicSpeedPacket)); std::memcpy(send_vector.data(), &characteristic_speed_packet, sizeof(CharacteristicSpeedPacket)); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } @@ -453,7 +394,7 @@ Status ContinentalArs548HwInterface::SetDrivingDirection(int direction) std::vector send_vector(sizeof(DrivingDirectionPacket)); std::memcpy(send_vector.data(), &driving_direction_packet, sizeof(DrivingDirectionPacket)); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } @@ -485,7 +426,7 @@ Status ContinentalArs548HwInterface::SetSteeringAngleFrontAxle(float angle_rad) std::memcpy( send_vector.data(), &steering_angle_front_axle_packet, sizeof(SteeringAngleFrontAxlePacket)); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } @@ -516,7 +457,7 @@ Status ContinentalArs548HwInterface::SetVelocityVehicle(float velocity_kmh) std::vector send_vector(sizeof(VelocityVehiclePacket)); std::memcpy(send_vector.data(), &steering_angle_front_axle_packet, sizeof(VelocityVehiclePacket)); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } @@ -547,7 +488,7 @@ Status ContinentalArs548HwInterface::SetYawRate(float yaw_rate) std::vector send_vector(sizeof(YawRatePacket)); std::memcpy(send_vector.data(), &yaw_rate_packet, sizeof(YawRatePacket)); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } @@ -558,13 +499,13 @@ Status ContinentalArs548HwInterface::SetYawRate(float yaw_rate) void ContinentalArs548HwInterface::SetLogger(std::shared_ptr logger) { - parent_node_logger = logger; + parent_node_logger_ptr_ = logger; } void ContinentalArs548HwInterface::PrintInfo(std::string info) { - if (parent_node_logger) { - RCLCPP_INFO_STREAM((*parent_node_logger), info); + if (parent_node_logger_ptr_) { + RCLCPP_INFO_STREAM((*parent_node_logger_ptr_), info); } else { std::cout << info << std::endl; } @@ -572,8 +513,8 @@ void ContinentalArs548HwInterface::PrintInfo(std::string info) void ContinentalArs548HwInterface::PrintError(std::string error) { - if (parent_node_logger) { - RCLCPP_ERROR_STREAM((*parent_node_logger), error); + if (parent_node_logger_ptr_) { + RCLCPP_ERROR_STREAM((*parent_node_logger_ptr_), error); } else { std::cerr << error << std::endl; } @@ -581,23 +522,13 @@ void ContinentalArs548HwInterface::PrintError(std::string error) void ContinentalArs548HwInterface::PrintDebug(std::string debug) { - if (parent_node_logger) { - RCLCPP_DEBUG_STREAM((*parent_node_logger), debug); + if (parent_node_logger_ptr_) { + RCLCPP_DEBUG_STREAM((*parent_node_logger_ptr_), debug); } else { std::cout << debug << std::endl; } } -void ContinentalArs548HwInterface::PrintDebug(const std::vector & bytes) -{ - std::stringstream ss; - for (const auto & b : bytes) { - ss << static_cast(b) << ", "; - } - ss << std::endl; - PrintDebug(ss.str()); -} - } // namespace continental_ars548 } // namespace drivers } // namespace nebula diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp index 5f56aa669..d7f61bc1e 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp @@ -34,7 +34,6 @@ #include #include -#include #include #include #include @@ -131,7 +130,7 @@ class ContinentalArs548DecoderWrapper rclcpp::Logger logger_; std::shared_ptr - sensor_cfg_{}; + config_ptr_{}; std::shared_ptr driver_ptr_{}; std::mutex mtx_driver_ptr_; diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp index 4250a5031..24f84d055 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp @@ -45,7 +45,7 @@ class ContinentalArs548HwInterfaceWrapper void OnConfigChange( const std::shared_ptr & - new_config); + new_config_ptr); /// @brief Get current status of the hw interface /// @return Current status @@ -103,26 +103,27 @@ class ContinentalArs548HwInterfaceWrapper response); rclcpp::Node * const parent_node_; - std::shared_ptr hw_interface_; + std::shared_ptr hw_interface_{}; rclcpp::Logger logger_; - nebula::Status status_; + nebula::Status status_{}; std::shared_ptr - config_; + config_ptr_{}; - rclcpp::Subscription::SharedPtr odometry_sub_; - rclcpp::Subscription::SharedPtr acceleration_sub_; - rclcpp::Subscription::SharedPtr steering_angle_sub_; + rclcpp::Subscription::SharedPtr odometry_sub_{}; + rclcpp::Subscription::SharedPtr + acceleration_sub_{}; + rclcpp::Subscription::SharedPtr steering_angle_sub_{}; bool standstill_{true}; rclcpp::Service::SharedPtr - set_network_configuration_service_server_; + set_network_configuration_service_server_{}; rclcpp::Service::SharedPtr - set_sensor_mounting_service_server_; + set_sensor_mounting_service_server_{}; rclcpp::Service::SharedPtr - set_vehicle_parameters_service_server_; + set_vehicle_parameters_service_server_{}; rclcpp::Service::SharedPtr - set_radar_parameters_service_server_; + set_radar_parameters_service_server_{}; }; } // namespace ros } // namespace nebula diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp index ebff025df..86f309f13 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp @@ -14,8 +14,6 @@ #pragma once -#include -#include #include #include #include @@ -31,8 +29,6 @@ #include #include -#include -#include #include #include #include @@ -61,7 +57,7 @@ class ContinentalArs548RosWrapper final : public rclcpp::Node private: /// @brief Callback from the hw interface's raw data - void ReceivePacketCallback(std::vector & packet); + void ReceivePacketCallback(std::unique_ptr msg_ptr); /// @brief Callback from replayed NebulaPackets void ReceivePacketsMessageCallback(std::unique_ptr packets_msg); @@ -83,7 +79,7 @@ class ContinentalArs548RosWrapper final : public rclcpp::Node Status wrapper_status_; std::shared_ptr - sensor_cfg_ptr_{}; + config_ptr_{}; /// @brief Stores received packets that have not been processed yet by the decoder thread mt_queue> packet_queue_; @@ -99,7 +95,7 @@ class ContinentalArs548RosWrapper final : public rclcpp::Node std::mutex mtx_config_; - OnSetParametersCallbackHandle::SharedPtr parameter_event_cb_; + OnSetParametersCallbackHandle::SharedPtr parameter_event_cb_{}; }; } // namespace ros diff --git a/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp index 0f73db190..ed1a0f577 100644 --- a/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp @@ -23,21 +23,21 @@ namespace ros ContinentalArs548DecoderWrapper::ContinentalArs548DecoderWrapper( rclcpp::Node * const parent_node, std::shared_ptr & - config, + config_ptr, bool launch_hw) : status_(nebula::Status::NOT_INITIALIZED), logger_(parent_node->get_logger().get_child("ContinentalArs548Decoder")), - sensor_cfg_(config) + config_ptr_(config_ptr) { using std::chrono_literals::operator""us; - if (!config) { + if (!config_ptr) { throw std::runtime_error( "ContinentalArs548DecoderWrapper cannot be instantiated without a valid config!"); } RCLCPP_INFO(logger_, "Starting Decoder"); - InitializeDriver(config); + InitializeDriver(config_ptr); status_ = driver_ptr_->GetStatus(); if (Status::OK != status_) { @@ -110,11 +110,12 @@ Status ContinentalArs548DecoderWrapper::InitializeDriver( void ContinentalArs548DecoderWrapper::OnConfigChange( const std::shared_ptr< - const nebula::drivers::continental_ars548::ContinentalArs548SensorConfiguration> & new_config) + const nebula::drivers::continental_ars548::ContinentalArs548SensorConfiguration> & + new_config_ptr) { std::lock_guard lock(mtx_driver_ptr_); - InitializeDriver(new_config); - sensor_cfg_ = new_config; + InitializeDriver(new_config_ptr); + config_ptr_ = new_config_ptr; } void ContinentalArs548DecoderWrapper::ProcessPacket( @@ -196,14 +197,14 @@ void ContinentalArs548DecoderWrapper::SensorStatusCallback( diagnostic_msgs::msg::DiagnosticArray diagnostic_array_msg; diagnostic_array_msg.header.stamp.sec = sensor_status.timestamp_seconds; diagnostic_array_msg.header.stamp.nanosec = sensor_status.timestamp_nanoseconds; - diagnostic_array_msg.header.frame_id = sensor_cfg_->frame_id; + diagnostic_array_msg.header.frame_id = config_ptr_->frame_id; diagnostic_array_msg.status.resize(1); auto & status = diagnostic_array_msg.status[0]; status.values.reserve(36); status.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - status.hardware_id = sensor_cfg_->frame_id; - status.name = sensor_cfg_->frame_id; + status.hardware_id = config_ptr_->frame_id; + status.name = config_ptr_->frame_id; status.message = "Diagnostic messages from ARS548"; auto add_diagnostic = [&status](const std::string & key, const std::string & value) { @@ -592,7 +593,7 @@ visualization_msgs::msg::MarkerArray ContinentalArs548DecoderWrapper::ConvertToM current_ids.emplace(object.object_id); visualization_msgs::msg::Marker box_marker; - box_marker.header.frame_id = sensor_cfg_->object_frame; + box_marker.header.frame_id = config_ptr_->object_frame; box_marker.header.stamp = msg.header.stamp; box_marker.ns = "boxes"; box_marker.id = object.object_id; @@ -673,7 +674,7 @@ visualization_msgs::msg::MarkerArray ContinentalArs548DecoderWrapper::ConvertToM } visualization_msgs::msg::Marker delete_marker; - delete_marker.header.frame_id = sensor_cfg_->object_frame; + delete_marker.header.frame_id = config_ptr_->object_frame; delete_marker.header.stamp = msg.header.stamp; delete_marker.ns = "boxes"; delete_marker.id = previous_id; diff --git a/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp index bf4974a69..7cbfef7ef 100644 --- a/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp @@ -25,17 +25,17 @@ namespace ros ContinentalArs548HwInterfaceWrapper::ContinentalArs548HwInterfaceWrapper( rclcpp::Node * const parent_node, std::shared_ptr & - config) + config_ptr) : parent_node_(parent_node), hw_interface_( std::make_shared()), logger_(parent_node->get_logger().get_child("HwInterfaceWrapper")), status_(Status::NOT_INITIALIZED), - config_(config) + config_ptr_(config_ptr) { hw_interface_->SetLogger( std::make_shared(parent_node->get_logger().get_child("HwInterface"))); - status_ = hw_interface_->SetSensorConfiguration(config); + status_ = hw_interface_->SetSensorConfiguration(config_ptr); if (status_ != Status::OK) { throw std::runtime_error( @@ -101,10 +101,11 @@ void ContinentalArs548HwInterfaceWrapper::SensorInterfaceStart() void ContinentalArs548HwInterfaceWrapper::OnConfigChange( const std::shared_ptr< - const nebula::drivers::continental_ars548::ContinentalArs548SensorConfiguration> & new_config) + const nebula::drivers::continental_ars548::ContinentalArs548SensorConfiguration> & + new_config_ptr_ptr) { - hw_interface_->SetSensorConfiguration(new_config); - config_ = new_config; + hw_interface_->SetSensorConfiguration(new_config_ptr_ptr); + config_ptr_ = new_config_ptr_ptr; } Status ContinentalArs548HwInterfaceWrapper::Status() @@ -191,12 +192,12 @@ void ContinentalArs548HwInterfaceWrapper::SetSensorMountingRequestCallback( geometry_msgs::msg::TransformStamped base_to_sensor_tf; try { base_to_sensor_tf = tf_buffer->lookupTransform( - config_->base_frame, config_->frame_id, rclcpp::Time(0), + config_ptr_->base_frame, config_ptr_->frame_id, rclcpp::Time(0), rclcpp::Duration::from_seconds(0.5)); } catch (tf2::TransformException & ex) { RCLCPP_ERROR( logger_, "Could not obtain the transform from the base frame to %s (%s)", - config_->frame_id.c_str(), ex.what()); + config_ptr_->frame_id.c_str(), ex.what()); response->success = false; response->message = ex.what(); return; @@ -207,7 +208,7 @@ void ContinentalArs548HwInterfaceWrapper::SetSensorMountingRequestCallback( tf2::Matrix3x3(tf2::Quaternion(quat.x, quat.y, quat.z, quat.w)).getRPY(rpy.x, rpy.y, rpy.z); longitudinal = - base_to_sensor_tf.transform.translation.x - config_->configuration_vehicle_wheelbase; + base_to_sensor_tf.transform.translation.x - config_ptr_->configuration_vehicle_wheelbase; lateral = base_to_sensor_tf.transform.translation.y; vertical = base_to_sensor_tf.transform.translation.z; yaw = rpy.z; @@ -236,29 +237,29 @@ void ContinentalArs548HwInterfaceWrapper::SetVehicleParametersRequestCallback( if (vehicle_length < 0.f) { RCLCPP_INFO( logger_, "Service vehicle_length is invalid. Falling back to configuration value (%.2f)", - config_->configuration_vehicle_length); - vehicle_length = config_->configuration_vehicle_length; + config_ptr_->configuration_vehicle_length); + vehicle_length = config_ptr_->configuration_vehicle_length; } if (vehicle_width < 0.f) { RCLCPP_INFO( logger_, "Service vehicle_width is invalid. Falling back to configuration value (%.2f)", - config_->configuration_vehicle_width); - vehicle_width = config_->configuration_vehicle_width; + config_ptr_->configuration_vehicle_width); + vehicle_width = config_ptr_->configuration_vehicle_width; } if (vehicle_height < 0.f) { RCLCPP_INFO( logger_, "Service vehicle_height is invalid. Falling back to configuration value (%.2f)", - config_->configuration_vehicle_height); - vehicle_height = config_->configuration_vehicle_height; + config_ptr_->configuration_vehicle_height); + vehicle_height = config_ptr_->configuration_vehicle_height; } if (vehicle_wheelbase < 0.f) { RCLCPP_INFO( logger_, "Service vehicle_wheelbase is invalid. Falling back to configuration value (%.2f)", - config_->configuration_vehicle_wheelbase); - vehicle_wheelbase = config_->configuration_vehicle_wheelbase; + config_ptr_->configuration_vehicle_wheelbase); + vehicle_wheelbase = config_ptr_->configuration_vehicle_wheelbase; } auto result = hw_interface_->SetVehicleParameters( diff --git a/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp index fb7fcbcd9..ec429bf05 100644 --- a/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp @@ -22,7 +22,6 @@ ContinentalArs548RosWrapper::ContinentalArs548RosWrapper(const rclcpp::NodeOptio : rclcpp::Node( "continental_ars548_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)), wrapper_status_(Status::NOT_INITIALIZED), - sensor_cfg_ptr_(nullptr), packet_queue_(3000), hw_interface_wrapper_(), decoder_wrapper_() @@ -36,15 +35,15 @@ ContinentalArs548RosWrapper::ContinentalArs548RosWrapper(const rclcpp::NodeOptio (std::stringstream{} << "Sensor configuration invalid: " << wrapper_status_).str()); } - RCLCPP_INFO_STREAM(get_logger(), "SensorConfig:" << *sensor_cfg_ptr_); + RCLCPP_INFO_STREAM(get_logger(), "SensorConfig:" << *config_ptr_); launch_hw_ = declare_parameter("launch_hw", param_read_only()); if (launch_hw_) { - hw_interface_wrapper_.emplace(this, sensor_cfg_ptr_); + hw_interface_wrapper_.emplace(this, config_ptr_); } - decoder_wrapper_.emplace(this, sensor_cfg_ptr_, launch_hw_); + decoder_wrapper_.emplace(this, config_ptr_, launch_hw_); RCLCPP_DEBUG(get_logger(), "Starting stream"); @@ -223,31 +222,31 @@ nebula::Status ContinentalArs548RosWrapper::DeclareAndGetSensorConfigParams() return Status::INVALID_SENSOR_MODEL; } - auto new_cfg_ptr = std::make_shared< + auto new_config_ptr = std::make_shared< const nebula::drivers::continental_ars548::ContinentalArs548SensorConfiguration>(config); - return ValidateAndSetConfig(new_cfg_ptr); + return ValidateAndSetConfig(new_config_ptr); } Status ContinentalArs548RosWrapper::ValidateAndSetConfig( std::shared_ptr & - new_config) + new_config_ptr) { - if (new_config->sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + if (new_config_ptr->sensor_model == nebula::drivers::SensorModel::UNKNOWN) { return Status::INVALID_SENSOR_MODEL; } - if (new_config->frame_id.empty()) { + if (new_config_ptr->frame_id.empty()) { return Status::SENSOR_CONFIG_ERROR; } if (hw_interface_wrapper_) { - hw_interface_wrapper_->OnConfigChange(new_config); + hw_interface_wrapper_->OnConfigChange(new_config_ptr); } if (decoder_wrapper_) { - decoder_wrapper_->OnConfigChange(new_config); + decoder_wrapper_->OnConfigChange(new_config_ptr); } - sensor_cfg_ptr_ = new_config; + config_ptr_ = new_config_ptr; return Status::OK; } @@ -304,25 +303,26 @@ rcl_interfaces::msg::SetParametersResult ContinentalArs548RosWrapper::OnParamete RCLCPP_INFO(get_logger(), "OnParameterChange"); - drivers::continental_ars548::ContinentalArs548SensorConfiguration new_cfg(*sensor_cfg_ptr_); + drivers::continental_ars548::ContinentalArs548SensorConfiguration new_config(*config_ptr_); bool got_any = - get_param(p, "frame_id", new_cfg.frame_id) | get_param(p, "base_frame", new_cfg.base_frame) | - get_param(p, "object_frame", new_cfg.object_frame) | - get_param(p, "configuration_vehicle_length", new_cfg.configuration_vehicle_length) | - get_param(p, "configuration_vehicle_width", new_cfg.configuration_vehicle_width) | - get_param(p, "configuration_vehicle_height", new_cfg.configuration_vehicle_height) | - get_param(p, "configuration_vehicle_wheelbase", new_cfg.configuration_vehicle_wheelbase) | - get_param(p, "configuration_host_port", new_cfg.configuration_host_port) | - get_param(p, "configuration_sensor_port", new_cfg.configuration_sensor_port); + get_param(p, "frame_id", new_config.frame_id) | + get_param(p, "base_frame", new_config.base_frame) | + get_param(p, "object_frame", new_config.object_frame) | + get_param(p, "configuration_vehicle_length", new_config.configuration_vehicle_length) | + get_param(p, "configuration_vehicle_width", new_config.configuration_vehicle_width) | + get_param(p, "configuration_vehicle_height", new_config.configuration_vehicle_height) | + get_param(p, "configuration_vehicle_wheelbase", new_config.configuration_vehicle_wheelbase) | + get_param(p, "configuration_host_port", new_config.configuration_host_port) | + get_param(p, "configuration_sensor_port", new_config.configuration_sensor_port); if (!got_any) { return rcl_interfaces::build().successful(true).reason(""); } - auto new_cfg_ptr = std::make_shared< - const nebula::drivers::continental_ars548::ContinentalArs548SensorConfiguration>(new_cfg); - auto status = ValidateAndSetConfig(new_cfg_ptr); + auto new_config_ptr = std::make_shared< + const nebula::drivers::continental_ars548::ContinentalArs548SensorConfiguration>(new_config); + auto status = ValidateAndSetConfig(new_config_ptr); if (status != Status::OK) { RCLCPP_WARN_STREAM(get_logger(), "OnParameterChange aborted: " << status); @@ -335,21 +335,13 @@ rcl_interfaces::msg::SetParametersResult ContinentalArs548RosWrapper::OnParamete return rcl_interfaces::build().successful(true).reason(""); } -void ContinentalArs548RosWrapper::ReceivePacketCallback(std::vector & packet) +void ContinentalArs548RosWrapper::ReceivePacketCallback( + std::unique_ptr msg_ptr) { if (!decoder_wrapper_ || decoder_wrapper_->Status() != Status::OK) { return; } - const auto now = std::chrono::high_resolution_clock::now(); - const auto timestamp_ns = - std::chrono::duration_cast(now.time_since_epoch()).count(); - - auto msg_ptr = std::make_unique(); - msg_ptr->stamp.sec = static_cast(timestamp_ns / 1'000'000'000); - msg_ptr->stamp.nanosec = static_cast(timestamp_ns % 1'000'000'000); - msg_ptr->data.swap(packet); - if (!packet_queue_.try_push(std::move(msg_ptr))) { RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped"); } diff --git a/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp b/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp index 7cfb75999..6d3f756bc 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp +++ b/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp @@ -30,6 +30,7 @@ #include #include #include +#include namespace nebula { @@ -278,9 +279,9 @@ void ContinentalRosDecoderTest::ReadBag() ASSERT_EQ(1, extracted_msg.packets.size()); - // auto extracted_msg_ptr = - // std::make_shared(extracted_msg); - driver_ptr_->ProcessPacket(extracted_msg.packets[0]); + auto extracted_msg_ptr = + std::make_unique(extracted_msg.packets[0]); + driver_ptr_->ProcessPacket(std::move(extracted_msg_ptr)); } } } From 3a7e0fcf5faab04e93a6d2dc2f7c379ab1dceb40 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 23 May 2024 04:31:50 +0900 Subject: [PATCH 04/47] chore: finished the refactoring. old bags will become unusable after this. need to test with actual hardware Signed-off-by: Kenzo Lobos-Tsunekawa --- .../decoders/continental_srr520_decoder.hpp | 19 ++- .../decoders/continental_srr520_decoder.cpp | 26 ++-- nebula_hw_interfaces/CMakeLists.txt | 1 + .../continental_srr520_hw_interface.hpp | 122 ++++++------------ .../continental_srr520_hw_interface.cpp | 117 ++++++----------- ...continental_ars548_decoder_ros_wrapper.hpp | 9 -- .../continental_ars548_decoder_wrapper.hpp | 13 +- .../continental_ars548_ros_wrapper.hpp | 4 +- ...continental_srr520_decoder_ros_wrapper.hpp | 9 -- .../continental_srr520_decoder_wrapper.hpp | 32 +---- ...nental_srr520_hw_interface_ros_wrapper.hpp | 2 +- ...ontinental_srr520_hw_interface_wrapper.hpp | 10 +- .../continental_srr520_ros_wrapper.hpp | 12 +- .../continental_ars548_decoder_wrapper.cpp | 7 +- .../continental_ars548_ros_wrapper.cpp | 37 +++--- .../continental_srr520_decoder_wrapper.cpp | 10 +- .../continental_srr520_ros_wrapper.cpp | 56 ++++---- .../continental_ros_decoder_test_srr520.cpp | 7 +- 18 files changed, 175 insertions(+), 318 deletions(-) diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp index 3e09ffaa9..d06df9d51 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp @@ -23,10 +23,10 @@ #include #include #include -#include #include #include +#include #include namespace nebula @@ -53,10 +53,6 @@ class ContinentalSrr520Decoder : public ContinentalPacketsDecoder /// @return Resulting flag bool ProcessPacket(std::unique_ptr packet_msg) override; - bool ParseDetectionsListPacket(const nebula_msgs::msg::NebulaPackets & nebula_packets, bool near); - bool ParseObjectsListPacket(const nebula_msgs::msg::NebulaPackets & nebula_packets); - bool ParseStatusPacket(const nebula_msgs::msg::NebulaPackets & nebula_packets); - /// @brief Register function to call whenever a new rdi near detection list is processed /// @param detection_list_callback /// @return Resulting status @@ -87,7 +83,8 @@ class ContinentalSrr520Decoder : public ContinentalPacketsDecoder /// @brief Register function to call whenever a sync fup packet is processed /// @param sync_fup_callback /// @return Resulting status - Status RegisterSyncFupCallback(std::function sync_fup_callback); + Status RegisterSyncFupCallback( + std::function sync_fup_callback); /// @brief Register function to call whenever enough packets have been processed /// @param object_list_callback @@ -181,7 +178,7 @@ class ContinentalSrr520Decoder : public ContinentalPacketsDecoder object_list_callback_{}; std::function msg)> status_callback_{}; - std::function sync_fup_callback_{}; + std::function sync_fup_callback_{}; std::function msg)> nebula_packets_callback_{}; @@ -197,15 +194,15 @@ class ContinentalSrr520Decoder : public ContinentalPacketsDecoder bool first_rdi_hrr_packet_{true}; bool first_object_packet_{true}; - ScanHeaderPacket rdi_near_header_packet_; - ScanHeaderPacket rdi_hrr_header_packet_; - ObjectHeaderPacket object_header_packet_; + ScanHeaderPacket rdi_near_header_packet_{}; + ScanHeaderPacket rdi_hrr_header_packet_{}; + ObjectHeaderPacket object_header_packet_{}; /// @brief SensorConfiguration for this decoder std::shared_ptr sensor_configuration_{}; - std::shared_ptr parent_node_logger; + std::shared_ptr parent_node_logger_ptr_{}; }; } // namespace continental_srr520 diff --git a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp index 761a505f2..37c9ecf08 100644 --- a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp @@ -72,7 +72,8 @@ Status ContinentalSrr520Decoder::RegisterStatusCallback( return Status::OK; } -Status ContinentalSrr520Decoder::RegisterSyncFupCallback(std::function sync_fup_callback) +Status ContinentalSrr520Decoder::RegisterSyncFupCallback( + std::function sync_fup_callback) { sync_fup_callback_ = std::move(sync_fup_callback); return Status::OK; @@ -1076,7 +1077,7 @@ void ContinentalSrr520Decoder::ProcessSyncFupPacket( std::unique_ptr packet_msg) { if (sync_fup_callback_) { - sync_fup_callback_(); + sync_fup_callback_(packet_msg->stamp); } auto nebula_packets = std::make_unique(); @@ -1089,22 +1090,15 @@ void ContinentalSrr520Decoder::ProcessSyncFupPacket( } } -bool ContinentalSrr520Decoder::ParseStatusPacket( - const nebula_msgs::msg::NebulaPackets & nebula_packets) -{ - assert(false); - return true; -} - void ContinentalSrr520Decoder::SetLogger(std::shared_ptr logger) { - parent_node_logger = logger; + parent_node_logger_ptr_ = logger; } void ContinentalSrr520Decoder::PrintInfo(std::string info) { - if (parent_node_logger) { - RCLCPP_INFO_STREAM((*parent_node_logger), info); + if (parent_node_logger_ptr_) { + RCLCPP_INFO_STREAM((*parent_node_logger_ptr_), info); } else { std::cout << info << std::endl; } @@ -1112,8 +1106,8 @@ void ContinentalSrr520Decoder::PrintInfo(std::string info) void ContinentalSrr520Decoder::PrintError(std::string error) { - if (parent_node_logger) { - RCLCPP_ERROR_STREAM((*parent_node_logger), error); + if (parent_node_logger_ptr_) { + RCLCPP_ERROR_STREAM((*parent_node_logger_ptr_), error); } else { std::cerr << error << std::endl; } @@ -1121,8 +1115,8 @@ void ContinentalSrr520Decoder::PrintError(std::string error) void ContinentalSrr520Decoder::PrintDebug(std::string debug) { - if (parent_node_logger) { - RCLCPP_DEBUG_STREAM((*parent_node_logger), debug); + if (parent_node_logger_ptr_) { + RCLCPP_DEBUG_STREAM((*parent_node_logger_ptr_), debug); } else { std::cout << debug << std::endl; } diff --git a/nebula_hw_interfaces/CMakeLists.txt b/nebula_hw_interfaces/CMakeLists.txt index 7cd8b8982..9f26318ad 100644 --- a/nebula_hw_interfaces/CMakeLists.txt +++ b/nebula_hw_interfaces/CMakeLists.txt @@ -40,6 +40,7 @@ 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/continental_srr520_hw_interface.cpp ) diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp index ff1724c31..fbc535547 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp @@ -14,19 +14,7 @@ #ifndef NEBULA_CONTINENTAL_SRR520_HW_INTERFACE_H #define NEBULA_CONTINENTAL_SRR520_HW_INTERFACE_H -// Have to define macros to silence warnings about deprecated headers being used by -// boost/property_tree/ in some versions of boost. -// See: https://github.com/boostorg/property_tree/issues/51 -#include -#if (BOOST_VERSION / 100 >= 1073 && BOOST_VERSION / 100 <= 1076) // Boost 1.73 - 1.76 -#define BOOST_BIND_GLOBAL_PLACEHOLDERS -#endif -#if (BOOST_VERSION / 100 == 1074) // Boost 1.74 -#define BOOST_ALLOW_DEPRECATED_HEADERS -#endif -#include -#include -#include + #include #include #include @@ -36,10 +24,6 @@ #include #include -#include -#include -#include - #include #include #include @@ -54,65 +38,6 @@ namespace continental_srr520 /// @brief Hardware interface of the Continental SRR520 radar class ContinentalSrr520HwInterface { -private: - // std::unique_ptr<::drivers::common::IoContext> sensor_io_context_; - // std::unique_ptr<::drivers::udp_driver::UdpDriver> sensor_udp_driver_; - - std::unique_ptr<::drivers::socketcan::SocketCanReceiver> can_receiver_; - std::unique_ptr<::drivers::socketcan::SocketCanSender> can_sender_; - std::unique_ptr receiver_thread_; - - std::shared_ptr sensor_configuration_; - std::unique_ptr nebula_packets_ptr_; - std::function buffer)> - nebula_packet_callback_; - - std::mutex sensor_status_mutex_; - std::mutex receiver_mutex_; - bool sensor_interface_active_{}; - - uint8_t sync_counter_{0}; - bool sync_fup_sent_{true}; - builtin_interfaces::msg::Time last_sync_stamp_; - - std::shared_ptr parent_node_logger; - - /// @brief Send a Fd frame - /// @param data a buffer containing the data to send - template - bool SendFrame(const std::array & data, int can_frame_id); - - /// @brief Printing the string to RCLCPP_INFO_STREAM - /// @param info Target string - void PrintInfo(std::string info); - - /// @brief Printing the string to RCLCPP_ERROR_STREAM - /// @param error Target string - void PrintError(std::string error); - - /// @brief Printing the string to RCLCPP_DEBUG_STREAM - /// @param debug Target string - void PrintDebug(std::string debug); - - /// @brief Printing the bytes to RCLCPP_DEBUG_STREAM - /// @param bytes Target byte vector - void PrintDebug(const std::vector & bytes); - - /// @brief Main loop of the CAN receiver thread - void ReceiveLoop(); - - /// @brief Process a new filter status packet - /// @param buffer The buffer containing the status packet - void ProcessFilterStatusPacket(const std::vector & buffer); - - /// @brief Process a new data packet - /// @param buffer The buffer containing the data packet - void ProcessDataPacket(const std::vector & buffer); - - /// @brief Callback function to receive the Cloud Packet data from the UDP Driver - /// @param buffer Buffer containing the data received from the UDP socket - void ReceiveSensorPacketCallback(const std::vector & buffer, int id, uint64_t stamp); - public: /// @brief Constructor ContinentalSrr520HwInterface(); @@ -129,8 +54,9 @@ class ContinentalSrr520HwInterface /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status Status SetSensorConfiguration( - std::shared_ptr< - const nebula::drivers::continental_srr520::ContinentalSrr520SensorConfiguration>); + const std::shared_ptr< + const nebula::drivers::continental_srr520::ContinentalSrr520SensorConfiguration> + new_config_ptr); /// @brief Registering callback for PandarScan /// @param scan_callback Callback function @@ -142,7 +68,7 @@ class ContinentalSrr520HwInterface void SensorSync(); /// @brief Process a new Sync Fup request - void SensorSyncFup(); + void SensorSyncFup(builtin_interfaces::msg::Time stamp); /// @brief Configure the sensor /// @param sensor_id Desired sensor id @@ -179,6 +105,44 @@ class ContinentalSrr520HwInterface /// @brief Setting rclcpp::Logger /// @param node Logger void SetLogger(std::shared_ptr node); + +private: + /// @brief Send a Fd frame + /// @param data a buffer containing the data to send + template + bool SendFrame(const std::array & data, int can_frame_id); + + /// @brief Printing the string to RCLCPP_INFO_STREAM + /// @param info Target string + void PrintInfo(std::string info); + + /// @brief Printing the string to RCLCPP_ERROR_STREAM + /// @param error Target string + void PrintError(std::string error); + + /// @brief Printing the string to RCLCPP_DEBUG_STREAM + /// @param debug Target string + void PrintDebug(std::string debug); + + /// @brief Main loop of the CAN receiver thread + void ReceiveLoop(); + + std::unique_ptr<::drivers::socketcan::SocketCanReceiver> can_receiver_ptr_; + std::unique_ptr<::drivers::socketcan::SocketCanSender> can_sender_ptr_; + std::unique_ptr receiver_thread_ptr_; + + std::shared_ptr config_ptr_; + std::function buffer)> + nebula_packet_callback_; + + std::mutex receiver_mutex_; + bool sensor_interface_active_{}; + + uint8_t sync_counter_{0}; + bool sync_fup_sent_{true}; + builtin_interfaces::msg::Time last_sync_stamp_; + + std::shared_ptr parent_node_logger_ptr_; }; } // namespace continental_srr520 } // namespace drivers diff --git a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp index ea364a20d..1300b7a85 100644 --- a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp @@ -26,14 +26,15 @@ namespace drivers namespace continental_srr520 { ContinentalSrr520HwInterface::ContinentalSrr520HwInterface() -: nebula_packets_ptr_{std::make_unique()} { } Status ContinentalSrr520HwInterface::SetSensorConfiguration( - std::shared_ptr) + const std::shared_ptr< + const nebula::drivers::continental_srr520::ContinentalSrr520SensorConfiguration> + new_config_ptr) { - sensor_configuration_ = sensor_configuration_; + config_ptr_ = new_config_ptr; return Status::OK; } @@ -43,21 +44,21 @@ Status ContinentalSrr520HwInterface::SensorInterfaceStart() std::lock_guard lock(receiver_mutex_); try { - can_sender_ = std::make_unique<::drivers::socketcan::SocketCanSender>( - sensor_configuration_->interface, true); - can_receiver_ = std::make_unique<::drivers::socketcan::SocketCanReceiver>( - sensor_configuration_->interface, true); + can_sender_ptr_ = + std::make_unique<::drivers::socketcan::SocketCanSender>(config_ptr_->interface, true); + can_receiver_ptr_ = + std::make_unique<::drivers::socketcan::SocketCanReceiver>(config_ptr_->interface, true); - can_receiver_->SetCanFilters( - ::drivers::socketcan::SocketCanReceiver::CanFilterList(sensor_configuration_->filters)); - PrintInfo(std::string("applied filters: ") + sensor_configuration_->filters); + can_receiver_ptr_->SetCanFilters( + ::drivers::socketcan::SocketCanReceiver::CanFilterList(config_ptr_->filters)); + PrintInfo(std::string("applied filters: ") + config_ptr_->filters); sensor_interface_active_ = true; - receiver_thread_ = + receiver_thread_ptr_ = std::make_unique(&ContinentalSrr520HwInterface::ReceiveLoop, this); } catch (const std::exception & ex) { Status status = Status::CAN_CONNECTION_ERROR; - std::cerr << status << sensor_configuration_->interface << std::endl; + std::cerr << status << config_ptr_->interface << std::endl; return status; } return Status::OK; @@ -70,10 +71,10 @@ bool ContinentalSrr520HwInterface::SendFrame(const std::array & data can_frame_id, 0, ::drivers::socketcan::FrameType::DATA, ::drivers::socketcan::StandardFrame); try { - can_sender_->send_fd( + can_sender_ptr_->send_fd( data.data(), data.size(), send_id, std::chrono::duration_cast( - std::chrono::duration(sensor_configuration_->sender_timeout_sec))); + std::chrono::duration(config_ptr_->sender_timeout_sec))); return true; } catch (const std::exception & ex) { PrintError(std::string("Error sending CAN message: ") + ex.what()); @@ -93,8 +94,8 @@ void ContinentalSrr520HwInterface::ReceiveLoop() { std::lock_guard lock(receiver_mutex_); receiver_timeout_nsec = std::chrono::duration_cast( - std::chrono::duration(sensor_configuration_->receiver_timeout_sec)); - use_bus_time = sensor_configuration_->use_bus_time; + std::chrono::duration(config_ptr_->receiver_timeout_sec)); + use_bus_time = config_ptr_->use_bus_time; if (!sensor_interface_active_) { break; @@ -103,7 +104,7 @@ void ContinentalSrr520HwInterface::ReceiveLoop() try { packet_msg_ptr->data.resize(68); // 64 bytes of data + 4 bytes of ID - receive_id = can_receiver_->receive_fd( + receive_id = can_receiver_ptr_->receive_fd( packet_msg_ptr->data.data() + 4 * sizeof(uint8_t), receiver_timeout_nsec); } catch (const std::exception & ex) { PrintError(std::string("Error receiving CAN FD message: ") + ex.what()); @@ -133,8 +134,6 @@ void ContinentalSrr520HwInterface::ReceiveLoop() } nebula_packet_callback_(std::move(packet_msg_ptr)); - - // ReceiveSensorPacketCallback(buffer, receive_id.identifier(), stamp); } } @@ -145,19 +144,19 @@ Status ContinentalSrr520HwInterface::RegisterPacketCallback( return Status::OK; } -void ContinentalSrr520HwInterface::SensorSyncFup() +void ContinentalSrr520HwInterface::SensorSyncFup(builtin_interfaces::msg::Time stamp) { - if (!can_sender_) { + if (!can_sender_ptr_) { PrintError("Can sender is invalid so can not do follow up"); } - if (!sensor_configuration_->sync_use_bus_time || sync_fup_sent_) { + if (!config_ptr_->sync_use_bus_time || sync_fup_sent_) { return; } auto t0s = last_sync_stamp_; t0s.nanosec = 0; - const auto t1r = stamp; + const auto & t1r = stamp; builtin_interfaces::msg::Time t4r = rclcpp::Time(rclcpp::Time() + (rclcpp::Time(t1r) - rclcpp::Time(t0s))); @@ -185,7 +184,7 @@ void ContinentalSrr520HwInterface::SensorSyncFup() void ContinentalSrr520HwInterface::SensorSync() { - if (!can_sender_) { + if (!can_sender_ptr_) { PrintError("Can sender is invalid so can not do sync up"); } @@ -219,7 +218,7 @@ void ContinentalSrr520HwInterface::SensorSync() SendFrame(data, SYNC_FUP_CAN_MESSAGE_ID); - if (sensor_configuration_->sync_use_bus_time) { + if (config_ptr_->sync_use_bus_time) { sync_fup_sent_ = false; return; } @@ -243,38 +242,14 @@ void ContinentalSrr520HwInterface::SensorSync() sync_fup_sent_ = true; } -void ContinentalSrr520HwInterface::ProcessDataPacket(const std::vector & buffer) -{ - nebula_msgs::msg::NebulaPacket nebula_packet; - nebula_packet.data = buffer; - auto now = std::chrono::system_clock::now(); - auto now_secs = std::chrono::duration_cast(now.time_since_epoch()).count(); - auto now_nanosecs = - std::chrono::duration_cast(now.time_since_epoch()).count(); - nebula_packet.stamp.sec = static_cast(now_secs); - nebula_packet.stamp.nanosec = - static_cast((now_nanosecs / 1000000000.0 - static_cast(now_secs)) * 1000000000); - nebula_packets_ptr_->packets.emplace_back(nebula_packet); - - nebula_packets_ptr_->header.stamp = nebula_packets_ptr_->packets.front().stamp; - - { - std::lock_guard lock(receiver_mutex_); - nebula_packets_ptr_->header.frame_id = sensor_configuration_->frame_id; - } - - nebula_packets_reception_callback_(std::move(nebula_packets_ptr_)); - nebula_packets_ptr_ = std::make_unique(); -} - Status ContinentalSrr520HwInterface::SensorInterfaceStop() { { std::lock_guard l(receiver_mutex_); - sensor_interface_active_ = true; + sensor_interface_active_ = false; } - receiver_thread_->join(); + receiver_thread_ptr_->join(); return Status::ERROR_1; } @@ -283,14 +258,14 @@ Status ContinentalSrr520HwInterface::ConfigureSensor( float yaw_autosar, float longitudinal_cog, float wheelbase, float cover_damping, bool plug_bottom, bool reset) { - std::cout << "longitudinal_autosar=" << longitudinal_autosar << std::endl; - std::cout << "lateral_autosar=" << lateral_autosar << std::endl; - std::cout << "vertical_autosar=" << vertical_autosar << std::endl; - std::cout << "longitudinal_cog=" << longitudinal_cog << std::endl; - std::cout << "wheelbase=" << wheelbase << std::endl; - std::cout << "yaw_autosar=" << yaw_autosar << std::endl; - std::cout << "sensor_id=" << static_cast(sensor_id) << std::endl << std::flush; - std::cout << "plug_bottom=" << plug_bottom << std::endl; + PrintInfo("longitudinal_autosar=" + std::to_string(longitudinal_autosar)); + PrintInfo("lateral_autosar=" + std::to_string(lateral_autosar)); + PrintInfo("vertical_autosar=" + std::to_string(vertical_autosar)); + PrintInfo("longitudinal_cog=" + std::to_string(longitudinal_cog)); + PrintInfo("wheelbase=" + std::to_string(wheelbase)); + PrintInfo("yaw_autosar=" + std::to_string(yaw_autosar)); + PrintInfo("sensor_id=" + std::to_string(static_cast(sensor_id))); + PrintInfo("plug_bottom=" + std::to_string(plug_bottom)); if ( longitudinal_autosar < -32.767f || longitudinal_autosar > 32.767f || @@ -390,13 +365,13 @@ Status ContinentalSrr520HwInterface::SetVehicleDynamics( void ContinentalSrr520HwInterface::SetLogger(std::shared_ptr logger) { - parent_node_logger = logger; + parent_node_logger_ptr_ = logger; } void ContinentalSrr520HwInterface::PrintInfo(std::string info) { - if (parent_node_logger) { - RCLCPP_INFO_STREAM((*parent_node_logger), info); + if (parent_node_logger_ptr_) { + RCLCPP_INFO_STREAM((*parent_node_logger_ptr_), info); } else { std::cout << info << std::endl; } @@ -404,8 +379,8 @@ void ContinentalSrr520HwInterface::PrintInfo(std::string info) void ContinentalSrr520HwInterface::PrintError(std::string error) { - if (parent_node_logger) { - RCLCPP_ERROR_STREAM((*parent_node_logger), error); + if (parent_node_logger_ptr_) { + RCLCPP_ERROR_STREAM((*parent_node_logger_ptr_), error); } else { std::cerr << error << std::endl; } @@ -413,23 +388,13 @@ void ContinentalSrr520HwInterface::PrintError(std::string error) void ContinentalSrr520HwInterface::PrintDebug(std::string debug) { - if (parent_node_logger) { - RCLCPP_DEBUG_STREAM((*parent_node_logger), debug); + if (parent_node_logger_ptr_) { + RCLCPP_DEBUG_STREAM((*parent_node_logger_ptr_), debug); } else { std::cout << debug << std::endl; } } -void ContinentalSrr520HwInterface::PrintDebug(const std::vector & bytes) -{ - std::stringstream ss; - for (const auto & b : bytes) { - ss << static_cast(b) << ", "; - } - ss << std::endl; - PrintDebug(ss.str()); -} - } // namespace continental_srr520 } // namespace drivers } // namespace nebula diff --git a/nebula_ros/continental_ars548_decoder_ros_wrapper.hpp b/nebula_ros/continental_ars548_decoder_ros_wrapper.hpp index 7cd43df44..d9142ca50 100644 --- a/nebula_ros/continental_ars548_decoder_ros_wrapper.hpp +++ b/nebula_ros/continental_ars548_decoder_ros_wrapper.hpp @@ -95,15 +95,6 @@ class ContinentalArs548DriverRosWrapper final : public rclcpp::Node, NebulaDrive Status GetParameters( drivers::continental_ars548::ContinentalArs548SensorConfiguration & sensor_configuration); - /// @brief Convert seconds to chrono::nanoseconds - /// @param seconds - /// @return chrono::nanoseconds - static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) - { - return std::chrono::duration_cast( - std::chrono::duration(seconds)); - } - /// @brief Callback to process new ContinentalArs548DetectionList from the driver /// @param msg The new ContinentalArs548DetectionList from the driver void DetectionListCallback( diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp index d7f61bc1e..5b6778467 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp @@ -117,15 +117,6 @@ class ContinentalArs548DecoderWrapper visualization_msgs::msg::MarkerArray ConvertToMarkers( const continental_msgs::msg::ContinentalArs548ObjectList & msg); - /// @brief Convert seconds to chrono::nanoseconds - /// @param seconds - /// @return chrono::nanoseconds - static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) - { - return std::chrono::duration_cast( - std::chrono::duration(seconds)); - } - nebula::Status status_; rclcpp::Logger logger_; @@ -148,7 +139,7 @@ class ContinentalArs548DecoderWrapper rclcpp::Publisher::SharedPtr objects_markers_pub_{}; rclcpp::Publisher::SharedPtr diagnostics_pub_{}; - std::unordered_set previous_ids_; + std::unordered_set previous_ids_{}; constexpr static int REFERENCE_POINTS_NUM = 9; constexpr static std::array, REFERENCE_POINTS_NUM> reference_to_center_ = { @@ -162,7 +153,7 @@ class ContinentalArs548DecoderWrapper {{0.0, -1.0}}, {{0.0, 0.0}}}}; - std::shared_ptr cloud_watchdog_; + std::shared_ptr watchdog_; }; } // namespace ros } // namespace nebula diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp index 86f309f13..aa8de3a41 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp @@ -57,10 +57,10 @@ class ContinentalArs548RosWrapper final : public rclcpp::Node private: /// @brief Callback from the hw interface's raw data - void ReceivePacketCallback(std::unique_ptr msg_ptr); + void ReceivePacketCallback(std::unique_ptr packet_msg_ptr); /// @brief Callback from replayed NebulaPackets - void ReceivePacketsMessageCallback(std::unique_ptr packets_msg); + void ReceivePacketsCallback(std::unique_ptr packets_msg_ptr); /// @brief Retrieve the parameters from ROS and set the driver and hw interface /// @return Resulting status diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_ros_wrapper.hpp index 54f4ddc81..9aa21e685 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_ros_wrapper.hpp @@ -87,15 +87,6 @@ class ContinentalSrr520DriverRosWrapper final : public rclcpp::Node, NebulaDrive Status GetParameters( drivers::continental_srr520::ContinentalSrr520SensorConfiguration & sensor_configuration); - /// @brief Convert seconds to chrono::nanoseconds - /// @param seconds - /// @return chrono::nanoseconds - static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) - { - return std::chrono::duration_cast( - std::chrono::duration(seconds)); - } - /// @brief Callback to process new Near ContinentalSrr520DetectionList from the driver /// @param msg The new ContinentalSrr520DetectionList from the driver void NearDetectionListCallback( diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp index d2dc3d095..dda6f3d3d 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp @@ -35,7 +35,6 @@ #include #include -//#include #include #include #include @@ -59,7 +58,7 @@ class ContinentalSrr520DecoderWrapper void OnConfigChange( const std::shared_ptr< const nebula::drivers::continental_srr520::ContinentalSrr520SensorConfiguration> & - new_config); + new_config_ptr); rcl_interfaces::msg::SetParametersResult OnParameterChange( const std::vector & p); @@ -85,7 +84,7 @@ class ContinentalSrr520DecoderWrapper void StatusCallback(std::unique_ptr msg); /// @brief Callback to process a new SyncFup message from the driver - void SyncFupCallback(); + void SyncFupCallback(builtin_interfaces::msg::Time stamp); /// @brief Callback to process a new NebulaPackets message from the driver /// @param msg The new NebulaPackets from the driver @@ -126,15 +125,6 @@ class ContinentalSrr520DecoderWrapper visualization_msgs::msg::MarkerArray ConvertToMarkers( const continental_msgs::msg::ContinentalSrr520ObjectList & msg); - /// @brief Convert seconds to chrono::nanoseconds - /// @param seconds - /// @return chrono::nanoseconds - static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) - { - return std::chrono::duration_cast( - std::chrono::duration(seconds)); - } - const rclcpp::Node * const parent_node_; nebula::Status status_; rclcpp::Logger logger_; @@ -163,21 +153,9 @@ class ContinentalSrr520DecoderWrapper rclcpp::Publisher::SharedPtr objects_raw_pub_{}; rclcpp::Publisher::SharedPtr objects_markers_pub_{}; - std::unordered_set previous_ids_; - - /* constexpr static int REFERENCE_POINTS_NUM = 9; - constexpr static std::array, 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 cloud_watchdog_; + std::unordered_set previous_ids_{}; + + std::shared_ptr watchdog_; }; } // namespace ros } // namespace nebula diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_ros_wrapper.hpp index 1ef242cdb..5783fb051 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_ros_wrapper.hpp @@ -74,7 +74,7 @@ class ContinentalSrr520HwInterfaceRosWrapper final : public rclcpp::Node, drivers::continental_srr520::ContinentalSrr520HwInterface hw_interface_; Status interface_status_; - drivers::continental_srr520::ContinentalSrr520SensorConfiguration sensor_configuration_; + drivers::continental_srr520::ContinentalSrr520SensorConfiguration config_ptr_; /// @brief Received Continental Radar message publisher rclcpp::Publisher::SharedPtr packets_pub_; diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp index e4e50487d..c0c7a68e4 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp @@ -75,11 +75,11 @@ class ContinentalSrr520HwInterfaceWrapper void syncTimerCallback(); rclcpp::Node * const parent_node_; - std::shared_ptr hw_interface_; + std::shared_ptr hw_interface_{}; rclcpp::Logger logger_; - nebula::Status status_; + nebula::Status status_{}; std::shared_ptr - config_ptr_; + config_ptr_{}; message_filters::Subscriber odometry_sub_; message_filters::Subscriber acceleration_sub_; @@ -87,13 +87,13 @@ class ContinentalSrr520HwInterfaceWrapper using ExactTimeSyncPolicy = message_filters::sync_policies::ExactTime< geometry_msgs::msg::TwistWithCovarianceStamped, geometry_msgs::msg::AccelWithCovarianceStamped>; using ExactTimeSync = message_filters::Synchronizer; - std::shared_ptr sync_ptr_; + std::shared_ptr sync_ptr_{}; rclcpp::TimerBase::SharedPtr sync_timer_; bool standstill_{true}; rclcpp::Service::SharedPtr - configure_sensor_service_server_; + configure_sensor_service_server_{}; }; } // namespace ros } // namespace nebula diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp index 217564775..9230b82f7 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp @@ -14,7 +14,6 @@ #pragma once -#include #include #include #include @@ -26,13 +25,8 @@ #include #include -/* #include -#include -#include */ #include -#include -#include #include #include #include @@ -61,10 +55,10 @@ class ContinentalSrr520RosWrapper final : public rclcpp::Node private: /// @brief Callback from the hw interface's raw data - void ReceivePacketCallback(std::vector & packet); + void ReceivePacketCallback(std::unique_ptr packet_msg_ptr); /// @brief Callback from replayed NebulaPackets - void ReceivePacketsMessageCallback(std::unique_ptr packets_msg); + void ReceivePacketsCallback(std::unique_ptr packet_packets_msg); /// @brief Retrieve the parameters from ROS and set the driver and hw interface /// @return Resulting status @@ -83,7 +77,7 @@ class ContinentalSrr520RosWrapper final : public rclcpp::Node Status wrapper_status_; std::shared_ptr - sensor_cfg_ptr_{}; + config_ptr_{}; /// @brief Stores received packets that have not been processed yet by the decoder thread mt_queue> packet_queue_; diff --git a/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp index ed1a0f577..1e3fa9d63 100644 --- a/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp @@ -81,11 +81,10 @@ ContinentalArs548DecoderWrapper::ContinentalArs548DecoderWrapper( RCLCPP_INFO_STREAM(logger_, ". Wrapper=" << status_); - cloud_watchdog_ = + watchdog_ = std::make_shared(*parent_node, 100'000us, [this, parent_node](bool ok) { if (ok) return; - RCLCPP_WARN_THROTTLE( - logger_, *parent_node->get_clock(), 5000, "Missed pointcloud output deadline"); + RCLCPP_WARN_THROTTLE(logger_, *parent_node->get_clock(), 5000, "Missed output deadline"); }); } @@ -123,7 +122,7 @@ void ContinentalArs548DecoderWrapper::ProcessPacket( { driver_ptr_->ProcessPacket(std::move(packet_msg)); - cloud_watchdog_->update(); + watchdog_->update(); } void ContinentalArs548DecoderWrapper::DetectionListCallback( diff --git a/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp index ec429bf05..57ffe7c85 100644 --- a/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp @@ -60,8 +60,7 @@ ContinentalArs548RosWrapper::ContinentalArs548RosWrapper(const rclcpp::NodeOptio } else { packets_sub_ = create_subscription( "nebula_packets", rclcpp::SensorDataQoS(), - std::bind( - &ContinentalArs548RosWrapper::ReceivePacketsMessageCallback, this, std::placeholders::_1)); + std::bind(&ContinentalArs548RosWrapper::ReceivePacketsCallback, this, std::placeholders::_1)); RCLCPP_INFO_STREAM( get_logger(), "Hardware connection disabled, listening for packets on " << packets_sub_->get_topic_name()); @@ -250,26 +249,38 @@ Status ContinentalArs548RosWrapper::ValidateAndSetConfig( return Status::OK; } -void ContinentalArs548RosWrapper::ReceivePacketsMessageCallback( - std::unique_ptr packets_msg) +void ContinentalArs548RosWrapper::ReceivePacketsCallback( + std::unique_ptr packets_msg_ptr) { if (hw_interface_wrapper_) { RCLCPP_ERROR_THROTTLE( get_logger(), *get_clock(), 1000, - "Ignoring NebulaPackets PandarScan. Launch with launch_hw:=false to enable NebulaPackets " + "Ignoring NebulaPackets. Launch with launch_hw:=false to enable NebulaPackets " "replay."); return; } - for (auto & packet : packets_msg->packets) { + for (auto & packet : packets_msg_ptr->packets) { auto nebula_packet_ptr = std::make_unique(); nebula_packet_ptr->stamp = packet.stamp; - std::copy(packet.data.begin(), packet.data.end(), std::back_inserter(nebula_packet_ptr->data)); + nebula_packet_ptr->data = std::move(packet.data); packet_queue_.push(std::move(nebula_packet_ptr)); } } +void ContinentalArs548RosWrapper::ReceivePacketCallback( + std::unique_ptr msg_ptr) +{ + if (!decoder_wrapper_ || decoder_wrapper_->Status() != Status::OK) { + return; + } + + if (!packet_queue_.try_push(std::move(msg_ptr))) { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped"); + } +} + Status ContinentalArs548RosWrapper::GetStatus() { return wrapper_status_; @@ -335,18 +346,6 @@ rcl_interfaces::msg::SetParametersResult ContinentalArs548RosWrapper::OnParamete return rcl_interfaces::build().successful(true).reason(""); } -void ContinentalArs548RosWrapper::ReceivePacketCallback( - std::unique_ptr msg_ptr) -{ - if (!decoder_wrapper_ || decoder_wrapper_->Status() != Status::OK) { - return; - } - - if (!packet_queue_.try_push(std::move(msg_ptr))) { - RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped"); - } -} - RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalArs548RosWrapper) } // namespace ros } // namespace nebula diff --git a/nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp index 6222c6aff..9d5c4264e 100644 --- a/nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp +++ b/nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp @@ -89,7 +89,7 @@ ContinentalSrr520DecoderWrapper::ContinentalSrr520DecoderWrapper( RCLCPP_INFO_STREAM(logger_, ". Wrapper=" << status_); - cloud_watchdog_ = + watchdog_ = std::make_shared(*parent_node, 100'000us, [this, parent_node](bool ok) { if (ok) return; RCLCPP_WARN_THROTTLE( @@ -115,7 +115,7 @@ Status ContinentalSrr520DecoderWrapper::InitializeDriver( if (hw_interface_ptr_) { driver_ptr_->RegisterSyncFupCallback( - std::bind(&ContinentalSrr520DecoderWrapper::SyncFupCallback, this)); + std::bind(&ContinentalSrr520DecoderWrapper::SyncFupCallback, this, std::placeholders::_1)); driver_ptr_->RegisterPacketsCallback( std::bind(&ContinentalSrr520DecoderWrapper::PacketsCallback, this, std::placeholders::_1)); } @@ -140,7 +140,7 @@ void ContinentalSrr520DecoderWrapper::ProcessPacket( { driver_ptr_->ProcessPacket(std::move(packet_msg)); - cloud_watchdog_->update(); + watchdog_->update(); } void ContinentalSrr520DecoderWrapper::NearDetectionListCallback( @@ -574,9 +574,9 @@ visualization_msgs::msg::MarkerArray ContinentalSrr520DecoderWrapper::ConvertToM return marker_array; } -void ContinentalSrr520DecoderWrapper::SyncFupCallback() +void ContinentalSrr520DecoderWrapper::SyncFupCallback(builtin_interfaces::msg::Time stamp) { - hw_interface_ptr_->SensorSyncFup(); + hw_interface_ptr_->SensorSyncFup(stamp); } void ContinentalSrr520DecoderWrapper::PacketsCallback( diff --git a/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp index 74a5217cc..d98aeaae5 100644 --- a/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp @@ -22,7 +22,6 @@ ContinentalSrr520RosWrapper::ContinentalSrr520RosWrapper(const rclcpp::NodeOptio : rclcpp::Node( "continental_srr520_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)), wrapper_status_(Status::NOT_INITIALIZED), - sensor_cfg_ptr_(nullptr), packet_queue_(3000), hw_interface_wrapper_(), decoder_wrapper_() @@ -36,15 +35,15 @@ ContinentalSrr520RosWrapper::ContinentalSrr520RosWrapper(const rclcpp::NodeOptio (std::stringstream{} << "Sensor configuration invalid: " << wrapper_status_).str()); } - RCLCPP_INFO_STREAM(get_logger(), "SensorConfig:" << *sensor_cfg_ptr_); + RCLCPP_INFO_STREAM(get_logger(), "SensorConfig:" << *config_ptr_); launch_hw_ = declare_parameter("launch_hw", param_read_only()); if (launch_hw_) { - hw_interface_wrapper_.emplace(this, sensor_cfg_ptr_); - decoder_wrapper_.emplace(this, sensor_cfg_ptr_, hw_interface_wrapper_->HwInterface()); + hw_interface_wrapper_.emplace(this, config_ptr_); + decoder_wrapper_.emplace(this, config_ptr_, hw_interface_wrapper_->HwInterface()); } else { - decoder_wrapper_.emplace(this, sensor_cfg_ptr_, nullptr); + decoder_wrapper_.emplace(this, config_ptr_, nullptr); } RCLCPP_DEBUG(get_logger(), "Starting stream"); @@ -56,14 +55,13 @@ ContinentalSrr520RosWrapper::ContinentalSrr520RosWrapper(const rclcpp::NodeOptio }); if (launch_hw_) { - hw_interface_wrapper_->HwInterface()->RegisterCallback( + hw_interface_wrapper_->HwInterface()->RegisterPacketCallback( std::bind(&ContinentalSrr520RosWrapper::ReceivePacketCallback, this, std::placeholders::_1)); StreamStart(); } else { packets_sub_ = create_subscription( "nebula_packets", rclcpp::SensorDataQoS(), - std::bind( - &ContinentalSrr520RosWrapper::ReceivePacketsMessageCallback, this, std::placeholders::_1)); + std::bind(&ContinentalSrr520RosWrapper::ReceivePacketsCallback, this, std::placeholders::_1)); RCLCPP_INFO_STREAM( get_logger(), "Hardware connection disabled, listening for packets on " << packets_sub_->get_topic_name()); @@ -191,17 +189,17 @@ Status ContinentalSrr520RosWrapper::ValidateAndSetConfig( decoder_wrapper_->OnConfigChange(new_config); } - sensor_cfg_ptr_ = new_config; + config_ptr_ = new_config; return Status::OK; } -void ContinentalSrr520RosWrapper::ReceivePacketsMessageCallback( +void ContinentalSrr520RosWrapper::ReceivePacketsCallback( std::unique_ptr packets_msg) { if (hw_interface_wrapper_) { RCLCPP_ERROR_THROTTLE( get_logger(), *get_clock(), 1000, - "Ignoring NebulaPackets PandarScan. Launch with launch_hw:=false to enable NebulaPackets " + "Ignoring NebulaPackets. Launch with launch_hw:=false to enable NebulaPackets " "replay."); return; } @@ -209,12 +207,24 @@ void ContinentalSrr520RosWrapper::ReceivePacketsMessageCallback( for (auto & packet : packets_msg->packets) { auto nebula_packet_ptr = std::make_unique(); nebula_packet_ptr->stamp = packet.stamp; - std::copy(packet.data.begin(), packet.data.end(), std::back_inserter(nebula_packet_ptr->data)); + nebula_packet_ptr->data = std::move(packet.data); packet_queue_.push(std::move(nebula_packet_ptr)); } } +void ContinentalSrr520RosWrapper::ReceivePacketCallback( + std::unique_ptr msg_ptr) +{ + if (!decoder_wrapper_ || decoder_wrapper_->Status() != Status::OK) { + return; + } + + if (!packet_queue_.try_push(std::move(msg_ptr))) { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped"); + } +} + Status ContinentalSrr520RosWrapper::GetStatus() { return wrapper_status_; @@ -246,7 +256,7 @@ rcl_interfaces::msg::SetParametersResult ContinentalSrr520RosWrapper::OnParamete RCLCPP_INFO(get_logger(), "OnParameterChange"); - drivers::continental_srr520::ContinentalSrr520SensorConfiguration new_config(*sensor_cfg_ptr_); + drivers::continental_srr520::ContinentalSrr520SensorConfiguration new_config(*config_ptr_); bool got_any = get_param(p, "frame_id", new_config.frame_id) | @@ -273,26 +283,6 @@ rcl_interfaces::msg::SetParametersResult ContinentalSrr520RosWrapper::OnParamete return rcl_interfaces::build().successful(true).reason(""); } -void ContinentalSrr520RosWrapper::ReceivePacketCallback(std::vector & packet) -{ - if (!decoder_wrapper_ || decoder_wrapper_->Status() != Status::OK) { - return; - } - - const auto now = std::chrono::high_resolution_clock::now(); - const auto timestamp_ns = - std::chrono::duration_cast(now.time_since_epoch()).count(); - - auto msg_ptr = std::make_unique(); - msg_ptr->stamp.sec = static_cast(timestamp_ns / 1'000'000'000); - msg_ptr->stamp.nanosec = static_cast(timestamp_ns % 1'000'000'000); - msg_ptr->data.swap(packet); - - if (!packet_queue_.try_push(std::move(msg_ptr))) { - RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped"); - } -} - RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalSrr520RosWrapper) } // namespace ros } // namespace nebula diff --git a/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp b/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp index 86efaf9c0..32e1c1d6a 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp +++ b/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp @@ -30,6 +30,7 @@ #include #include #include +#include namespace nebula { @@ -289,8 +290,10 @@ void ContinentalRosDecoderTest::ReadBag() std::cout << "Found data in topic " << bag_message->topic_name << ": " << bag_message->time_stamp << std::endl; - auto extracted_msg_ptr = std::make_shared(extracted_msg); - driver_ptr_->ProcessPackets(extracted_msg); + for (auto & packet_msg : extracted_msg.packets) { + auto extracted_msg_ptr = std::make_unique(packet_msg); + driver_ptr_->ProcessPacket(std::move(extracted_msg_ptr)); + } } } } From f2ad48eaa3df33c59b4926051f28aa04ff30c213 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 23 May 2024 04:33:21 +0900 Subject: [PATCH 05/47] chore: deleted all nebula ros files Signed-off-by: Kenzo Lobos-Tsunekawa --- ...continental_srr520_decoder_ros_wrapper.hpp | 153 ----- ...nental_srr520_hw_interface_ros_wrapper.hpp | 161 ----- ...continental_srr520_decoder_ros_wrapper.cpp | 646 ------------------ ...nental_srr520_hw_interface_ros_wrapper.cpp | 384 ----------- 4 files changed, 1344 deletions(-) delete mode 100644 nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_ros_wrapper.hpp delete mode 100644 nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_ros_wrapper.hpp delete mode 100644 nebula_ros/src/continental/continental_srr520_decoder_ros_wrapper.cpp delete mode 100644 nebula_ros/src/continental/continental_srr520_hw_interface_ros_wrapper.cpp diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_ros_wrapper.hpp deleted file mode 100644 index 9aa21e685..000000000 --- a/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_ros_wrapper.hpp +++ /dev/null @@ -1,153 +0,0 @@ -// 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. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef NEBULA_ContinentalSrr520DriverRosWrapper_H -#define NEBULA_ContinentalSrr520DriverRosWrapper_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -namespace nebula -{ -namespace ros -{ -/// @brief Ros wrapper of continental radar ethernet driver -class ContinentalSrr520DriverRosWrapper final : public rclcpp::Node, NebulaDriverRosWrapperBase -{ - std::shared_ptr decoder_ptr_; - Status wrapper_status_; - - rclcpp::Subscription::SharedPtr packets_sub_; - - rclcpp::Publisher::SharedPtr - near_detection_list_pub_; - rclcpp::Publisher::SharedPtr - hrr_detection_list_pub_; - rclcpp::Publisher::SharedPtr object_list_pub_; - rclcpp::Publisher::SharedPtr status_pub_; - rclcpp::Publisher::SharedPtr near_detection_pointcloud_pub_; - rclcpp::Publisher::SharedPtr hrr_detection_pointcloud_pub_; - rclcpp::Publisher::SharedPtr object_pointcloud_pub_; - rclcpp::Publisher::SharedPtr near_scan_raw_pub_; - rclcpp::Publisher::SharedPtr hrr_scan_raw_pub_; - rclcpp::Publisher::SharedPtr objects_raw_pub_; - rclcpp::Publisher::SharedPtr objects_markers_pub_; - - std::unordered_set previous_ids_; - - std::shared_ptr - sensor_cfg_ptr_; - - drivers::continental_srr520::ContinentalSrr520HwInterface hw_interface_; - - /// @brief Initializing ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @return Resulting status - Status InitializeDriver(std::shared_ptr sensor_configuration); - - /// @brief Get configurations from ros parameters - /// @param sensor_configuration Output of SensorConfiguration - /// @param calibration_configuration Output of CalibrationConfiguration - /// @param correction_configuration Output of CorrectionConfiguration (for AT) - /// @return Resulting status - Status GetParameters( - drivers::continental_srr520::ContinentalSrr520SensorConfiguration & sensor_configuration); - - /// @brief Callback to process new Near ContinentalSrr520DetectionList from the driver - /// @param msg The new ContinentalSrr520DetectionList from the driver - void NearDetectionListCallback( - std::unique_ptr msg); - - /// @brief Callback to process new HRR ContinentalSrr520DetectionList from the driver - /// @param msg The new ContinentalSrr520DetectionList from the driver - void HRRDetectionListCallback( - std::unique_ptr msg); - - /// @brief Callback to process new ContinentalSrr520ObjectList from the driver - /// @param msg The new ContinentalSrr520ObjectList from the driver - void ObjectListCallback(std::unique_ptr msg); - - /// @brief Callback to process new DiagnosticArray from the driver - /// @param msg The new DiagnosticArray from the driver - void StatusCallback(std::unique_ptr msg); - -public: - explicit ContinentalSrr520DriverRosWrapper(const rclcpp::NodeOptions & options); - - /// @brief Callback for NebulaPackets subscriber - /// @param scan_msg Received NebulaPackets - void ReceivePacketsMsgCallback(const nebula_msgs::msg::NebulaPackets::SharedPtr scan_msg); - - /// @brief Get current status of this driver - /// @return Current status - Status GetStatus(); - - /// @brief Convert SRR520 detections to a pointcloud - /// @param msg The SRR520 detection list msg - /// @return Resulting detection pointcloud - pcl::PointCloud::Ptr - ConvertToPointcloud(const continental_msgs::msg::ContinentalSrr520DetectionList & msg); - - /// @brief Convert SRR520 objects to a pointcloud - /// @param msg The SRR520 object list msg - /// @return Resulting object pointcloud - pcl::PointCloud::Ptr ConvertToPointcloud( - const continental_msgs::msg::ContinentalSrr520ObjectList & msg); - - /// @brief Convert SRR520 detections to a standard RadarScan msg - /// @param msg The SRR520 detection list msg - /// @return Resulting RadarScan msg - radar_msgs::msg::RadarScan ConvertToRadarScan( - const continental_msgs::msg::ContinentalSrr520DetectionList & msg); - - /// @brief Convert SRR520 objects to a standard RadarTracks msg - /// @param msg The SRR520 object list msg - /// @return Resulting RadarTracks msg - radar_msgs::msg::RadarTracks ConvertToRadarTracks( - const continental_msgs::msg::ContinentalSrr520ObjectList & msg); - - /// @brief Convert SRR520 objects to a standard MarkerArray msg - /// @param msg The SRR520 object list msg - /// @return Resulting MarkerArray msg - visualization_msgs::msg::MarkerArray ConvertToMarkers( - const continental_msgs::msg::ContinentalSrr520ObjectList & msg); -}; - -} // namespace ros -} // namespace nebula - -#endif // NEBULA_ContinentalSrr520DriverRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_ros_wrapper.hpp deleted file mode 100644 index 5783fb051..000000000 --- a/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_ros_wrapper.hpp +++ /dev/null @@ -1,161 +0,0 @@ -// 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. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef NEBULA_ContinentalSrr520HwInterfaceRosWrapper_H -#define NEBULA_ContinentalSrr520HwInterfaceRosWrapper_H - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include - -#include -#include -#include -#include -#include - -namespace nebula -{ -namespace ros -{ -/// @brief Get parameter from rclcpp::Parameter -/// @tparam T -/// @param p Parameter from rclcpp parameter callback -/// @param name Target parameter name -/// @param value Corresponding value -/// @return Whether the target name existed -template -bool get_param(const std::vector & p, const std::string & name, T & value) -{ - auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { - return parameter.get_name() == name; - }); - if (it != p.cend()) { - value = it->template get_value(); - return true; - } - return false; -} - -/// @brief Hardware interface ros wrapper of continental radar ethernet driver -class ContinentalSrr520HwInterfaceRosWrapper final : public rclcpp::Node, - NebulaHwInterfaceWrapperBase -{ - drivers::continental_srr520::ContinentalSrr520HwInterface hw_interface_; - Status interface_status_; - - drivers::continental_srr520::ContinentalSrr520SensorConfiguration config_ptr_; - - /// @brief Received Continental Radar message publisher - rclcpp::Publisher::SharedPtr packets_pub_; - - message_filters::Subscriber odometry_sub_; - message_filters::Subscriber acceleration_sub_; - - using ExactTimeSyncPolicy = message_filters::sync_policies::ExactTime< - geometry_msgs::msg::TwistWithCovarianceStamped, geometry_msgs::msg::AccelWithCovarianceStamped>; - using ExactTimeSync = message_filters::Synchronizer; - std::shared_ptr sync_ptr_; - rclcpp::TimerBase::SharedPtr sync_timer_; - - bool standstill_{true}; - - rclcpp::Service::SharedPtr - configure_sensor_service_server_; - - /// @brief Initializing hardware interface ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @return Resulting status - Status InitializeHwInterface( - const drivers::SensorConfigurationBase & sensor_configuration) override; - /// @brief Callback for receiving NebulaPackets - /// @param packets_buffer Received NebulaPackets - void ReceivePacketsDataCallback(std::unique_ptr packets_buffer); - - /// @brief Callback to send the odometry information to the radar device - /// @param odometry_msg The odometry message - /// @param acceleration_msg The acceleration message - void dynamicsCallback( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr & odometry_msg, - const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr & acceleration_msg); - - /// @brief Service callback to set the new sensor mounting position - /// @param request Empty service request - /// @param response Empty service response - void ConfigureSensorRequestCallback( - const std::shared_ptr - request, - const std::shared_ptr - response); - - /// @brief Method periodically called to initiate the sensor synchronization mechanism - void syncTimerCallback(); - -public: - explicit ContinentalSrr520HwInterfaceRosWrapper(const rclcpp::NodeOptions & options); - ~ContinentalSrr520HwInterfaceRosWrapper() noexcept override; - - /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) - /// @return Resulting status - Status StreamStart() override; - /// @brief Stop point cloud streaming (not used) - /// @return Resulting status - Status StreamStop() override; - /// @brief Shutdown (not used) - /// @return Resulting status - Status Shutdown() override; - /// @brief Get configurations from ros parameters - /// @param sensor_configuration Output of SensorConfiguration - /// @return Resulting status - Status GetParameters( - drivers::continental_srr520::ContinentalSrr520SensorConfiguration & sensor_configuration); - -private: - std::mutex mtx_config_; - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - - /// @brief rclcpp parameter callback - /// @param parameters Received parameters - /// @return SetParametersResult - rcl_interfaces::msg::SetParametersResult paramCallback( - const std::vector & parameters); - - /// @brief Updating rclcpp parameter - /// @return SetParametersResult - std::vector updateParameters(); -}; - -} // namespace ros -} // namespace nebula - -#endif // NEBULA_ContinentalSrr520HwInterfaceRosWrapper_H diff --git a/nebula_ros/src/continental/continental_srr520_decoder_ros_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_decoder_ros_wrapper.cpp deleted file mode 100644 index 9184a0cfd..000000000 --- a/nebula_ros/src/continental/continental_srr520_decoder_ros_wrapper.cpp +++ /dev/null @@ -1,646 +0,0 @@ -// 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. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include - -namespace nebula -{ -namespace ros -{ -ContinentalSrr520DriverRosWrapper::ContinentalSrr520DriverRosWrapper( - const rclcpp::NodeOptions & options) -: rclcpp::Node("continental_srr520_driver_ros_wrapper", options), hw_interface_() -{ - drivers::continental_srr520::ContinentalSrr520SensorConfiguration sensor_configuration; - - setvbuf(stdout, NULL, _IONBF, BUFSIZ); - - hw_interface_.SetLogger(std::make_shared(this->get_logger())); - - wrapper_status_ = GetParameters(sensor_configuration); - if (Status::OK != wrapper_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); - return; - } - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); - - sensor_cfg_ptr_ = - std::make_shared( - sensor_configuration); - - wrapper_status_ = InitializeDriver( - std::const_pointer_cast( - sensor_cfg_ptr_)); - - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); - packets_sub_ = create_subscription( - "nebula_packets", rclcpp::SensorDataQoS(), - std::bind( - &ContinentalSrr520DriverRosWrapper::ReceivePacketsMsgCallback, this, std::placeholders::_1)); - - near_detection_list_pub_ = - this->create_publisher( - "near_continental_detections", rclcpp::SensorDataQoS()); - hrr_detection_list_pub_ = - this->create_publisher( - "hrr_continental_detections", rclcpp::SensorDataQoS()); - object_list_pub_ = this->create_publisher( - "continental_objects", rclcpp::SensorDataQoS()); - status_pub_ = this->create_publisher("diagnostics", 10); - - near_detection_pointcloud_pub_ = this->create_publisher( - "near_detection_points", rclcpp::SensorDataQoS()); - hrr_detection_pointcloud_pub_ = this->create_publisher( - "hrr_detection_points", rclcpp::SensorDataQoS()); - object_pointcloud_pub_ = - this->create_publisher("object_points", rclcpp::SensorDataQoS()); - - near_scan_raw_pub_ = - this->create_publisher("near_scan_raw", rclcpp::SensorDataQoS()); - hrr_scan_raw_pub_ = - this->create_publisher("hrr_scan_raw", rclcpp::SensorDataQoS()); - - objects_raw_pub_ = - this->create_publisher("objects_raw", rclcpp::SensorDataQoS()); - - objects_markers_pub_ = - this->create_publisher("marker_array", 10); -} - -void ContinentalSrr520DriverRosWrapper::ReceivePacketsMsgCallback( - const nebula_msgs::msg::NebulaPackets::SharedPtr scan_msg) -{ - decoder_ptr_->ProcessPackets(*scan_msg); -} - -Status ContinentalSrr520DriverRosWrapper::InitializeDriver( - std::shared_ptr sensor_configuration) -{ - decoder_ptr_ = std::make_shared( - std::static_pointer_cast( - sensor_configuration)); - - decoder_ptr_->RegisterNearDetectionListCallback(std::bind( - &ContinentalSrr520DriverRosWrapper::NearDetectionListCallback, this, std::placeholders::_1)); - decoder_ptr_->RegisterHRRDetectionListCallback(std::bind( - &ContinentalSrr520DriverRosWrapper::HRRDetectionListCallback, this, std::placeholders::_1)); - decoder_ptr_->RegisterObjectListCallback( - std::bind(&ContinentalSrr520DriverRosWrapper::ObjectListCallback, this, std::placeholders::_1)); - decoder_ptr_->RegisterStatusCallback( - std::bind(&ContinentalSrr520DriverRosWrapper::StatusCallback, this, std::placeholders::_1)); - - return Status::OK; -} - -Status ContinentalSrr520DriverRosWrapper::GetStatus() -{ - return wrapper_status_; -} - -Status ContinentalSrr520DriverRosWrapper::GetParameters( - drivers::continental_srr520::ContinentalSrr520SensorConfiguration & sensor_configuration) -{ - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", descriptor); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("interface", descriptor); - sensor_configuration.interface = this->get_parameter("interface").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("receiver_timeout_sec", descriptor); - sensor_configuration.receiver_timeout_sec = - this->get_parameter("receiver_timeout_sec").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sender_timeout_sec", descriptor); - sensor_configuration.sender_timeout_sec = this->get_parameter("sender_timeout_sec").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("filters", descriptor); - sensor_configuration.filters = this->get_parameter("filters").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("base_frame", descriptor); - sensor_configuration.base_frame = this->get_parameter("base_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("use_bus_time", descriptor); - sensor_configuration.use_bus_time = this->get_parameter("use_bus_time").as_bool(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_wheelbase", descriptor); - sensor_configuration.configuration_vehicle_wheelbase = - static_cast(this->get_parameter("configuration_vehicle_wheelbase").as_double()); - } - - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - - if (sensor_configuration.frame_id.empty()) { - return Status::SENSOR_CONFIG_ERROR; - } - - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; -} - -void ContinentalSrr520DriverRosWrapper::NearDetectionListCallback( - std::unique_ptr msg) -{ - if ( - near_detection_pointcloud_pub_->get_subscription_count() > 0 || - near_detection_pointcloud_pub_->get_intra_process_subscription_count() > 0) { - const auto detection_pointcloud_ptr = ConvertToPointcloud(*msg); - auto detection_pointcloud_msg_ptr = std::make_unique(); - pcl::toROSMsg(*detection_pointcloud_ptr, *detection_pointcloud_msg_ptr); - - detection_pointcloud_msg_ptr->header = msg->header; - near_detection_pointcloud_pub_->publish(std::move(detection_pointcloud_msg_ptr)); - } - - if ( - near_scan_raw_pub_->get_subscription_count() > 0 || - near_scan_raw_pub_->get_intra_process_subscription_count() > 0) { - auto radar_scan_msg = ConvertToRadarScan(*msg); - radar_scan_msg.header = msg->header; - near_scan_raw_pub_->publish(std::move(radar_scan_msg)); - } - - if ( - near_detection_list_pub_->get_subscription_count() > 0 || - near_detection_list_pub_->get_intra_process_subscription_count() > 0) { - near_detection_list_pub_->publish(std::move(msg)); - } -} - -void ContinentalSrr520DriverRosWrapper::HRRDetectionListCallback( - std::unique_ptr msg) -{ - if ( - hrr_detection_pointcloud_pub_->get_subscription_count() > 0 || - hrr_detection_pointcloud_pub_->get_intra_process_subscription_count() > 0) { - const auto detection_pointcloud_ptr = ConvertToPointcloud(*msg); - auto detection_pointcloud_msg_ptr = std::make_unique(); - pcl::toROSMsg(*detection_pointcloud_ptr, *detection_pointcloud_msg_ptr); - - detection_pointcloud_msg_ptr->header = msg->header; - hrr_detection_pointcloud_pub_->publish(std::move(detection_pointcloud_msg_ptr)); - } - - if ( - hrr_scan_raw_pub_->get_subscription_count() > 0 || - hrr_scan_raw_pub_->get_intra_process_subscription_count() > 0) { - auto radar_scan_msg = ConvertToRadarScan(*msg); - radar_scan_msg.header = msg->header; - hrr_scan_raw_pub_->publish(std::move(radar_scan_msg)); - } - - if ( - hrr_detection_list_pub_->get_subscription_count() > 0 || - hrr_detection_list_pub_->get_intra_process_subscription_count() > 0) { - hrr_detection_list_pub_->publish(std::move(msg)); - } -} - -void ContinentalSrr520DriverRosWrapper::ObjectListCallback( - std::unique_ptr msg) -{ - if ( - object_pointcloud_pub_->get_subscription_count() > 0 || - object_pointcloud_pub_->get_intra_process_subscription_count() > 0) { - const auto object_pointcloud_ptr = ConvertToPointcloud(*msg); - auto object_pointcloud_msg_ptr = std::make_unique(); - pcl::toROSMsg(*object_pointcloud_ptr, *object_pointcloud_msg_ptr); - - object_pointcloud_msg_ptr->header = msg->header; - object_pointcloud_pub_->publish(std::move(object_pointcloud_msg_ptr)); - } - - if ( - objects_raw_pub_->get_subscription_count() > 0 || - objects_raw_pub_->get_intra_process_subscription_count() > 0) { - auto objects_raw_msg = ConvertToRadarTracks(*msg); - objects_raw_msg.header = msg->header; - objects_raw_pub_->publish(std::move(objects_raw_msg)); - } - - if ( - objects_markers_pub_->get_subscription_count() > 0 || - objects_markers_pub_->get_intra_process_subscription_count() > 0) { - auto marker_array_msg = ConvertToMarkers(*msg); - objects_markers_pub_->publish(std::move(marker_array_msg)); - } - - if ( - object_list_pub_->get_subscription_count() > 0 || - object_list_pub_->get_intra_process_subscription_count() > 0) { - object_list_pub_->publish(std::move(msg)); - } -} - -void ContinentalSrr520DriverRosWrapper::StatusCallback( - std::unique_ptr status_msg_ptr) -{ - status_pub_->publish(std::move(status_msg_ptr)); -} - -pcl::PointCloud::Ptr -ContinentalSrr520DriverRosWrapper::ConvertToPointcloud( - const continental_msgs::msg::ContinentalSrr520DetectionList & msg) -{ - pcl::PointCloud::Ptr output_pointcloud( - new pcl::PointCloud); - output_pointcloud->reserve(msg.detections.size()); - - nebula::drivers::continental_srr520::PointSrr520Detection point{}; - for (const auto & detection : msg.detections) { - point.x = std::cos(detection.azimuth_angle) * detection.range; - point.y = std::sin(detection.azimuth_angle) * detection.range; - point.z = 0.f; - - point.azimuth = detection.azimuth_angle; - point.range = detection.range; - point.range_rate = detection.range_rate; - point.rcs = detection.rcs; - point.snr = detection.snr; - point.pdh00 = detection.pdh00; - point.pdh01 = detection.pdh01; - point.pdh02 = detection.pdh02; - point.pdh03 = detection.pdh03; - point.pdh04 = detection.pdh04; - point.pdh05 = detection.pdh05; - - output_pointcloud->points.emplace_back(point); - } - - output_pointcloud->height = 1; - output_pointcloud->width = output_pointcloud->points.size(); - return output_pointcloud; -} - -pcl::PointCloud::Ptr -ContinentalSrr520DriverRosWrapper::ConvertToPointcloud( - const continental_msgs::msg::ContinentalSrr520ObjectList & msg) -{ - pcl::PointCloud::Ptr output_pointcloud( - new pcl::PointCloud); - output_pointcloud->reserve(msg.objects.size()); - - nebula::drivers::continental_srr520::PointSrr520Object point{}; - for (const auto & object : msg.objects) { - point.x = object.dist_x; - point.y = object.dist_y; - point.z = 0.f; - - point.id = object.object_id; - point.age = object.life_cycles; - - point.box_length = object.box_length; - point.box_width = object.box_width; - - point.object_status = object.object_status; - point.orientation = object.orientation; - point.rcs = object.rcs; - point.score = object.score; - point.dynamics_abs_vel_x = object.v_abs_x; - point.dynamics_abs_vel_y = object.v_abs_y; - point.dynamics_abs_acc_x = object.a_abs_x; - point.dynamics_abs_acc_y = object.a_abs_y; - - output_pointcloud->points.emplace_back(point); - } - - output_pointcloud->height = 1; - output_pointcloud->width = output_pointcloud->points.size(); - return output_pointcloud; -} - -radar_msgs::msg::RadarScan ContinentalSrr520DriverRosWrapper::ConvertToRadarScan( - const continental_msgs::msg::ContinentalSrr520DetectionList & msg) -{ - radar_msgs::msg::RadarScan output_msg; - output_msg.header = msg.header; - output_msg.returns.reserve(msg.detections.size()); - - radar_msgs::msg::RadarReturn return_msg; - for (const auto & detection : msg.detections) { - if (detection.pdh00 > 0 || detection.pdh00 > 1 || detection.pdh02 > 0) { - continue; - } - - return_msg.range = detection.range; - return_msg.azimuth = detection.azimuth_angle; - return_msg.elevation = 0.f; - return_msg.doppler_velocity = detection.range_rate; - return_msg.amplitude = detection.rcs; - output_msg.returns.emplace_back(return_msg); - } - - return output_msg; -} - -radar_msgs::msg::RadarTracks ContinentalSrr520DriverRosWrapper::ConvertToRadarTracks( - const continental_msgs::msg::ContinentalSrr520ObjectList & msg) -{ - radar_msgs::msg::RadarTracks output_msg; - output_msg.tracks.reserve(msg.objects.size()); - output_msg.header = msg.header; - - constexpr int16_t UNKNOWN_ID = 32000; - constexpr float INVALID_COVARIANCE = 1e6; - - radar_msgs::msg::RadarTrack track_msg; - for (const auto & object : msg.objects) { - if (!object.box_valid || object.object_status == 0) { - continue; - } - - track_msg.uuid.uuid[0] = static_cast(object.object_id & 0xff); - track_msg.uuid.uuid[1] = static_cast((object.object_id >> 8) & 0xff); - track_msg.uuid.uuid[2] = static_cast((object.object_id >> 16) & 0xff); - track_msg.uuid.uuid[3] = static_cast((object.object_id >> 24) & 0xff); - - track_msg.position.x = static_cast(object.dist_x); - track_msg.position.y = static_cast(object.dist_y); - track_msg.position.z = 0.0; - - track_msg.velocity.x = static_cast(object.v_abs_x); - track_msg.velocity.y = static_cast(object.v_abs_y); - track_msg.velocity.z = 0.0; - track_msg.acceleration.x = static_cast(object.a_abs_x); - track_msg.acceleration.y = static_cast(object.a_abs_y); - track_msg.acceleration.z = 0.0; - track_msg.size.x = object.box_length; - track_msg.size.y = object.box_width; - track_msg.size.z = 1.f; - - track_msg.classification = UNKNOWN_ID; - - track_msg.position_covariance[0] = object.dist_x_std * object.dist_x_std; - track_msg.position_covariance[1] = INVALID_COVARIANCE; - track_msg.position_covariance[2] = 0.f; - track_msg.position_covariance[3] = object.dist_y_std * object.dist_y_std; - track_msg.position_covariance[4] = 0.f; - track_msg.position_covariance[5] = INVALID_COVARIANCE; - - track_msg.velocity_covariance[0] = object.v_abs_x_std * object.v_abs_x_std; - track_msg.velocity_covariance[1] = INVALID_COVARIANCE; - track_msg.velocity_covariance[2] = 0.f; - track_msg.velocity_covariance[3] = object.v_abs_y_std * object.v_abs_y_std; - track_msg.velocity_covariance[4] = 0.f; - track_msg.velocity_covariance[5] = INVALID_COVARIANCE; - - track_msg.acceleration_covariance[0] = object.a_abs_x_std * object.a_abs_x_std; - track_msg.acceleration_covariance[1] = INVALID_COVARIANCE; - track_msg.acceleration_covariance[2] = 0.f; - track_msg.acceleration_covariance[3] = object.a_abs_y_std * object.a_abs_y_std; - track_msg.acceleration_covariance[4] = 0.f; - track_msg.acceleration_covariance[5] = INVALID_COVARIANCE; - - track_msg.size_covariance[0] = INVALID_COVARIANCE; - track_msg.size_covariance[1] = 0.f; - track_msg.size_covariance[2] = 0.f; - track_msg.size_covariance[3] = INVALID_COVARIANCE; - track_msg.size_covariance[4] = 0.f; - track_msg.size_covariance[5] = INVALID_COVARIANCE; - - output_msg.tracks.emplace_back(track_msg); - } - - return output_msg; -} - -visualization_msgs::msg::MarkerArray ContinentalSrr520DriverRosWrapper::ConvertToMarkers( - const continental_msgs::msg::ContinentalSrr520ObjectList & msg) -{ - visualization_msgs::msg::MarkerArray marker_array; - marker_array.markers.reserve(4 * msg.objects.size()); - - constexpr int LINE_STRIP_CORNERS_NUM = 17; - constexpr std::array, LINE_STRIP_CORNERS_NUM> cube_corners = { - {{{-1.0, -1.0, -1.0}}, - {{-1.0, -1.0, 1.0}}, - {{-1.0, 1.0, 1.0}}, - {{-1.0, 1.0, -1.0}}, - {{-1.0, -1.0, -1.0}}, - {{1.0, -1.0, -1.0}}, - {{1.0, -1.0, 1.0}}, - {{1.0, 1.0, 1.0}}, - {{1.0, 1.0, -1.0}}, - {{1.0, -1.0, -1.0}}, - {{-1.0, -1.0, -1.0}}, - {{-1.0, -1.0, 1.0}}, - {{1.0, -1.0, 1.0}}, - {{1.0, 1.0, 1.0}}, - {{-1.0, 1.0, 1.0}}, - {{-1.0, 1.0, -1.0}}, - {{1.0, 1.0, -1.0}}}}; - - constexpr int PALETTE_SIZE = 32; - constexpr std::array, PALETTE_SIZE> color_array = {{ - {{1.0, 0.0, 0.0}}, {{0.0, 1.0, 0.0}}, - {{0.0, 0.0, 1.0}}, // Red, Green, Blue - {{1.0, 1.0, 0.0}}, {{0.0, 1.0, 1.0}}, - {{1.0, 0.0, 1.0}}, // Yellow, Cyan, Magenta - {{1.0, 0.647, 0.0}}, {{0.749, 1.0, 0.0}}, - {{0.0, 0.502, 0.502}}, // Orange, Lime, Teal - {{0.502, 0.0, 0.502}}, {{1.0, 0.753, 0.796}}, - {{0.647, 0.165, 0.165}}, // Purple, Pink, Brown - {{0.502, 0.0, 0.0}}, {{0.502, 0.502, 0.0}}, - {{0.0, 0.0, 0.502}}, // Maroon, Olive, Navy - {{0.502, 0.502, 0.502}}, {{1.0, 0.4, 0.4}}, - {{0.4, 1.0, 0.4}}, // Grey, Light Red, Light Green - {{0.4, 0.4, 1.0}}, {{1.0, 1.0, 0.4}}, - {{0.4, 1.0, 1.0}}, // Light Blue, Light Yellow, Light Cyan - {{1.0, 0.4, 1.0}}, {{1.0, 0.698, 0.4}}, - {{0.698, 0.4, 1.0}}, // Light Magenta, Light Orange, Light Purple - {{1.0, 0.6, 0.8}}, {{0.71, 0.396, 0.114}}, - {{0.545, 0.0, 0.0}}, // Light Pink, Light Brown, Dark Red - {{0.0, 0.392, 0.0}}, {{0.0, 0.0, 0.545}}, - {{0.545, 0.545, 0.0}}, // Dark Green, Dark Blue, Dark Yellow - {{0.0, 0.545, 0.545}}, {{0.545, 0.0, 0.545}} // Dark Cyan, Dark Magenta - }}; - - std::unordered_set current_ids; - - for (const auto & object : msg.objects) { - if (!object.box_valid || object.object_status == 0) { - continue; - } - - const double half_length = 0.5 * object.box_length; - const double half_width = 0.5 * object.box_width; - constexpr double DEFAULT_HALF_SIZE = 1.0; - - const double & yaw = object.orientation; - current_ids.emplace(object.object_id); - - visualization_msgs::msg::Marker box_marker; - box_marker.header.frame_id = sensor_cfg_ptr_->base_frame; - box_marker.header.stamp = msg.header.stamp; - box_marker.ns = "boxes"; - box_marker.id = object.object_id; - box_marker.action = visualization_msgs::msg::Marker::ADD; - box_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; - box_marker.lifetime = rclcpp::Duration::from_seconds(0); - box_marker.color.r = color_array[object.object_id % PALETTE_SIZE][0]; - box_marker.color.g = color_array[object.object_id % PALETTE_SIZE][1]; - box_marker.color.b = color_array[object.object_id % PALETTE_SIZE][2]; - box_marker.color.a = 1.0; - box_marker.scale.x = 0.1; - - box_marker.pose.position.x = object.dist_x; - box_marker.pose.position.y = object.dist_y; - box_marker.pose.position.z = DEFAULT_HALF_SIZE; - box_marker.pose.orientation.w = std::cos(0.5 * yaw); - box_marker.pose.orientation.z = std::sin(0.5 * yaw); - - for (const auto & corner : cube_corners) { - geometry_msgs::msg::Point p; - p.x = half_length * corner[0]; - p.y = half_width * corner[1]; - p.z = DEFAULT_HALF_SIZE * corner[2]; - box_marker.points.emplace_back(p); - } - - marker_array.markers.emplace_back(box_marker); - - visualization_msgs::msg::Marker text_marker = box_marker; - text_marker.ns = "object_age"; - text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; - text_marker.color.r = 1.0; - text_marker.color.g = 1.0; - text_marker.color.b = 1.0; - text_marker.color.a = 1.0; - text_marker.scale.x = 0.3; - text_marker.scale.y = 0.3; - text_marker.scale.z = 0.3; - text_marker.pose.position.z += 0.5; - text_marker.text = "ID=" + std::to_string(object.object_id) + - " Age=" + std::to_string(object.life_cycles) + "ms"; - - marker_array.markers.emplace_back(text_marker); - - std::stringstream object_status_ss; - object_status_ss << std::fixed << std::setprecision(3) - << "ID=" << static_cast(object.object_id) << "\n" - << static_cast(object.box_length) << "/" - << static_cast(object.object_status); - - text_marker.ns = "object_status"; - text_marker.text = object_status_ss.str(); - - marker_array.markers.emplace_back(text_marker); - - std::stringstream object_dynamics_ss; - object_dynamics_ss << std::fixed << std::setprecision(3) - << "ID=" << static_cast(object.object_id) - << "\nyaw=" << object.orientation << "\nvx=" << object.v_abs_x - << "\nvy=" << object.v_abs_y << "\nax=" << object.a_abs_x - << "\nay=" << object.a_abs_y; - - text_marker.ns = "object_dynamics"; - text_marker.text = object_dynamics_ss.str(); - - marker_array.markers.emplace_back(text_marker); - } - - for (const auto & previous_id : previous_ids_) { - if (current_ids.find(previous_id) != current_ids.end()) { - continue; - } - - visualization_msgs::msg::Marker delete_marker; - delete_marker.header.frame_id = sensor_cfg_ptr_->base_frame; - delete_marker.header.stamp = msg.header.stamp; - delete_marker.ns = "boxes"; - delete_marker.id = previous_id; - delete_marker.action = visualization_msgs::msg::Marker::DELETE; - - marker_array.markers.push_back(delete_marker); - - delete_marker.ns = "object_age"; - marker_array.markers.push_back(delete_marker); - - delete_marker.ns = "object_status"; - marker_array.markers.push_back(delete_marker); - - delete_marker.ns = "object_dynamics"; - marker_array.markers.push_back(delete_marker); - } - - previous_ids_.clear(); - previous_ids_ = current_ids; - - return marker_array; -} - -RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalSrr520DriverRosWrapper) -} // namespace ros -} // namespace nebula diff --git a/nebula_ros/src/continental/continental_srr520_hw_interface_ros_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_hw_interface_ros_wrapper.cpp deleted file mode 100644 index 78b734e9c..000000000 --- a/nebula_ros/src/continental/continental_srr520_hw_interface_ros_wrapper.cpp +++ /dev/null @@ -1,384 +0,0 @@ -// 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. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include -#include - -#include -#include - -namespace nebula -{ -namespace ros -{ -ContinentalSrr520HwInterfaceRosWrapper::ContinentalSrr520HwInterfaceRosWrapper( - const rclcpp::NodeOptions & options) -: rclcpp::Node("continental_srr520_hw_interface_ros_wrapper", options), hw_interface_() -{ - if (mtx_config_.try_lock()) { - interface_status_ = GetParameters(sensor_configuration_); - mtx_config_.unlock(); - } - if (Status::OK != interface_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); - return; - } - hw_interface_.SetLogger(std::make_shared(this->get_logger())); - std::shared_ptr - sensor_cfg_ptr = - std::make_shared( - sensor_configuration_); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - - hw_interface_.RegisterScanCallback(std::bind( - &ContinentalSrr520HwInterfaceRosWrapper::ReceivePacketsDataCallback, this, - std::placeholders::_1)); - packets_pub_ = this->create_publisher( - "nebula_packets", rclcpp::SensorDataQoS()); - - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&ContinentalSrr520HwInterfaceRosWrapper::paramCallback, this, std::placeholders::_1)); - - StreamStart(); -} - -ContinentalSrr520HwInterfaceRosWrapper::~ContinentalSrr520HwInterfaceRosWrapper() -{ -} - -Status ContinentalSrr520HwInterfaceRosWrapper::StreamStart() -{ - using std::chrono_literals::operator""ms; - - if (Status::OK == interface_status_) { - interface_status_ = hw_interface_.SensorInterfaceStart(); - } - - if (Status::OK == interface_status_) { - odometry_sub_.subscribe(this, "odometry_input"); - acceleration_sub_.subscribe(this, "acceleration_input"); - - sync_ptr_ = - std::make_shared(ExactTimeSyncPolicy(10), odometry_sub_, acceleration_sub_); - sync_ptr_->registerCallback(&ContinentalSrr520HwInterfaceRosWrapper::dynamicsCallback, this); - - configure_sensor_service_server_ = - this->create_service( - "configure_sensor", - std::bind( - &ContinentalSrr520HwInterfaceRosWrapper::ConfigureSensorRequestCallback, this, - std::placeholders::_1, std::placeholders::_2)); - - sync_timer_ = rclcpp::create_timer( - this, get_clock(), 100ms, - std::bind(&ContinentalSrr520HwInterfaceRosWrapper::syncTimerCallback, this)); - } - - return interface_status_; -} - -Status ContinentalSrr520HwInterfaceRosWrapper::StreamStop() -{ - hw_interface_.SensorInterfaceStop(); - return Status::OK; -} -Status ContinentalSrr520HwInterfaceRosWrapper::Shutdown() -{ - hw_interface_.SensorInterfaceStop(); - return Status::OK; -} - -Status ContinentalSrr520HwInterfaceRosWrapper::InitializeHwInterface( // todo: don't think - // this is needed - const drivers::SensorConfigurationBase & sensor_configuration) -{ - std::stringstream ss; - ss << sensor_configuration; - RCLCPP_DEBUG_STREAM(this->get_logger(), ss.str()); - return Status::OK; -} - -Status ContinentalSrr520HwInterfaceRosWrapper::GetParameters( - drivers::continental_srr520::ContinentalSrr520SensorConfiguration & sensor_configuration) -{ - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", descriptor); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("interface", descriptor); - sensor_configuration.interface = this->get_parameter("interface").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("receiver_timeout_sec", descriptor); - sensor_configuration.receiver_timeout_sec = - this->get_parameter("receiver_timeout_sec").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sender_timeout_sec", descriptor); - sensor_configuration.sender_timeout_sec = this->get_parameter("sender_timeout_sec").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("filters", descriptor); - sensor_configuration.filters = this->get_parameter("filters").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("base_frame", descriptor); - sensor_configuration.base_frame = this->get_parameter("base_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("use_bus_time", descriptor); - sensor_configuration.use_bus_time = this->get_parameter("use_bus_time").as_bool(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_wheelbase", descriptor); - sensor_configuration.configuration_vehicle_wheelbase = - static_cast(this->get_parameter("configuration_vehicle_wheelbase").as_double()); - } - - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - - if (sensor_configuration.frame_id.empty()) { - return Status::SENSOR_CONFIG_ERROR; - } - - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; -} - -void ContinentalSrr520HwInterfaceRosWrapper::ReceivePacketsDataCallback( - std::unique_ptr scan_buffer) -{ - packets_pub_->publish(std::move(scan_buffer)); -} - -rcl_interfaces::msg::SetParametersResult ContinentalSrr520HwInterfaceRosWrapper::paramCallback( - const std::vector & p) -{ - std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback"); - RCLCPP_DEBUG_STREAM(this->get_logger(), p); - RCLCPP_DEBUG_STREAM(this->get_logger(), sensor_configuration_); - RCLCPP_INFO_STREAM(this->get_logger(), p); - - drivers::continental_srr520::ContinentalSrr520SensorConfiguration new_param{ - sensor_configuration_}; - RCLCPP_INFO_STREAM(this->get_logger(), new_param); - std::string sensor_model_str; - std::string return_mode_str; - - if ( - get_param(p, "sensor_model", sensor_model_str) | - get_param(p, "interface", new_param.interface) | - get_param(p, "receiver_timeout_sec", new_param.receiver_timeout_sec) | - get_param(p, "sender_timeout_sec", new_param.sender_timeout_sec) | - get_param(p, "frame_id", new_param.frame_id) | - get_param(p, "base_frame", new_param.base_frame) | get_param(p, "filters", new_param.filters) | - get_param(p, "use_bus_time", new_param.use_bus_time) | - get_param(p, "configuration_vehicle_wheelbase", new_param.configuration_vehicle_wheelbase)) { - if (0 < sensor_model_str.length()) - new_param.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_str); - - sensor_configuration_ = new_param; - RCLCPP_INFO_STREAM(this->get_logger(), "Update sensor_configuration"); - std::shared_ptr sensor_cfg_ptr = - std::make_shared( - sensor_configuration_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - hw_interface_.CheckAndSetConfig(); - } - - auto result = std::make_shared(); - result->successful = true; - result->reason = "success"; - - RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback success"); - - return *result; -} - -void ContinentalSrr520HwInterfaceRosWrapper::dynamicsCallback( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr & odometry_msg, - const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr & acceleration_msg) -{ - constexpr float speed_to_standstill = 0.5f; - constexpr float speed_to_moving = 2.f; - - if (standstill_ && std::abs(odometry_msg->twist.twist.linear.x) > speed_to_moving) { - standstill_ = false; - } else if (!standstill_ && std::abs(odometry_msg->twist.twist.linear.x) < speed_to_standstill) { - standstill_ = true; - } - - hw_interface_.SetVehicleDynamics( - acceleration_msg->accel.accel.linear.x, acceleration_msg->accel.accel.linear.y, - odometry_msg->twist.twist.angular.z, odometry_msg->twist.twist.linear.x, standstill_); -} - -void ContinentalSrr520HwInterfaceRosWrapper::ConfigureSensorRequestCallback( - const std::shared_ptr - request, - const std::shared_ptr - response) -{ - std::scoped_lock lock(mtx_config_); - - auto tf_buffer = std::make_unique(this->get_clock()); - auto tf_listener = std::make_unique(*tf_buffer); - - float longitudinal = request->longitudinal; - float lateral = request->lateral; - float vertical = request->vertical; - float yaw = request->yaw; - float vehicle_wheelbase = request->vehicle_wheelbase; - - if (vehicle_wheelbase < 0.f) { - RCLCPP_INFO( - this->get_logger(), - "Service vehicle_wheelbase is invalid. Falling back to configuration value (%.2f)", - sensor_configuration_.configuration_vehicle_wheelbase); - vehicle_wheelbase = sensor_configuration_.configuration_vehicle_wheelbase; - } - - if (request->autoconfigure_extrinsics) { - RCLCPP_INFO( - this->get_logger(), - "autoconfigure_extrinsics was set to true, so the mounting position will be derived from the " - "tfs"); - - geometry_msgs::msg::TransformStamped base_to_sensor_tf; - try { - base_to_sensor_tf = tf_buffer->lookupTransform( - sensor_configuration_.base_frame, sensor_configuration_.frame_id, rclcpp::Time(0), - rclcpp::Duration::from_seconds(0.5)); - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR( - this->get_logger(), "Could not obtain the transform from the base frame to %s (%s)", - sensor_configuration_.frame_id.c_str(), ex.what()); - response->success = false; - response->message = ex.what(); - return; - } - - const auto & quat = base_to_sensor_tf.transform.rotation; - geometry_msgs::msg::Vector3 rpy; - tf2::Matrix3x3(tf2::Quaternion(quat.x, quat.y, quat.z, quat.w)).getRPY(rpy.x, rpy.y, rpy.z); - - longitudinal = base_to_sensor_tf.transform.translation.x - vehicle_wheelbase; - lateral = base_to_sensor_tf.transform.translation.y; - vertical = base_to_sensor_tf.transform.translation.z; - yaw = rpy.z; - } - - yaw = std::min(std::max(yaw, -3.14159f), 3.14159f); - - auto result = hw_interface_.ConfigureSensor( - request->sensor_id, longitudinal, lateral, vertical, yaw, - longitudinal + 0.5 * vehicle_wheelbase, vehicle_wheelbase, request->cover_damping, - request->plug_bottom, request->reset_sensor_configuration); - - response->success = result == Status::OK; - response->message = (std::stringstream() << result).str(); -} - -void ContinentalSrr520HwInterfaceRosWrapper::syncTimerCallback() -{ - hw_interface_.SensorSync(); -} - -std::vector -ContinentalSrr520HwInterfaceRosWrapper::updateParameters() -{ - std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters start"); - std::ostringstream os_sensor_model; - os_sensor_model << sensor_configuration_.sensor_model; - RCLCPP_INFO_STREAM(this->get_logger(), "set_parameters"); - - auto results = set_parameters( - {rclcpp::Parameter("sensor_model", os_sensor_model.str()), - rclcpp::Parameter("interface", sensor_configuration_.interface), - rclcpp::Parameter("receiver_timeout_sec", sensor_configuration_.receiver_timeout_sec), - rclcpp::Parameter("sender_timeout_sec", sensor_configuration_.sender_timeout_sec), - rclcpp::Parameter("frame_id", sensor_configuration_.frame_id), - rclcpp::Parameter("base_frame", sensor_configuration_.base_frame), - rclcpp::Parameter("filters", sensor_configuration_.filters), - rclcpp::Parameter("use_bus_time", sensor_configuration_.use_bus_time), - rclcpp::Parameter( - "configuration_vehicle_wheelbase", sensor_configuration_.configuration_vehicle_wheelbase)}); - - RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters end"); - return results; -} - -RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalSrr520HwInterfaceRosWrapper) -} // namespace ros -} // namespace nebula From f8bea9a5b7fd7ad85580fbaaca83a431f60682fc Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Mon, 27 May 2024 15:47:22 +0900 Subject: [PATCH 06/47] chote: rebased and fixed rebse-related errors Signed-off-by: Kenzo Lobos-Tsunekawa --- .pre-commit-config.yaml | 2 +- README.md | 1 + build_depends.repos | 4 + .../include/nebula_common/nebula_common.hpp | 81 ++++++++++++++----- .../include/nebula_common/nebula_status.hpp | 15 ++++ nebula_decoders/CMakeLists.txt | 1 + .../decoders/continental_ars548_decoder.hpp | 21 +---- .../decoders/continental_ars548_decoder.cpp | 51 ++++++------ nebula_hw_interfaces/package.xml | 1 + .../continental_ars548_hw_interface.cpp | 22 ++--- .../continental_msgs/CMakeLists.txt | 4 + .../msg/ContinentalSrr520Detection.msg | 11 +++ .../msg/ContinentalSrr520DetectionList.msg | 10 +++ .../msg/ContinentalSrr520Object.msg | 21 +++++ .../msg/ContinentalSrr520ObjectList.msg | 12 +++ .../continental_srvs/CMakeLists.txt | 1 + .../ContinentalSrr520SetRadarParameters.srv | 15 ++++ nebula_ros/config/continental/SRR520.yaml | 15 ++++ 18 files changed, 217 insertions(+), 71 deletions(-) create mode 100644 nebula_messages/continental_msgs/msg/ContinentalSrr520Detection.msg create mode 100644 nebula_messages/continental_msgs/msg/ContinentalSrr520DetectionList.msg create mode 100644 nebula_messages/continental_msgs/msg/ContinentalSrr520Object.msg create mode 100644 nebula_messages/continental_msgs/msg/ContinentalSrr520ObjectList.msg create mode 100644 nebula_messages/continental_srvs/srv/ContinentalSrr520SetRadarParameters.srv create mode 100644 nebula_ros/config/continental/SRR520.yaml diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index b4c2e9f2f..498a511ff 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -18,7 +18,7 @@ repos: args: [--markdown-linebreak-ext=md] - repo: https://github.com/igorshubovych/markdownlint-cli - rev: v0.40.0 + rev: v0.41.0 hooks: - id: markdownlint args: [-c, .markdownlint.yaml, --fix] diff --git a/README.md b/README.md index a509950ca..bc0745b09 100644 --- a/README.md +++ b/README.md @@ -99,6 +99,7 @@ Supported models, where sensor_model is the ROS param to be used at launch: | Velodyne | VLP-32 | VLP32 | VLP32.yaml | :warning: | | Velodyne | VLS-128 | VLS128 | VLS128.yaml | :warning: | | Continental | ARS548 | ARS548 | ARS548.yaml | :warning: | +| Continental | SRR520 | SRR520 | SRR520.yaml | :warning: | Test status:\ :heavy_check_mark:: complete\ diff --git a/build_depends.repos b/build_depends.repos index e9278685a..80e0d3165 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -4,3 +4,7 @@ repositories: type: git url: https://github.com/mojomex/transport_drivers version: mutable-buffer-in-udp-callback + ros2_socketcan: + type: git + url: https://github.com/knzo25/ros2_socketcan + version: feat/continental_fd \ No newline at end of file diff --git a/nebula_common/include/nebula_common/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp index a7deeae7c..6713fdd22 100644 --- a/nebula_common/include/nebula_common/nebula_common.hpp +++ b/nebula_common/include/nebula_common/nebula_common.hpp @@ -2,10 +2,7 @@ #define NEBULA_COMMON_H #include - #include - -#include #include #include #include @@ -327,9 +324,11 @@ enum class SensorModel { VELODYNE_HDL32, VELODYNE_VLP16, ROBOSENSE_HELIOS, + ROBOSENSE_BPEARL, ROBOSENSE_BPEARL_V3, ROBOSENSE_BPEARL_V4, - CONTINENTAL_ARS548 + CONTINENTAL_ARS548, + CONTINENTAL_SRR520 }; /// @brief not used? @@ -344,11 +343,24 @@ enum class datatype { FLOAT64 = 8 }; -enum class PtpProfile { IEEE_1588v2 = 0, IEEE_802_1AS, IEEE_802_1AS_AUTO, UNKNOWN_PROFILE }; +enum class PtpProfile { + IEEE_1588v2 = 0, + IEEE_802_1AS, + IEEE_802_1AS_AUTO, + UNKNOWN_PROFILE +}; -enum class PtpTransportType { UDP_IP = 0, L2, UNKNOWN_TRANSPORT }; +enum class PtpTransportType { + UDP_IP = 0, + L2, + UNKNOWN_TRANSPORT +}; -enum class PtpSwitchType { NON_TSN = 0, TSN, UNKNOWN_SWITCH }; +enum class PtpSwitchType { + NON_TSN = 0, + TSN, + UNKNOWN_SWITCH +}; /// @brief not used? struct PointField @@ -417,6 +429,9 @@ inline std::ostream & operator<<(std::ostream & os, nebula::drivers::SensorModel case SensorModel::ROBOSENSE_HELIOS: os << "HELIOS"; break; + case SensorModel::ROBOSENSE_BPEARL: + os << "BPEARL"; + break; case SensorModel::ROBOSENSE_BPEARL_V3: os << "BPEARL V3.0"; break; @@ -426,6 +441,9 @@ inline std::ostream & operator<<(std::ostream & os, nebula::drivers::SensorModel case SensorModel::CONTINENTAL_ARS548: os << "ARS548"; break; + case SensorModel::CONTINENTAL_SRR520: + os << "SRR520"; + break; case SensorModel::UNKNOWN: os << "Sensor Unknown"; break; @@ -448,6 +466,16 @@ struct EthernetSensorConfigurationBase : SensorConfigurationBase uint16_t data_port; }; +/// @brief Base struct for CAN-based Sensor configuration +struct CANSensorConfigurationBase : SensorConfigurationBase +{ + std::string interface; + float receiver_timeout_sec{}; + float sender_timeout_sec{}; + std::string filters{}; + bool use_bus_time{}; +}; + /// @brief Base struct for Lidar configuration struct LidarConfigurationBase : EthernetSensorConfigurationBase { @@ -483,6 +511,19 @@ inline std::ostream & operator<<(std::ostream & os, EthernetSensorConfigurationB return os; } +/// @brief Convert CANSensorConfigurationBase to string (Overloading the << operator) +/// @param os +/// @param arg +/// @return stream +inline std::ostream & operator<<(std::ostream & os, CANSensorConfigurationBase const & arg) +{ + os << (SensorConfigurationBase)(arg) + << ", Interface: " << arg.interface << ", ReceiverTimeoutSec: " << arg.receiver_timeout_sec + << ", SenderTimeoutSec: " << arg.sender_timeout_sec << ", Filters: " << arg.filters + << ", UseBusTime: " << arg.use_bus_time; + return os; +} + /// @brief Convert LidarConfigurationBase to string (Overloading the << operator) /// @param os /// @param arg @@ -526,10 +567,12 @@ inline SensorModel SensorModelFromString(const std::string & sensor_model) if (sensor_model == "VLP16") return SensorModel::VELODYNE_VLP16; // Robosense if (sensor_model == "Helios") return SensorModel::ROBOSENSE_HELIOS; - if (sensor_model == "Bpearl" || sensor_model == "Bpearl_V4") - return SensorModel::ROBOSENSE_BPEARL_V4; + if (sensor_model == "Bpearl") return SensorModel::ROBOSENSE_BPEARL; if (sensor_model == "Bpearl_V3") return SensorModel::ROBOSENSE_BPEARL_V3; + if (sensor_model == "Bpearl_V4") return SensorModel::ROBOSENSE_BPEARL_V4; + // Continental if (sensor_model == "ARS548") return SensorModel::CONTINENTAL_ARS548; + if (sensor_model == "SRR520") return SensorModel::CONTINENTAL_SRR520; return SensorModel::UNKNOWN; } @@ -571,12 +614,17 @@ inline std::string SensorModelToString(const SensorModel & sensor_model) // Robosense case SensorModel::ROBOSENSE_HELIOS: return "Helios"; + case SensorModel::ROBOSENSE_BPEARL: + return "Bpearl"; case SensorModel::ROBOSENSE_BPEARL_V3: return "Bpearl_V3"; case SensorModel::ROBOSENSE_BPEARL_V4: return "Bpearl_V4"; + // Continental case SensorModel::CONTINENTAL_ARS548: return "ARS548"; + case SensorModel::CONTINENTAL_SRR520: + return "SRR520"; default: return "UNKNOWN"; } @@ -602,9 +650,8 @@ inline PtpProfile PtpProfileFromString(const std::string & ptp_profile) { // Hesai auto tmp_str = ptp_profile; - std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), [](unsigned char c) { - return std::tolower(c); - }); + std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), + [](unsigned char c){ return std::tolower(c); }); if (tmp_str == "1588v2") return PtpProfile::IEEE_1588v2; if (tmp_str == "802.1as") return PtpProfile::IEEE_802_1AS; if (tmp_str == "automotive") return PtpProfile::IEEE_802_1AS_AUTO; @@ -642,9 +689,8 @@ inline PtpTransportType PtpTransportTypeFromString(const std::string & transport { // Hesai auto tmp_str = transport_type; - std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), [](unsigned char c) { - return std::tolower(c); - }); + std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), + [](unsigned char c){ return std::tolower(c); }); if (tmp_str == "udp") return PtpTransportType::UDP_IP; if (tmp_str == "l2") return PtpTransportType::L2; @@ -678,9 +724,8 @@ inline PtpSwitchType PtpSwitchTypeFromString(const std::string & switch_type) { // Hesai auto tmp_str = switch_type; - std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), [](unsigned char c) { - return std::tolower(c); - }); + std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), + [](unsigned char c){ return std::tolower(c); }); if (tmp_str == "tsn") return PtpSwitchType::TSN; if (tmp_str == "non_tsn") return PtpSwitchType::NON_TSN; diff --git a/nebula_common/include/nebula_common/nebula_status.hpp b/nebula_common/include/nebula_common/nebula_status.hpp index f36d8b7d2..aff304f27 100644 --- a/nebula_common/include/nebula_common/nebula_status.hpp +++ b/nebula_common/include/nebula_common/nebula_status.hpp @@ -1,3 +1,17 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #ifndef NEBULA_STATUS_HPP #define NEBULA_STATUS_HPP @@ -14,6 +28,7 @@ struct Status enum Type { OK = 0, UDP_CONNECTION_ERROR, + CAN_CONNECTION_ERROR, SENSOR_CONFIG_ERROR, INVALID_SENSOR_MODEL, INVALID_ECHO_MODE, diff --git a/nebula_decoders/CMakeLists.txt b/nebula_decoders/CMakeLists.txt index 31385de6f..0096fbeee 100644 --- a/nebula_decoders/CMakeLists.txt +++ b/nebula_decoders/CMakeLists.txt @@ -55,6 +55,7 @@ ament_auto_add_library(nebula_decoders_robosense_info SHARED # Continental ament_auto_add_library(nebula_decoders_continental SHARED src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp + src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp ) if(BUILD_TESTING) diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp index b492c0197..c6eeb300f 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp @@ -51,21 +51,6 @@ class ContinentalArs548Decoder : public ContinentalPacketsDecoder /// @return Resulting flag bool ProcessPacket(std::unique_ptr packet_msg) override; - /// @brief Function for parsing detection lists - /// @param data - /// @return Resulting flag - bool ParseDetectionsListPacket(std::unique_ptr packet_msg); - - /// @brief Function for parsing object lists - /// @param data - /// @return Resulting flag - bool ParseObjectsListPacket(std::unique_ptr packet_msg); - - /// @brief Function for parsing sensor status messages - /// @param data - /// @return Resulting flag - bool ParseSensorStatusPacket(std::unique_ptr packet_msg); - /// @brief Register function to call when a new detection list is processed /// @param detection_list_callback /// @return Resulting status @@ -111,10 +96,10 @@ class ContinentalArs548Decoder : public ContinentalPacketsDecoder std::function msg)> detection_list_callback_{}; std::function msg)> - object_list_callback_; - std::function sensor_status_callback_; + object_list_callback_{}; + std::function sensor_status_callback_{}; std::function msg)> - nebula_packets_callback_; + nebula_packets_callback_{}; ContinentalArs548Status radar_status_{}; diff --git a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp index 07a1dc18a..9080db862 100644 --- a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp @@ -89,13 +89,13 @@ bool ContinentalArs548Decoder::ProcessPacket( return false; } - return ParseDetectionsListPacket(std::move(packet_msg)); + 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(std::move(packet_msg)); + ParseObjectsListPacket(*packet_msg); } else if (header.method_id.value() == SENSOR_STATUS_METHOD_ID) { if ( data.size() != SENSOR_STATUS_UDP_PAYLOAD || @@ -103,22 +103,31 @@ bool ContinentalArs548Decoder::ProcessPacket( return false; } - return ParseSensorStatusPacket(std::move(packet_msg)); + 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(); + 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( - std::unique_ptr packet_msg) + const nebula_msgs::msg::NebulaPacket & packet_msg) { auto msg_ptr = std::make_unique(); auto & msg = *msg_ptr; DetectionListPacket detection_list; - assert(sizeof(DetectionListPacket) == packet_msg->data.size()); + assert(sizeof(DetectionListPacket) == packet_msg.data.size()); - std::memcpy(&detection_list, packet_msg->data.data(), sizeof(DetectionListPacket)); + std::memcpy(&detection_list, packet_msg.data.data(), sizeof(DetectionListPacket)); msg.header.frame_id = config_ptr_->frame_id; @@ -126,7 +135,7 @@ bool ContinentalArs548Decoder::ParseDetectionsListPacket( msg.header.stamp.nanosec = detection_list.stamp.timestamp_nanoseconds.value(); msg.header.stamp.sec = detection_list.stamp.timestamp_seconds.value(); } else { - msg.header.stamp = packet_msg->stamp; + msg.header.stamp = packet_msg.stamp; } msg.stamp_sync_status = detection_list.stamp.timestamp_sync_status; @@ -229,21 +238,23 @@ 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( - std::unique_ptr packet_msg) + const nebula_msgs::msg::NebulaPacket & packet_msg) { auto msg_ptr = std::make_unique(); auto & msg = *msg_ptr; ObjectListPacket object_list; - assert(sizeof(ObjectListPacket) == packet_msg->data.size()); + assert(sizeof(ObjectListPacket) == packet_msg.data.size()); - std::memcpy(&object_list, packet_msg->data.data(), sizeof(object_list)); + std::memcpy(&object_list, packet_msg.data.data(), sizeof(object_list)); msg.header.frame_id = config_ptr_->object_frame; @@ -251,7 +262,7 @@ bool ContinentalArs548Decoder::ParseObjectsListPacket( msg.header.stamp.nanosec = object_list.stamp.timestamp_nanoseconds.value(); msg.header.stamp.sec = object_list.stamp.timestamp_seconds.value(); } else { - msg.header.stamp = packet_msg->stamp; + msg.header.stamp = packet_msg.stamp; } msg.stamp_sync_status = object_list.stamp.timestamp_sync_status; @@ -381,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( - std::unique_ptr packet_msg) + const nebula_msgs::msg::NebulaPacket & packet_msg) { SensorStatusPacket sensor_status_packet; - std::memcpy(&sensor_status_packet, packet_msg->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(); @@ -624,14 +637,6 @@ bool ContinentalArs548Decoder::ParseSensorStatusPacket( sensor_status_callback_(radar_status_); } - if (nebula_packets_callback_) { - auto packets_msg = std::make_unique(); - packets_msg->packets.emplace_back(std::move(*packet_msg)); - packets_msg->header.stamp = packet_msg->stamp; - packets_msg->header.frame_id = sensor_configuration_->frame_id; - nebula_packets_callback_(std::move(packets_msg)); - } - return true; } diff --git a/nebula_hw_interfaces/package.xml b/nebula_hw_interfaces/package.xml index b526108c9..7e929405b 100644 --- a/nebula_hw_interfaces/package.xml +++ b/nebula_hw_interfaces/package.xml @@ -19,6 +19,7 @@ pandar_msgs rclcpp robosense_msgs + ros2_socketcan sensor_msgs velodyne_msgs diff --git a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp index 0a32ad185..47dd10f2c 100644 --- a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp @@ -143,7 +143,7 @@ Status ContinentalArs548HwInterface::SetSensorMounting( return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -182,7 +182,7 @@ Status ContinentalArs548HwInterface::SetVehicleParameters( return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -226,7 +226,7 @@ Status ContinentalArs548HwInterface::SetRadarParameters( return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -267,7 +267,7 @@ Status ContinentalArs548HwInterface::SetSensorIPAddress(const std::string & sens return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -298,7 +298,7 @@ Status ContinentalArs548HwInterface::SetAccelerationLateralCog(float lateral_acc return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -331,7 +331,7 @@ Status ContinentalArs548HwInterface::SetAccelerationLongitudinalCog(float longit return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -362,7 +362,7 @@ Status ContinentalArs548HwInterface::SetCharacteristicSpeed(float characteristic return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -398,7 +398,7 @@ Status ContinentalArs548HwInterface::SetDrivingDirection(int direction) return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -430,7 +430,7 @@ Status ContinentalArs548HwInterface::SetSteeringAngleFrontAxle(float angle_rad) return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -461,7 +461,7 @@ Status ContinentalArs548HwInterface::SetVelocityVehicle(float velocity_kmh) return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -492,7 +492,7 @@ Status ContinentalArs548HwInterface::SetYawRate(float yaw_rate) return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } diff --git a/nebula_messages/continental_msgs/CMakeLists.txt b/nebula_messages/continental_msgs/CMakeLists.txt index c7b7978f6..724189b70 100644 --- a/nebula_messages/continental_msgs/CMakeLists.txt +++ b/nebula_messages/continental_msgs/CMakeLists.txt @@ -19,6 +19,10 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/ContinentalArs548DetectionList.msg" "msg/ContinentalArs548Object.msg" "msg/ContinentalArs548ObjectList.msg" + "msg/ContinentalSrr520Detection.msg" + "msg/ContinentalSrr520DetectionList.msg" + "msg/ContinentalSrr520Object.msg" + "msg/ContinentalSrr520ObjectList.msg" DEPENDENCIES std_msgs geometry_msgs diff --git a/nebula_messages/continental_msgs/msg/ContinentalSrr520Detection.msg b/nebula_messages/continental_msgs/msg/ContinentalSrr520Detection.msg new file mode 100644 index 000000000..9536ffa12 --- /dev/null +++ b/nebula_messages/continental_msgs/msg/ContinentalSrr520Detection.msg @@ -0,0 +1,11 @@ +float32 range +float32 azimuth_angle +float32 range_rate +float32 rcs +uint8 pdh00 +uint8 pdh01 +uint8 pdh02 +uint8 pdh03 +uint8 pdh04 +uint8 pdh05 +float32 snr diff --git a/nebula_messages/continental_msgs/msg/ContinentalSrr520DetectionList.msg b/nebula_messages/continental_msgs/msg/ContinentalSrr520DetectionList.msg new file mode 100644 index 000000000..e2c159901 --- /dev/null +++ b/nebula_messages/continental_msgs/msg/ContinentalSrr520DetectionList.msg @@ -0,0 +1,10 @@ +std_msgs/Header header +uint32 internal_time_stamp_usec +uint8 global_time_stamp_sync_status +uint8 signal_status +uint8 sequence_counter +uint32 cycle_counter +float32 vambig # cSpell:ignore vambig +float32 max_range + +ContinentalSrr520Detection[] detections diff --git a/nebula_messages/continental_msgs/msg/ContinentalSrr520Object.msg b/nebula_messages/continental_msgs/msg/ContinentalSrr520Object.msg new file mode 100644 index 000000000..a12787fa9 --- /dev/null +++ b/nebula_messages/continental_msgs/msg/ContinentalSrr520Object.msg @@ -0,0 +1,21 @@ +uint8 object_id +float32 dist_x +float32 dist_y +float32 v_abs_x +float32 v_abs_y +float32 a_abs_x +float32 a_abs_y +float32 dist_x_std +float32 dist_y_std +float32 v_abs_x_std +float32 v_abs_y_std +float32 a_abs_x_std +float32 a_abs_y_std +float32 box_length +float32 box_width +float32 orientation +float32 rcs +float32 score +uint16 life_cycles +uint8 box_valid +uint8 object_status diff --git a/nebula_messages/continental_msgs/msg/ContinentalSrr520ObjectList.msg b/nebula_messages/continental_msgs/msg/ContinentalSrr520ObjectList.msg new file mode 100644 index 000000000..f873eb05a --- /dev/null +++ b/nebula_messages/continental_msgs/msg/ContinentalSrr520ObjectList.msg @@ -0,0 +1,12 @@ +std_msgs/Header header + +uint32 internal_time_stamp_usec +uint8 global_time_stamp_sync_status +uint8 signal_status +uint8 sequence_counter +uint32 cycle_counter +float32 ego_vx +float32 ego_yaw_rate +uint8 motion_type + +ContinentalSrr520Object[] objects diff --git a/nebula_messages/continental_srvs/CMakeLists.txt b/nebula_messages/continental_srvs/CMakeLists.txt index ffa839825..b45d309f2 100644 --- a/nebula_messages/continental_srvs/CMakeLists.txt +++ b/nebula_messages/continental_srvs/CMakeLists.txt @@ -19,6 +19,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/ContinentalArs548SetVehicleParameters.srv" "srv/ContinentalArs548SetRadarParameters.srv" "srv/ContinentalArs548SetNetworkConfiguration.srv" + "srv/ContinentalSrr520SetRadarParameters.srv" DEPENDENCIES std_msgs ) diff --git a/nebula_messages/continental_srvs/srv/ContinentalSrr520SetRadarParameters.srv b/nebula_messages/continental_srvs/srv/ContinentalSrr520SetRadarParameters.srv new file mode 100644 index 000000000..4d0d3b297 --- /dev/null +++ b/nebula_messages/continental_srvs/srv/ContinentalSrr520SetRadarParameters.srv @@ -0,0 +1,15 @@ +bool autoconfigure_extrinsics 1 +float32 longitudinal +float32 lateral +float32 vertical +float32 yaw +float32 pitch +float32 vehicle_wheelbase -1.0 + +uint8 sensor_id # The new sensor id +float32 cover_damping # Cover damping in dB +bool plug_bottom # Whether the plug is in the bottom +bool reset_sensor_configuration +--- +bool success +string message # status messages diff --git a/nebula_ros/config/continental/SRR520.yaml b/nebula_ros/config/continental/SRR520.yaml new file mode 100644 index 000000000..644d424f7 --- /dev/null +++ b/nebula_ros/config/continental/SRR520.yaml @@ -0,0 +1,15 @@ +/**: + ros__parameters: + sensor_model: "SRR520" + frame_id: "continental" + base_frame: "base_link" + interface: "can4" + receiver_timeout_sec: 0.03 + sender_timeout_sec: 0.01 + filters: "0:0" + use_bus_time: false + new_sensor_id: 0 + new_plug_bottom: true + new_cover_damping: 0.0 + reset_sensor_configuration: false + configuration_vehicle_wheelbase: 2.79 From 1fa1bb10fb771ddc62823d334fa662933ab7b873 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 30 May 2024 10:24:44 +0900 Subject: [PATCH 07/47] chore: forgot make changes Signed-off-by: Kenzo Lobos-Tsunekawa --- nebula_ros/CMakeLists.txt | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/nebula_ros/CMakeLists.txt b/nebula_ros/CMakeLists.txt index 0c54ae945..eb92c32e0 100644 --- a/nebula_ros/CMakeLists.txt +++ b/nebula_ros/CMakeLists.txt @@ -90,6 +90,18 @@ rclcpp_components_register_node(continental_ars548_ros_wrapper EXECUTABLE continental_ars548_ros_wrapper_node ) +ament_auto_add_library(continental_srr520_ros_wrapper SHARED + src/continental/continental_srr520_ros_wrapper.cpp + src/continental/continental_srr520_decoder_wrapper.cpp + src/continental/continental_srr520_hw_interface_wrapper.cpp + src/common/parameter_descriptors.cpp +) + +rclcpp_components_register_node(continental_srr520_ros_wrapper + PLUGIN "ContinentalSrr520RosWrapper" + EXECUTABLE continental_srr520_ros_wrapper_node +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() From 9a0c89b57694441df982341ca54a1e63e0ec62fa Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 30 May 2024 10:32:50 +0900 Subject: [PATCH 08/47] chore: fixed coyright Signed-off-by: Kenzo Lobos-Tsunekawa --- .../include/nebula_common/continental/continental_srr520.hpp | 2 +- nebula_common/include/nebula_common/continental/crc.hpp | 2 +- nebula_common/include/nebula_common/nebula_status.hpp | 2 +- .../decoders/continental_ars548_decoder.hpp | 2 +- .../decoders/continental_packets_decoder.hpp | 2 +- .../decoders/continental_srr520_decoder.hpp | 2 +- .../decoders/continental_ars548_decoder.cpp | 2 +- .../decoders/continental_srr520_decoder.cpp | 2 +- .../continental_ars548_hw_interface.hpp | 2 +- .../continental_srr520_hw_interface.hpp | 2 +- .../continental_ars548_hw_interface.cpp | 2 +- .../continental_srr520_hw_interface.cpp | 2 +- nebula_ros/continental_ars548_decoder_ros_wrapper.cpp | 2 +- nebula_ros/continental_ars548_decoder_ros_wrapper.hpp | 2 +- nebula_ros/continental_ars548_hw_interface_ros_wrapper.cpp | 2 +- nebula_ros/continental_ars548_hw_interface_ros_wrapper.hpp | 2 +- .../continental/continental_ars548_decoder_wrapper.hpp | 2 +- .../continental/continental_ars548_hw_interface_wrapper.hpp | 2 +- .../nebula_ros/continental/continental_ars548_ros_wrapper.hpp | 2 +- .../continental/continental_srr520_decoder_wrapper.hpp | 2 +- .../continental/continental_srr520_hw_interface_wrapper.hpp | 2 +- .../nebula_ros/continental/continental_srr520_ros_wrapper.hpp | 2 +- .../multi_continental_ars548_hw_interface_ros_wrapper.cpp | 2 +- .../multi_continental_ars548_hw_interface_ros_wrapper.hpp | 2 +- .../src/continental/continental_ars548_decoder_wrapper.cpp | 2 +- .../src/continental/continental_ars548_hw_interface_wrapper.cpp | 2 +- nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp | 2 +- .../src/continental/continental_srr520_decoder_wrapper.cpp | 2 +- .../src/continental/continental_srr520_hw_interface_wrapper.cpp | 2 +- nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp | 2 +- .../continental/continental_ros_decoder_test_ars548.cpp | 2 +- .../continental/continental_ros_decoder_test_ars548.hpp | 2 +- .../continental/continental_ros_decoder_test_main_ars548.cpp | 2 +- .../continental/continental_ros_decoder_test_srr520.cpp | 2 +- .../continental/continental_ros_decoder_test_srr520.hpp | 2 +- 35 files changed, 35 insertions(+), 35 deletions(-) diff --git a/nebula_common/include/nebula_common/continental/continental_srr520.hpp b/nebula_common/include/nebula_common/continental/continental_srr520.hpp index 1d21d4f9e..921c4f9d3 100644 --- a/nebula_common/include/nebula_common/continental/continental_srr520.hpp +++ b/nebula_common/include/nebula_common/continental/continental_srr520.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. diff --git a/nebula_common/include/nebula_common/continental/crc.hpp b/nebula_common/include/nebula_common/continental/crc.hpp index d3085b776..c66d050e1 100644 --- a/nebula_common/include/nebula_common/continental/crc.hpp +++ b/nebula_common/include/nebula_common/continental/crc.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. diff --git a/nebula_common/include/nebula_common/nebula_status.hpp b/nebula_common/include/nebula_common/nebula_status.hpp index aff304f27..7d04fae55 100644 --- a/nebula_common/include/nebula_common/nebula_status.hpp +++ b/nebula_common/include/nebula_common/nebula_status.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. diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp index c6eeb300f..921a161e3 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp +++ b/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. diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp index 0d3c2022c..8760c11b0 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp +++ b/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. diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp index d06df9d51..83a7fdcdc 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_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. diff --git a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp index 9080db862..6b4900c58 100644 --- a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp +++ b/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. diff --git a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp index 37c9ecf08..2ae28aff8 100644 --- a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_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. diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp index e341edd8e..c09941c5f 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.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. diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp index fbc535547..a3ae563e5 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.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. diff --git a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp index 47dd10f2c..ef79e1015 100644 --- a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.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. diff --git a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp index 1300b7a85..9a34ea052 100644 --- a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.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. diff --git a/nebula_ros/continental_ars548_decoder_ros_wrapper.cpp b/nebula_ros/continental_ars548_decoder_ros_wrapper.cpp index d45d256de..552401f0f 100644 --- a/nebula_ros/continental_ars548_decoder_ros_wrapper.cpp +++ b/nebula_ros/continental_ars548_decoder_ros_wrapper.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. diff --git a/nebula_ros/continental_ars548_decoder_ros_wrapper.hpp b/nebula_ros/continental_ars548_decoder_ros_wrapper.hpp index d9142ca50..eae105c25 100644 --- a/nebula_ros/continental_ars548_decoder_ros_wrapper.hpp +++ b/nebula_ros/continental_ars548_decoder_ros_wrapper.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. diff --git a/nebula_ros/continental_ars548_hw_interface_ros_wrapper.cpp b/nebula_ros/continental_ars548_hw_interface_ros_wrapper.cpp index 40ea3b1e6..9d7fb2901 100644 --- a/nebula_ros/continental_ars548_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/continental_ars548_hw_interface_ros_wrapper.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. diff --git a/nebula_ros/continental_ars548_hw_interface_ros_wrapper.hpp b/nebula_ros/continental_ars548_hw_interface_ros_wrapper.hpp index e85309efd..fc8ff6f57 100644 --- a/nebula_ros/continental_ars548_hw_interface_ros_wrapper.hpp +++ b/nebula_ros/continental_ars548_hw_interface_ros_wrapper.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. diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp index 5b6778467..8046fbbfd 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.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. diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp index 24f84d055..e29d44068 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.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. diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp index aa8de3a41..c2c0b85e4 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.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. diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp index dda6f3d3d..7b5e09f70 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.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. diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp index c0c7a68e4..39a008b55 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.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. diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp index 9230b82f7..d6af211db 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.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. diff --git a/nebula_ros/multi_continental_ars548_hw_interface_ros_wrapper.cpp b/nebula_ros/multi_continental_ars548_hw_interface_ros_wrapper.cpp index 965ff9457..828f0df94 100644 --- a/nebula_ros/multi_continental_ars548_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/multi_continental_ars548_hw_interface_ros_wrapper.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. diff --git a/nebula_ros/multi_continental_ars548_hw_interface_ros_wrapper.hpp b/nebula_ros/multi_continental_ars548_hw_interface_ros_wrapper.hpp index ef7a42452..a1fb7645c 100644 --- a/nebula_ros/multi_continental_ars548_hw_interface_ros_wrapper.hpp +++ b/nebula_ros/multi_continental_ars548_hw_interface_ros_wrapper.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. diff --git a/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp index 1e3fa9d63..fcdc78e0f 100644 --- a/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_decoder_wrapper.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. diff --git a/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp index 7cbfef7ef..b98828d15 100644 --- a/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.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. diff --git a/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp index 57ffe7c85..07f749bc0 100644 --- a/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_ros_wrapper.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. diff --git a/nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp index 9d5c4264e..bb556d445 100644 --- a/nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp +++ b/nebula_ros/src/continental/continental_srr520_decoder_wrapper.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. diff --git a/nebula_ros/src/continental/continental_srr520_hw_interface_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_hw_interface_wrapper.cpp index e4841e082..000bdf816 100644 --- a/nebula_ros/src/continental/continental_srr520_hw_interface_wrapper.cpp +++ b/nebula_ros/src/continental/continental_srr520_hw_interface_wrapper.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. diff --git a/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp index d98aeaae5..aa002a8c3 100644 --- a/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_srr520_ros_wrapper.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. diff --git a/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp b/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp index 6d3f756bc..25a9c690c 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp +++ b/nebula_tests/continental/continental_ros_decoder_test_ars548.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. diff --git a/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp b/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp index fe606c069..8b7aa8c0d 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp +++ b/nebula_tests/continental/continental_ros_decoder_test_ars548.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. diff --git a/nebula_tests/continental/continental_ros_decoder_test_main_ars548.cpp b/nebula_tests/continental/continental_ros_decoder_test_main_ars548.cpp index a3203da89..9935e446f 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_main_ars548.cpp +++ b/nebula_tests/continental/continental_ros_decoder_test_main_ars548.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. diff --git a/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp b/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp index 32e1c1d6a..3a20efb7a 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp +++ b/nebula_tests/continental/continental_ros_decoder_test_srr520.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. diff --git a/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp b/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp index c4473e49e..e5d4299ab 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp +++ b/nebula_tests/continental/continental_ros_decoder_test_srr520.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. From dc5a24f2acaa1e3e2aceb84d0aef6b0cc81d8748 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 30 May 2024 12:11:29 +0900 Subject: [PATCH 09/47] chore: replaced Srr520 for SRR520 in class names and deleted unused files Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental/continental_ars548.hpp | 24 +- .../continental/continental_srr520.hpp | 14 +- .../decoders/continental_ars548_decoder.hpp | 14 +- .../decoders/continental_srr520_decoder.hpp | 8 +- .../decoders/continental_ars548_decoder.cpp | 24 +- .../decoders/continental_srr520_decoder.cpp | 52 +- .../continental_ars548_hw_interface.hpp | 8 +- .../continental_srr520_hw_interface.hpp | 8 +- .../continental_ars548_hw_interface.cpp | 48 +- .../continental_srr520_hw_interface.cpp | 34 +- nebula_ros/CMakeLists.txt | 4 +- ...continental_ars548_decoder_ros_wrapper.cpp | 802 ------------------ ...continental_ars548_decoder_ros_wrapper.hpp | 157 ---- ...nental_ars548_hw_interface_ros_wrapper.cpp | 561 ------------ ...nental_ars548_hw_interface_ros_wrapper.hpp | 188 ---- .../continental_ars548_decoder_wrapper.hpp | 24 +- ...ontinental_ars548_hw_interface_wrapper.hpp | 14 +- .../continental_ars548_ros_wrapper.hpp | 14 +- .../continental_srr520_decoder_wrapper.hpp | 22 +- ...ontinental_srr520_hw_interface_wrapper.hpp | 14 +- .../continental_srr520_ros_wrapper.hpp | 14 +- ...nental_ars548_hw_interface_ros_wrapper.cpp | 356 -------- ...nental_ars548_hw_interface_ros_wrapper.hpp | 146 ---- .../continental_ars548_decoder_wrapper.cpp | 66 +- ...ontinental_ars548_hw_interface_wrapper.cpp | 46 +- .../continental_ars548_ros_wrapper.cpp | 34 +- .../continental_srr520_decoder_wrapper.cpp | 74 +- ...ontinental_srr520_hw_interface_wrapper.cpp | 30 +- .../continental_srr520_ros_wrapper.cpp | 34 +- .../continental_ros_decoder_test_ars548.cpp | 14 +- .../continental_ros_decoder_test_ars548.hpp | 8 +- .../continental_ros_decoder_test_srr520.cpp | 14 +- .../continental_ros_decoder_test_srr520.hpp | 8 +- 33 files changed, 334 insertions(+), 2544 deletions(-) delete mode 100644 nebula_ros/continental_ars548_decoder_ros_wrapper.cpp delete mode 100644 nebula_ros/continental_ars548_decoder_ros_wrapper.hpp delete mode 100644 nebula_ros/continental_ars548_hw_interface_ros_wrapper.cpp delete mode 100644 nebula_ros/continental_ars548_hw_interface_ros_wrapper.hpp delete mode 100644 nebula_ros/multi_continental_ars548_hw_interface_ros_wrapper.cpp delete mode 100644 nebula_ros/multi_continental_ars548_hw_interface_ros_wrapper.hpp diff --git a/nebula_common/include/nebula_common/continental/continental_ars548.hpp b/nebula_common/include/nebula_common/continental/continental_ars548.hpp index 98f813c9f..58eaa0c0c 100644 --- a/nebula_common/include/nebula_common/continental/continental_ars548.hpp +++ b/nebula_common/include/nebula_common/continental/continental_ars548.hpp @@ -36,7 +36,7 @@ namespace continental_ars548 { /// @brief struct for ARS548 sensor configuration -struct ContinentalArs548SensorConfiguration : EthernetSensorConfigurationBase +struct ContinentalARS548SensorConfiguration : EthernetSensorConfigurationBase { std::string multicast_ip{}; std::string base_frame{}; @@ -51,7 +51,7 @@ struct ContinentalArs548SensorConfiguration : EthernetSensorConfigurationBase }; /// @brief struct for Multiple ARS548 sensor configuration -struct MultiContinentalArs548SensorConfiguration : ContinentalArs548SensorConfiguration +struct MultiContinentalARS548SensorConfiguration : ContinentalARS548SensorConfiguration { std::vector sensor_ips{}; std::vector frame_ids{}; @@ -63,7 +63,7 @@ struct MultiContinentalArs548SensorConfiguration : ContinentalArs548SensorConfig /// @param arg /// @return stream inline std::ostream & operator<<( - std::ostream & os, ContinentalArs548SensorConfiguration const & arg) + std::ostream & os, ContinentalARS548SensorConfiguration const & arg) { os << (EthernetSensorConfigurationBase)(arg) << ", MulticastIP: " << arg.multicast_ip << ", BaseFrame: " << arg.base_frame << ", ObjectFrame: " << arg.object_frame @@ -77,13 +77,13 @@ inline std::ostream & operator<<( return os; } -/// @brief Convert MultiContinentalArs548SensorConfiguration to string (Overloading the << +/// @brief Convert MultiContinentalARS548SensorConfiguration to string (Overloading the << /// operator) /// @param os /// @param arg /// @return stream inline std::ostream & operator<<( - std::ostream & os, MultiContinentalArs548SensorConfiguration const & arg) + std::ostream & os, MultiContinentalARS548SensorConfiguration const & arg) { std::stringstream sensor_ips_ss; sensor_ips_ss << "["; @@ -99,13 +99,13 @@ inline std::ostream & operator<<( } frame_ids_ss << "]"; - os << (ContinentalArs548SensorConfiguration)(arg) << ", MulticastIP: " << arg.multicast_ip + os << (ContinentalARS548SensorConfiguration)(arg) << ", MulticastIP: " << arg.multicast_ip << ", SensorIPs: " << sensor_ips_ss.str() << ", FrameIds: " << frame_ids_ss.str(); return os; } /// @brief semantic struct of ARS548 Status -struct ContinentalArs548Status +struct ContinentalARS548Status { // Filled with raw sensor data uint32_t timestamp_nanoseconds{}; @@ -167,7 +167,7 @@ struct ContinentalArs548Status /// @param os /// @param arg /// @return stream - friend std::ostream & operator<<(std::ostream & os, ContinentalArs548Status const & arg) + friend std::ostream & operator<<(std::ostream & os, ContinentalARS548Status const & arg) { os << "timestamp_nanoseconds: " << arg.timestamp_nanoseconds; os << ", "; @@ -613,7 +613,7 @@ struct FilterStatusPacket #pragma pack(pop) -struct PointArs548Detection +struct PointARS548Detection { PCL_ADD_POINT4D; float azimuth; @@ -636,7 +636,7 @@ struct PointArs548Detection // Note we only use a subset of the data since POINT_CLOUD_REGISTER_POINT_STRUCT has a limit in the // number of fields -struct PointArs548Object +struct PointARS548Object { PCL_ADD_POINT4D; uint32_t id; @@ -664,7 +664,7 @@ struct PointArs548Object } // namespace nebula POINT_CLOUD_REGISTER_POINT_STRUCT( - nebula::drivers::continental_ars548::PointArs548Detection, + nebula::drivers::continental_ars548::PointARS548Detection, (float, x, x)(float, y, y)(float, z, z)(float, azimuth, azimuth)(float, azimuth_std, azimuth_std)( float, elevation, elevation)(float, elevation_std, elevation_std)(float, range, range)( float, range_std, range_std)(int8_t, rcs, rcs)(uint16_t, measurement_id, measurement_id)( @@ -675,7 +675,7 @@ POINT_CLOUD_REGISTER_POINT_STRUCT( // Note: we can only use up to 20 fields POINT_CLOUD_REGISTER_POINT_STRUCT( - nebula::drivers::continental_ars548::PointArs548Object, + nebula::drivers::continental_ars548::PointARS548Object, (float, x, x)(float, y, y)(float, z, z)(uint32_t, id, id)(uint16_t, age, age)( uint8_t, status_measurement, status_measurement)(uint8_t, status_movement, status_movement)( uint8_t, position_reference, diff --git a/nebula_common/include/nebula_common/continental/continental_srr520.hpp b/nebula_common/include/nebula_common/continental/continental_srr520.hpp index 921c4f9d3..64554086d 100644 --- a/nebula_common/include/nebula_common/continental/continental_srr520.hpp +++ b/nebula_common/include/nebula_common/continental/continental_srr520.hpp @@ -40,20 +40,20 @@ namespace continental_srr520 { /// @brief struct for SRR520 sensor configuration -struct ContinentalSrr520SensorConfiguration : CANSensorConfigurationBase +struct ContinentalSRR520SensorConfiguration : CANSensorConfigurationBase { std::string base_frame{}; bool sync_use_bus_time{}; float configuration_vehicle_wheelbase{}; }; -/// @brief Convert ContinentalSrr520SensorConfiguration to string (Overloading the << +/// @brief Convert ContinentalSRR520SensorConfiguration to string (Overloading the << /// operator) /// @param os /// @param arg /// @return stream inline std::ostream & operator<<( - std::ostream & os, ContinentalSrr520SensorConfiguration const & arg) + std::ostream & os, ContinentalSRR520SensorConfiguration const & arg) { os << (CANSensorConfigurationBase)(arg) << ", BaseFrame: " << arg.base_frame << ", SyncUseBusTime: " << arg.sync_use_bus_time @@ -237,7 +237,7 @@ struct FollowUpPacket #pragma pack(pop) -struct PointSrr520Detection +struct PointSRR520Detection { PCL_ADD_POINT4D; float range; @@ -256,7 +256,7 @@ struct PointSrr520Detection // Note we only use a subset of the data since POINT_CLOUD_REGISTER_POINT_STRUCT has a limit in the // number of fields -struct PointSrr520Object +struct PointSRR520Object { PCL_ADD_POINT4D; uint32_t id; @@ -279,14 +279,14 @@ struct PointSrr520Object } // namespace nebula POINT_CLOUD_REGISTER_POINT_STRUCT( - nebula::drivers::continental_srr520::PointSrr520Detection, + nebula::drivers::continental_srr520::PointSRR520Detection, (float, x, x)(float, y, y)(float, z, z)(float, azimuth, azimuth)(float, range, range)( float, range_rate, range_rate)(int8_t, rcs, rcs)(uint8_t, pdh00, pdh00)(uint8_t, pdh01, pdh01)( uint8_t, pdh02, pdh02)(uint16_t, pdh03, pdh03)(uint8_t, pdh04, pdh04)(uint8_t, pdh05, pdh05)(float, snr, snr)) POINT_CLOUD_REGISTER_POINT_STRUCT( - nebula::drivers::continental_srr520::PointSrr520Object, + nebula::drivers::continental_srr520::PointSRR520Object, (float, x, x)(float, y, y)(float, z, z)(uint32_t, id, id)(uint16_t, age, age)( float, orientation, orientation)(float, rcs, rcs)(float, score, score)(uint8_t, object_status, object_status)( diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp index 921a161e3..af0c508e3 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp @@ -34,13 +34,13 @@ namespace drivers namespace continental_ars548 { /// @brief Continental Radar decoder (ARS548) -class ContinentalArs548Decoder : public ContinentalPacketsDecoder +class ContinentalARS548Decoder : public ContinentalPacketsDecoder { public: /// @brief Constructor /// @param sensor_configuration SensorConfiguration for this decoder - explicit ContinentalArs548Decoder( - const std::shared_ptr & sensor_configuration); + explicit ContinentalARS548Decoder( + const std::shared_ptr & sensor_configuration); /// @brief Get current status of this driver /// @return Current status @@ -69,7 +69,7 @@ class ContinentalArs548Decoder : public ContinentalPacketsDecoder /// @param object_list_callback /// @return Resulting status Status RegisterSensorStatusCallback( - std::function sensor_status_callback); + std::function sensor_status_callback); /// @brief Register function to call when a new sensor status message is processed /// @param object_list_callback @@ -97,14 +97,14 @@ class ContinentalArs548Decoder : public ContinentalPacketsDecoder detection_list_callback_{}; std::function msg)> object_list_callback_{}; - std::function sensor_status_callback_{}; + std::function sensor_status_callback_{}; std::function msg)> nebula_packets_callback_{}; - ContinentalArs548Status radar_status_{}; + ContinentalARS548Status radar_status_{}; /// @brief SensorConfiguration for this decoder - std::shared_ptr config_ptr_{}; + std::shared_ptr config_ptr_{}; }; } // namespace continental_ars548 diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp index 83a7fdcdc..535ef1144 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp @@ -36,13 +36,13 @@ namespace drivers namespace continental_srr520 { /// @brief Continental Radar decoder (SRR520) -class ContinentalSrr520Decoder : public ContinentalPacketsDecoder +class ContinentalSRR520Decoder : public ContinentalPacketsDecoder { public: /// @brief Constructor /// @param sensor_configuration SensorConfiguration for this decoder - explicit ContinentalSrr520Decoder( - const std::shared_ptr & sensor_configuration); + explicit ContinentalSRR520Decoder( + const std::shared_ptr & sensor_configuration); /// @brief Get current status of this driver /// @return Current status @@ -199,7 +199,7 @@ class ContinentalSrr520Decoder : public ContinentalPacketsDecoder ObjectHeaderPacket object_header_packet_{}; /// @brief SensorConfiguration for this decoder - std::shared_ptr + std::shared_ptr sensor_configuration_{}; std::shared_ptr parent_node_logger_ptr_{}; diff --git a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp index 6b4900c58..8423d4a0b 100644 --- a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp @@ -24,19 +24,19 @@ namespace drivers { namespace continental_ars548 { -ContinentalArs548Decoder::ContinentalArs548Decoder( - const std::shared_ptr & +ContinentalARS548Decoder::ContinentalARS548Decoder( + const std::shared_ptr & sensor_configuration) { config_ptr_ = sensor_configuration; } -Status ContinentalArs548Decoder::GetStatus() +Status ContinentalARS548Decoder::GetStatus() { return Status::OK; } -Status ContinentalArs548Decoder::RegisterDetectionListCallback( +Status ContinentalARS548Decoder::RegisterDetectionListCallback( std::function)> detection_list_callback) { @@ -44,7 +44,7 @@ Status ContinentalArs548Decoder::RegisterDetectionListCallback( return Status::OK; } -Status ContinentalArs548Decoder::RegisterObjectListCallback( +Status ContinentalARS548Decoder::RegisterObjectListCallback( std::function)> object_list_callback) { @@ -52,21 +52,21 @@ Status ContinentalArs548Decoder::RegisterObjectListCallback( return Status::OK; } -Status ContinentalArs548Decoder::RegisterSensorStatusCallback( - std::function sensor_status_callback) +Status ContinentalARS548Decoder::RegisterSensorStatusCallback( + std::function sensor_status_callback) { sensor_status_callback_ = std::move(sensor_status_callback); return Status::OK; } -Status ContinentalArs548Decoder::RegisterPacketsCallback( +Status ContinentalARS548Decoder::RegisterPacketsCallback( std::function)> nebula_packets_callback) { nebula_packets_callback_ = std::move(nebula_packets_callback); return Status::OK; } -bool ContinentalArs548Decoder::ProcessPacket( +bool ContinentalARS548Decoder::ProcessPacket( std::unique_ptr packet_msg) { const auto & data = packet_msg->data; @@ -118,7 +118,7 @@ bool ContinentalArs548Decoder::ProcessPacket( return true; } -bool ContinentalArs548Decoder::ParseDetectionsListPacket( +bool ContinentalARS548Decoder::ParseDetectionsListPacket( const nebula_msgs::msg::NebulaPacket & packet_msg) { auto msg_ptr = std::make_unique(); @@ -245,7 +245,7 @@ bool ContinentalArs548Decoder::ParseDetectionsListPacket( return true; } -bool ContinentalArs548Decoder::ParseObjectsListPacket( +bool ContinentalARS548Decoder::ParseObjectsListPacket( const nebula_msgs::msg::NebulaPacket & packet_msg) { auto msg_ptr = std::make_unique(); @@ -399,7 +399,7 @@ bool ContinentalArs548Decoder::ParseObjectsListPacket( return true; } -bool ContinentalArs548Decoder::ParseSensorStatusPacket( +bool ContinentalARS548Decoder::ParseSensorStatusPacket( const nebula_msgs::msg::NebulaPacket & packet_msg) { SensorStatusPacket sensor_status_packet; diff --git a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp index 2ae28aff8..e66226933 100644 --- a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp @@ -29,19 +29,19 @@ namespace drivers { namespace continental_srr520 { -ContinentalSrr520Decoder::ContinentalSrr520Decoder( - const std::shared_ptr & +ContinentalSRR520Decoder::ContinentalSRR520Decoder( + const std::shared_ptr & sensor_configuration) { sensor_configuration_ = sensor_configuration; } -Status ContinentalSrr520Decoder::GetStatus() +Status ContinentalSRR520Decoder::GetStatus() { return Status::OK; } -Status ContinentalSrr520Decoder::RegisterNearDetectionListCallback( +Status ContinentalSRR520Decoder::RegisterNearDetectionListCallback( std::function)> detection_list_callback) { @@ -49,7 +49,7 @@ Status ContinentalSrr520Decoder::RegisterNearDetectionListCallback( return Status::OK; } -Status ContinentalSrr520Decoder::RegisterHRRDetectionListCallback( +Status ContinentalSRR520Decoder::RegisterHRRDetectionListCallback( std::function)> detection_list_callback) { @@ -57,7 +57,7 @@ Status ContinentalSrr520Decoder::RegisterHRRDetectionListCallback( return Status::OK; } -Status ContinentalSrr520Decoder::RegisterObjectListCallback( +Status ContinentalSRR520Decoder::RegisterObjectListCallback( std::function)> object_list_callback) { @@ -65,28 +65,28 @@ Status ContinentalSrr520Decoder::RegisterObjectListCallback( return Status::OK; } -Status ContinentalSrr520Decoder::RegisterStatusCallback( +Status ContinentalSRR520Decoder::RegisterStatusCallback( std::function)> status_callback) { status_callback_ = std::move(status_callback); return Status::OK; } -Status ContinentalSrr520Decoder::RegisterSyncFupCallback( +Status ContinentalSRR520Decoder::RegisterSyncFupCallback( std::function sync_fup_callback) { sync_fup_callback_ = std::move(sync_fup_callback); return Status::OK; } -Status ContinentalSrr520Decoder::RegisterPacketsCallback( +Status ContinentalSRR520Decoder::RegisterPacketsCallback( std::function)> nebula_packets_callback) { nebula_packets_callback_ = std::move(nebula_packets_callback); return Status::OK; } -bool ContinentalSrr520Decoder::ProcessPacket( +bool ContinentalSRR520Decoder::ProcessPacket( std::unique_ptr packet_msg) { const uint32_t can_message_id = (static_cast(packet_msg->data[0]) << 24) | @@ -165,7 +165,7 @@ bool ContinentalSrr520Decoder::ProcessPacket( return true; } -void ContinentalSrr520Decoder::ProcessNearHeaderPacket( +void ContinentalSRR520Decoder::ProcessNearHeaderPacket( std::unique_ptr packet_msg) { rdi_near_packets_ptr_ = std::make_unique(); @@ -215,7 +215,7 @@ void ContinentalSrr520Decoder::ProcessNearHeaderPacket( rdi_near_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); } -void ContinentalSrr520Decoder::ProcessNearElementPacket( +void ContinentalSRR520Decoder::ProcessNearElementPacket( std::unique_ptr packet_msg) { if (rdi_near_packets_ptr_->packets.size() == 0) { @@ -285,7 +285,7 @@ void ContinentalSrr520Decoder::ProcessNearElementPacket( rdi_near_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); } -void ContinentalSrr520Decoder::ProcessHRRHeaderPacket( +void ContinentalSRR520Decoder::ProcessHRRHeaderPacket( std::unique_ptr packet_msg) { rdi_hrr_packets_ptr_ = std::make_unique(); @@ -334,7 +334,7 @@ void ContinentalSrr520Decoder::ProcessHRRHeaderPacket( rdi_near_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); } -void ContinentalSrr520Decoder::ProcessHRRElementPacket( +void ContinentalSRR520Decoder::ProcessHRRElementPacket( std::unique_ptr packet_msg) { if (rdi_hrr_packets_ptr_->packets.size() == 0) { @@ -404,7 +404,7 @@ void ContinentalSrr520Decoder::ProcessHRRElementPacket( rdi_hrr_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); } -void ContinentalSrr520Decoder::ProcessObjectHeaderPacket( +void ContinentalSRR520Decoder::ProcessObjectHeaderPacket( std::unique_ptr packet_msg) { object_packets_ptr_ = std::make_unique(); @@ -450,7 +450,7 @@ void ContinentalSrr520Decoder::ProcessObjectHeaderPacket( object_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); } -void ContinentalSrr520Decoder::ProcessObjectElementPacket( +void ContinentalSRR520Decoder::ProcessObjectElementPacket( std::unique_ptr packet_msg) { if (object_packets_ptr_->packets.size() == 0) { @@ -579,7 +579,7 @@ void ContinentalSrr520Decoder::ProcessObjectElementPacket( object_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); } -void ContinentalSrr520Decoder::ProcessCRCListPacket( +void ContinentalSRR520Decoder::ProcessCRCListPacket( std::unique_ptr packet_msg) { const auto crc_id = packet_msg->data[4]; // first 4 bits are the can id @@ -595,7 +595,7 @@ void ContinentalSrr520Decoder::ProcessCRCListPacket( } } -void ContinentalSrr520Decoder::ProcessNearCRCListPacket( +void ContinentalSRR520Decoder::ProcessNearCRCListPacket( std::unique_ptr packet_msg) { if (rdi_near_packets_ptr_->packets.size() != RDI_NEAR_PACKET_NUM + 1) { @@ -636,7 +636,7 @@ void ContinentalSrr520Decoder::ProcessNearCRCListPacket( rdi_near_packets_ptr_.reset(); } -void ContinentalSrr520Decoder::ProcessHRRCRCListPacket( +void ContinentalSRR520Decoder::ProcessHRRCRCListPacket( std::unique_ptr packet_msg) { if (rdi_hrr_packets_ptr_->packets.size() != RDI_HRR_PACKET_NUM + 1) { @@ -676,7 +676,7 @@ void ContinentalSrr520Decoder::ProcessHRRCRCListPacket( rdi_hrr_packets_ptr_.reset(); } -void ContinentalSrr520Decoder::ProcessObjectCRCListPacket( +void ContinentalSRR520Decoder::ProcessObjectCRCListPacket( std::unique_ptr packet_msg) { if (object_packets_ptr_->packets.size() != OBJECT_PACKET_NUM + 1) { @@ -719,7 +719,7 @@ void ContinentalSrr520Decoder::ProcessObjectCRCListPacket( object_packets_ptr_.reset(); } -void ContinentalSrr520Decoder::ProcessSensorStatusPacket( +void ContinentalSRR520Decoder::ProcessSensorStatusPacket( std::unique_ptr packet_msg) { static_assert(sizeof(StatusPacket) == STATUS_PACKET_SIZE); @@ -1073,7 +1073,7 @@ void ContinentalSrr520Decoder::ProcessSensorStatusPacket( } } -void ContinentalSrr520Decoder::ProcessSyncFupPacket( +void ContinentalSRR520Decoder::ProcessSyncFupPacket( std::unique_ptr packet_msg) { if (sync_fup_callback_) { @@ -1090,12 +1090,12 @@ void ContinentalSrr520Decoder::ProcessSyncFupPacket( } } -void ContinentalSrr520Decoder::SetLogger(std::shared_ptr logger) +void ContinentalSRR520Decoder::SetLogger(std::shared_ptr logger) { parent_node_logger_ptr_ = logger; } -void ContinentalSrr520Decoder::PrintInfo(std::string info) +void ContinentalSRR520Decoder::PrintInfo(std::string info) { if (parent_node_logger_ptr_) { RCLCPP_INFO_STREAM((*parent_node_logger_ptr_), info); @@ -1104,7 +1104,7 @@ void ContinentalSrr520Decoder::PrintInfo(std::string info) } } -void ContinentalSrr520Decoder::PrintError(std::string error) +void ContinentalSRR520Decoder::PrintError(std::string error) { if (parent_node_logger_ptr_) { RCLCPP_ERROR_STREAM((*parent_node_logger_ptr_), error); @@ -1113,7 +1113,7 @@ void ContinentalSrr520Decoder::PrintError(std::string error) } } -void ContinentalSrr520Decoder::PrintDebug(std::string debug) +void ContinentalSRR520Decoder::PrintDebug(std::string debug) { if (parent_node_logger_ptr_) { RCLCPP_DEBUG_STREAM((*parent_node_logger_ptr_), debug); diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp index c09941c5f..ed578b66c 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp @@ -33,11 +33,11 @@ namespace drivers namespace continental_ars548 { /// @brief Hardware interface of the Continental ARS548 radar -class ContinentalArs548HwInterface +class ContinentalARS548HwInterface { public: /// @brief Constructor - ContinentalArs548HwInterface(); + ContinentalARS548HwInterface(); /// @brief Starting the interface that handles UDP streams /// @return Resulting status @@ -51,7 +51,7 @@ class ContinentalArs548HwInterface /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status Status SetSensorConfiguration( - std::shared_ptr sensor_configuration); + std::shared_ptr sensor_configuration); /// @brief Registering callback /// @param callback Callback function @@ -165,7 +165,7 @@ class ContinentalArs548HwInterface std::unique_ptr<::drivers::common::IoContext> sensor_io_context_ptr_; std::unique_ptr<::drivers::udp_driver::UdpDriver> sensor_udp_driver_ptr_; - std::shared_ptr config_ptr_; + std::shared_ptr config_ptr_; std::function)> packet_callback_; std::shared_ptr parent_node_logger_ptr_; diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp index a3ae563e5..e40cfa37a 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp @@ -36,11 +36,11 @@ namespace drivers namespace continental_srr520 { /// @brief Hardware interface of the Continental SRR520 radar -class ContinentalSrr520HwInterface +class ContinentalSRR520HwInterface { public: /// @brief Constructor - ContinentalSrr520HwInterface(); + ContinentalSRR520HwInterface(); /// @brief Starting the interface that handles UDP streams /// @return Resulting status @@ -55,7 +55,7 @@ class ContinentalSrr520HwInterface /// @return Resulting status Status SetSensorConfiguration( const std::shared_ptr< - const nebula::drivers::continental_srr520::ContinentalSrr520SensorConfiguration> + const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration> new_config_ptr); /// @brief Registering callback for PandarScan @@ -131,7 +131,7 @@ class ContinentalSrr520HwInterface std::unique_ptr<::drivers::socketcan::SocketCanSender> can_sender_ptr_; std::unique_ptr receiver_thread_ptr_; - std::shared_ptr config_ptr_; + std::shared_ptr config_ptr_; std::function buffer)> nebula_packet_callback_; diff --git a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp index ef79e1015..bf05631b5 100644 --- a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp @@ -22,20 +22,20 @@ namespace drivers { namespace continental_ars548 { -ContinentalArs548HwInterface::ContinentalArs548HwInterface() +ContinentalARS548HwInterface::ContinentalARS548HwInterface() : sensor_io_context_ptr_{new ::drivers::common::IoContext(1)}, sensor_udp_driver_ptr_{new ::drivers::udp_driver::UdpDriver(*sensor_io_context_ptr_)} { } -Status ContinentalArs548HwInterface::SetSensorConfiguration( - std::shared_ptr new_config_ptr) +Status ContinentalARS548HwInterface::SetSensorConfiguration( + std::shared_ptr new_config_ptr) { config_ptr_ = new_config_ptr; return Status::OK; } -Status ContinentalArs548HwInterface::SensorInterfaceStart() +Status ContinentalARS548HwInterface::SensorInterfaceStart() { try { sensor_udp_driver_ptr_->init_receiver( @@ -45,7 +45,7 @@ Status ContinentalArs548HwInterface::SensorInterfaceStart() sensor_udp_driver_ptr_->receiver()->open(); sensor_udp_driver_ptr_->receiver()->bind(); sensor_udp_driver_ptr_->receiver()->asyncReceiveWithSender(std::bind( - &ContinentalArs548HwInterface::ReceiveSensorPacketCallbackWithSender, this, + &ContinentalARS548HwInterface::ReceiveSensorPacketCallbackWithSender, this, std::placeholders::_1, std::placeholders::_2)); sensor_udp_driver_ptr_->init_sender( @@ -66,21 +66,21 @@ Status ContinentalArs548HwInterface::SensorInterfaceStart() return Status::OK; } -Status ContinentalArs548HwInterface::RegisterCallback( +Status ContinentalARS548HwInterface::RegisterCallback( std::function)> packet_callback) { packet_callback_ = std::move(packet_callback); return Status::OK; } -void ContinentalArs548HwInterface::ReceiveSensorPacketCallbackWithSender( +void ContinentalARS548HwInterface::ReceiveSensorPacketCallbackWithSender( std::vector & buffer, const std::string & sender_ip) { if (sender_ip == config_ptr_->sensor_ip) { ReceiveSensorPacketCallback(buffer); } } -void ContinentalArs548HwInterface::ReceiveSensorPacketCallback(std::vector & buffer) +void ContinentalARS548HwInterface::ReceiveSensorPacketCallback(std::vector & buffer) { if (buffer.size() < sizeof(HeaderPacket)) { PrintError("Unrecognized packet. Too short"); @@ -99,12 +99,12 @@ void ContinentalArs548HwInterface::ReceiveSensorPacketCallback(std::vector ip_bytes; @@ -272,7 +272,7 @@ Status ContinentalArs548HwInterface::SetSensorIPAddress(const std::string & sens return Status::OK; } -Status ContinentalArs548HwInterface::SetAccelerationLateralCog(float lateral_acceleration) +Status ContinentalARS548HwInterface::SetAccelerationLateralCog(float lateral_acceleration) { constexpr uint16_t ACCELERATION_LATERAL_COG_SERVICE_ID = 0; constexpr uint16_t ACCELERATION_LATERAL_COG_METHOD_ID = 321; @@ -303,7 +303,7 @@ Status ContinentalArs548HwInterface::SetAccelerationLateralCog(float lateral_acc return Status::OK; } -Status ContinentalArs548HwInterface::SetAccelerationLongitudinalCog(float longitudinal_acceleration) +Status ContinentalARS548HwInterface::SetAccelerationLongitudinalCog(float longitudinal_acceleration) { constexpr uint16_t ACCELERATION_LONGITUDINAL_COG_SERVICE_ID = 0; constexpr uint16_t ACCELERATION_LONGITUDINAL_COG_METHOD_ID = 322; @@ -336,7 +336,7 @@ Status ContinentalArs548HwInterface::SetAccelerationLongitudinalCog(float longit return Status::OK; } -Status ContinentalArs548HwInterface::SetCharacteristicSpeed(float characteristic_speed) +Status ContinentalARS548HwInterface::SetCharacteristicSpeed(float characteristic_speed) { constexpr uint16_t CHARACTERISTIC_SPEED_SERVICE_ID = 0; constexpr uint16_t CHARACTERISTIC_SPEED_METHOD_ID = 328; @@ -367,7 +367,7 @@ Status ContinentalArs548HwInterface::SetCharacteristicSpeed(float characteristic return Status::OK; } -Status ContinentalArs548HwInterface::SetDrivingDirection(int direction) +Status ContinentalARS548HwInterface::SetDrivingDirection(int direction) { constexpr uint16_t DRIVING_DIRECTION_SERVICE_ID = 0; constexpr uint16_t DRIVING_DIRECTION_METHOD_ID = 325; @@ -403,7 +403,7 @@ Status ContinentalArs548HwInterface::SetDrivingDirection(int direction) return Status::OK; } -Status ContinentalArs548HwInterface::SetSteeringAngleFrontAxle(float angle_rad) +Status ContinentalARS548HwInterface::SetSteeringAngleFrontAxle(float angle_rad) { constexpr uint16_t STEERING_ANGLE_SERVICE_ID = 0; constexpr uint16_t STEERING_ANGLE_METHOD_ID = 327; @@ -435,7 +435,7 @@ Status ContinentalArs548HwInterface::SetSteeringAngleFrontAxle(float angle_rad) return Status::OK; } -Status ContinentalArs548HwInterface::SetVelocityVehicle(float velocity_kmh) +Status ContinentalARS548HwInterface::SetVelocityVehicle(float velocity_kmh) { if (velocity_kmh < 0.f || velocity_kmh > 350.f) { PrintError("Invalid velocity value"); @@ -466,7 +466,7 @@ Status ContinentalArs548HwInterface::SetVelocityVehicle(float velocity_kmh) return Status::OK; } -Status ContinentalArs548HwInterface::SetYawRate(float yaw_rate) +Status ContinentalARS548HwInterface::SetYawRate(float yaw_rate) { if (yaw_rate < -163.83 || yaw_rate > 163.83) { PrintError("Invalid yaw rate value"); @@ -497,12 +497,12 @@ Status ContinentalArs548HwInterface::SetYawRate(float yaw_rate) return Status::OK; } -void ContinentalArs548HwInterface::SetLogger(std::shared_ptr logger) +void ContinentalARS548HwInterface::SetLogger(std::shared_ptr logger) { parent_node_logger_ptr_ = logger; } -void ContinentalArs548HwInterface::PrintInfo(std::string info) +void ContinentalARS548HwInterface::PrintInfo(std::string info) { if (parent_node_logger_ptr_) { RCLCPP_INFO_STREAM((*parent_node_logger_ptr_), info); @@ -511,7 +511,7 @@ void ContinentalArs548HwInterface::PrintInfo(std::string info) } } -void ContinentalArs548HwInterface::PrintError(std::string error) +void ContinentalARS548HwInterface::PrintError(std::string error) { if (parent_node_logger_ptr_) { RCLCPP_ERROR_STREAM((*parent_node_logger_ptr_), error); @@ -520,7 +520,7 @@ void ContinentalArs548HwInterface::PrintError(std::string error) } } -void ContinentalArs548HwInterface::PrintDebug(std::string debug) +void ContinentalARS548HwInterface::PrintDebug(std::string debug) { if (parent_node_logger_ptr_) { RCLCPP_DEBUG_STREAM((*parent_node_logger_ptr_), debug); diff --git a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp index 9a34ea052..5a391baf6 100644 --- a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp @@ -25,13 +25,13 @@ namespace drivers { namespace continental_srr520 { -ContinentalSrr520HwInterface::ContinentalSrr520HwInterface() +ContinentalSRR520HwInterface::ContinentalSRR520HwInterface() { } -Status ContinentalSrr520HwInterface::SetSensorConfiguration( +Status ContinentalSRR520HwInterface::SetSensorConfiguration( const std::shared_ptr< - const nebula::drivers::continental_srr520::ContinentalSrr520SensorConfiguration> + const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration> new_config_ptr) { config_ptr_ = new_config_ptr; @@ -39,7 +39,7 @@ Status ContinentalSrr520HwInterface::SetSensorConfiguration( return Status::OK; } -Status ContinentalSrr520HwInterface::SensorInterfaceStart() +Status ContinentalSRR520HwInterface::SensorInterfaceStart() { std::lock_guard lock(receiver_mutex_); @@ -55,7 +55,7 @@ Status ContinentalSrr520HwInterface::SensorInterfaceStart() sensor_interface_active_ = true; receiver_thread_ptr_ = - std::make_unique(&ContinentalSrr520HwInterface::ReceiveLoop, this); + std::make_unique(&ContinentalSRR520HwInterface::ReceiveLoop, this); } catch (const std::exception & ex) { Status status = Status::CAN_CONNECTION_ERROR; std::cerr << status << config_ptr_->interface << std::endl; @@ -65,7 +65,7 @@ Status ContinentalSrr520HwInterface::SensorInterfaceStart() } template -bool ContinentalSrr520HwInterface::SendFrame(const std::array & data, int can_frame_id) +bool ContinentalSRR520HwInterface::SendFrame(const std::array & data, int can_frame_id) { ::drivers::socketcan::CanId send_id( can_frame_id, 0, ::drivers::socketcan::FrameType::DATA, ::drivers::socketcan::StandardFrame); @@ -82,7 +82,7 @@ bool ContinentalSrr520HwInterface::SendFrame(const std::array & data } } -void ContinentalSrr520HwInterface::ReceiveLoop() +void ContinentalSRR520HwInterface::ReceiveLoop() { ::drivers::socketcan::CanId receive_id{}; std::chrono::nanoseconds receiver_timeout_nsec; @@ -137,14 +137,14 @@ void ContinentalSrr520HwInterface::ReceiveLoop() } } -Status ContinentalSrr520HwInterface::RegisterPacketCallback( +Status ContinentalSRR520HwInterface::RegisterPacketCallback( std::function)> callback) { nebula_packet_callback_ = std::move(callback); return Status::OK; } -void ContinentalSrr520HwInterface::SensorSyncFup(builtin_interfaces::msg::Time stamp) +void ContinentalSRR520HwInterface::SensorSyncFup(builtin_interfaces::msg::Time stamp) { if (!can_sender_ptr_) { PrintError("Can sender is invalid so can not do follow up"); @@ -182,7 +182,7 @@ void ContinentalSrr520HwInterface::SensorSyncFup(builtin_interfaces::msg::Time s sync_counter_ = sync_counter_ == 15 ? 0 : sync_counter_ + 1; } -void ContinentalSrr520HwInterface::SensorSync() +void ContinentalSRR520HwInterface::SensorSync() { if (!can_sender_ptr_) { PrintError("Can sender is invalid so can not do sync up"); @@ -242,7 +242,7 @@ void ContinentalSrr520HwInterface::SensorSync() sync_fup_sent_ = true; } -Status ContinentalSrr520HwInterface::SensorInterfaceStop() +Status ContinentalSRR520HwInterface::SensorInterfaceStop() { { std::lock_guard l(receiver_mutex_); @@ -253,7 +253,7 @@ Status ContinentalSrr520HwInterface::SensorInterfaceStop() return Status::ERROR_1; } -Status ContinentalSrr520HwInterface::ConfigureSensor( +Status ContinentalSRR520HwInterface::ConfigureSensor( uint8_t sensor_id, float longitudinal_autosar, float lateral_autosar, float vertical_autosar, float yaw_autosar, float longitudinal_cog, float wheelbase, float cover_damping, bool plug_bottom, bool reset) @@ -319,7 +319,7 @@ Status ContinentalSrr520HwInterface::ConfigureSensor( } } -Status ContinentalSrr520HwInterface::SetVehicleDynamics( +Status ContinentalSRR520HwInterface::SetVehicleDynamics( float longitudinal_acceleration, float lateral_acceleration, float yaw_rate, float longitudinal_velocity, bool standstill) { @@ -363,12 +363,12 @@ Status ContinentalSrr520HwInterface::SetVehicleDynamics( } } -void ContinentalSrr520HwInterface::SetLogger(std::shared_ptr logger) +void ContinentalSRR520HwInterface::SetLogger(std::shared_ptr logger) { parent_node_logger_ptr_ = logger; } -void ContinentalSrr520HwInterface::PrintInfo(std::string info) +void ContinentalSRR520HwInterface::PrintInfo(std::string info) { if (parent_node_logger_ptr_) { RCLCPP_INFO_STREAM((*parent_node_logger_ptr_), info); @@ -377,7 +377,7 @@ void ContinentalSrr520HwInterface::PrintInfo(std::string info) } } -void ContinentalSrr520HwInterface::PrintError(std::string error) +void ContinentalSRR520HwInterface::PrintError(std::string error) { if (parent_node_logger_ptr_) { RCLCPP_ERROR_STREAM((*parent_node_logger_ptr_), error); @@ -386,7 +386,7 @@ void ContinentalSrr520HwInterface::PrintError(std::string error) } } -void ContinentalSrr520HwInterface::PrintDebug(std::string debug) +void ContinentalSRR520HwInterface::PrintDebug(std::string debug) { if (parent_node_logger_ptr_) { RCLCPP_DEBUG_STREAM((*parent_node_logger_ptr_), debug); diff --git a/nebula_ros/CMakeLists.txt b/nebula_ros/CMakeLists.txt index eb92c32e0..6fd02633e 100644 --- a/nebula_ros/CMakeLists.txt +++ b/nebula_ros/CMakeLists.txt @@ -86,7 +86,7 @@ ament_auto_add_library(continental_ars548_ros_wrapper SHARED ) rclcpp_components_register_node(continental_ars548_ros_wrapper - PLUGIN "ContinentalArs548RosWrapper" + PLUGIN "ContinentalARS548RosWrapper" EXECUTABLE continental_ars548_ros_wrapper_node ) @@ -98,7 +98,7 @@ ament_auto_add_library(continental_srr520_ros_wrapper SHARED ) rclcpp_components_register_node(continental_srr520_ros_wrapper - PLUGIN "ContinentalSrr520RosWrapper" + PLUGIN "ContinentalSRR520RosWrapper" EXECUTABLE continental_srr520_ros_wrapper_node ) diff --git a/nebula_ros/continental_ars548_decoder_ros_wrapper.cpp b/nebula_ros/continental_ars548_decoder_ros_wrapper.cpp deleted file mode 100644 index 552401f0f..000000000 --- a/nebula_ros/continental_ars548_decoder_ros_wrapper.cpp +++ /dev/null @@ -1,802 +0,0 @@ -// 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. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include - -namespace nebula -{ -namespace ros -{ -ContinentalArs548DriverRosWrapper::ContinentalArs548DriverRosWrapper( - const rclcpp::NodeOptions & options) -: rclcpp::Node("continental_ars548_driver_ros_wrapper", options), hw_interface_() -{ - drivers::continental_ars548::ContinentalArs548SensorConfiguration sensor_configuration; - - setvbuf(stdout, NULL, _IONBF, BUFSIZ); - - hw_interface_.SetLogger(std::make_shared(this->get_logger())); - - wrapper_status_ = GetParameters(sensor_configuration); - if (Status::OK != wrapper_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); - return; - } - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); - - sensor_cfg_ptr_ = - std::make_shared( - sensor_configuration); - - wrapper_status_ = InitializeDriver( - std::const_pointer_cast( - sensor_cfg_ptr_)); - - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); - packets_sub_ = create_subscription( - "nebula_packets", rclcpp::SensorDataQoS(), - std::bind( - &ContinentalArs548DriverRosWrapper::ReceivePacketsMsgCallback, this, std::placeholders::_1)); - - detection_list_pub_ = - this->create_publisher( - "continental_detections", rclcpp::SensorDataQoS()); - object_list_pub_ = this->create_publisher( - "continental_objects", rclcpp::SensorDataQoS()); - - detection_pointcloud_pub_ = this->create_publisher( - "detection_points", rclcpp::SensorDataQoS()); - object_pointcloud_pub_ = - this->create_publisher("object_points", rclcpp::SensorDataQoS()); - - scan_raw_pub_ = - this->create_publisher("scan_raw", rclcpp::SensorDataQoS()); - - objects_raw_pub_ = - this->create_publisher("objects_raw", rclcpp::SensorDataQoS()); - - objects_markers_pub_ = - this->create_publisher("marker_array", 10); - - diagnostics_pub_ = - this->create_publisher("diagnostics", 10); -} - -void ContinentalArs548DriverRosWrapper::ReceivePacketsMsgCallback( - const nebula_msgs::msg::NebulaPackets::SharedPtr scan_msg) -{ - decoder_ptr_->ProcessPackets(*scan_msg); -} - -Status ContinentalArs548DriverRosWrapper::InitializeDriver( - std::shared_ptr sensor_configuration) -{ - decoder_ptr_ = std::make_shared( - std::static_pointer_cast( - sensor_configuration)); - - decoder_ptr_->RegisterDetectionListCallback(std::bind( - &ContinentalArs548DriverRosWrapper::DetectionListCallback, this, std::placeholders::_1)); - decoder_ptr_->RegisterObjectListCallback( - std::bind(&ContinentalArs548DriverRosWrapper::ObjectListCallback, this, std::placeholders::_1)); - decoder_ptr_->RegisterSensorStatusCallback(std::bind( - &ContinentalArs548DriverRosWrapper::SensorStatusCallback, this, std::placeholders::_1)); - - return Status::OK; -} - -Status ContinentalArs548DriverRosWrapper::GetStatus() -{ - return wrapper_status_; -} - -Status ContinentalArs548DriverRosWrapper::GetParameters( - drivers::continental_ars548::ContinentalArs548SensorConfiguration & sensor_configuration) -{ - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("host_ip", descriptor); - sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_ip", descriptor); - sensor_configuration.sensor_ip = this->get_parameter("sensor_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("data_port", descriptor); - sensor_configuration.data_port = this->get_parameter("data_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model"); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("base_frame", descriptor); - sensor_configuration.base_frame = this->get_parameter("base_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("object_frame", descriptor); - sensor_configuration.object_frame = this->get_parameter("object_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("use_sensor_time", descriptor); - sensor_configuration.use_sensor_time = this->get_parameter("use_sensor_time").as_bool(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_length", descriptor); - sensor_configuration.configuration_vehicle_length = - static_cast(this->get_parameter("configuration_vehicle_length").as_double()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_width", descriptor); - sensor_configuration.configuration_vehicle_width = - static_cast(this->get_parameter("configuration_vehicle_width").as_double()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_height", descriptor); - sensor_configuration.configuration_vehicle_height = - static_cast(this->get_parameter("configuration_vehicle_height").as_double()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_wheelbase", descriptor); - sensor_configuration.configuration_vehicle_wheelbase = - static_cast(this->get_parameter("configuration_vehicle_wheelbase").as_double()); - } - - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - - std::shared_ptr sensor_cfg_ptr = - std::make_shared( - sensor_configuration); - - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; -} - -void ContinentalArs548DriverRosWrapper::DetectionListCallback( - std::unique_ptr msg) -{ - if ( - detection_pointcloud_pub_->get_subscription_count() > 0 || - detection_pointcloud_pub_->get_intra_process_subscription_count() > 0) { - const auto detection_pointcloud_ptr = ConvertToPointcloud(*msg); - auto detection_pointcloud_msg_ptr = std::make_unique(); - pcl::toROSMsg(*detection_pointcloud_ptr, *detection_pointcloud_msg_ptr); - - detection_pointcloud_msg_ptr->header = msg->header; - detection_pointcloud_pub_->publish(std::move(detection_pointcloud_msg_ptr)); - } - - if ( - scan_raw_pub_->get_subscription_count() > 0 || - scan_raw_pub_->get_intra_process_subscription_count() > 0) { - auto radar_scan_msg = ConvertToRadarScan(*msg); - radar_scan_msg.header = msg->header; - scan_raw_pub_->publish(std::move(radar_scan_msg)); - } - - if ( - detection_list_pub_->get_subscription_count() > 0 || - detection_list_pub_->get_intra_process_subscription_count() > 0) { - detection_list_pub_->publish(std::move(msg)); - } -} - -void ContinentalArs548DriverRosWrapper::ObjectListCallback( - std::unique_ptr msg) -{ - if ( - object_pointcloud_pub_->get_subscription_count() > 0 || - object_pointcloud_pub_->get_intra_process_subscription_count() > 0) { - const auto object_pointcloud_ptr = ConvertToPointcloud(*msg); - auto object_pointcloud_msg_ptr = std::make_unique(); - pcl::toROSMsg(*object_pointcloud_ptr, *object_pointcloud_msg_ptr); - - object_pointcloud_msg_ptr->header = msg->header; - object_pointcloud_pub_->publish(std::move(object_pointcloud_msg_ptr)); - } - - if ( - objects_raw_pub_->get_subscription_count() > 0 || - objects_raw_pub_->get_intra_process_subscription_count() > 0) { - auto objects_raw_msg = ConvertToRadarTracks(*msg); - objects_raw_msg.header = msg->header; - objects_raw_pub_->publish(std::move(objects_raw_msg)); - } - - if ( - objects_markers_pub_->get_subscription_count() > 0 || - objects_markers_pub_->get_intra_process_subscription_count() > 0) { - auto marker_array_msg = ConvertToMarkers(*msg); - objects_markers_pub_->publish(std::move(marker_array_msg)); - } - - if ( - object_list_pub_->get_subscription_count() > 0 || - object_list_pub_->get_intra_process_subscription_count() > 0) { - object_list_pub_->publish(std::move(msg)); - } -} - -void ContinentalArs548DriverRosWrapper::SensorStatusCallback( - const drivers::continental_ars548::ContinentalArs548Status & sensor_status) -{ - diagnostic_msgs::msg::DiagnosticArray diagnostic_array_msg; - diagnostic_array_msg.header.stamp.sec = sensor_status.timestamp_seconds; - diagnostic_array_msg.header.stamp.nanosec = sensor_status.timestamp_nanoseconds; - diagnostic_array_msg.header.frame_id = sensor_cfg_ptr_->frame_id; - - diagnostic_array_msg.status.resize(1); - auto & status = diagnostic_array_msg.status[0]; - status.values.reserve(36); - status.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - status.hardware_id = sensor_cfg_ptr_->frame_id; - status.name = sensor_cfg_ptr_->frame_id; - status.message = "Diagnostic messages from ARS548"; - - auto add_diagnostic = [&status](const std::string & key, const std::string & value) { - diagnostic_msgs::msg::KeyValue key_value; - key_value.key = key; - key_value.value = value; - status.values.push_back(key_value); - }; - - add_diagnostic("timestamp_nanoseconds", std::to_string(sensor_status.timestamp_nanoseconds)); - add_diagnostic("timestamp_seconds", std::to_string(sensor_status.timestamp_seconds)); - add_diagnostic("timestamp_sync_status", sensor_status.timestamp_sync_status); - add_diagnostic("sw_version_major", std::to_string(sensor_status.sw_version_major)); - add_diagnostic("sw_version_minor", std::to_string(sensor_status.sw_version_minor)); - add_diagnostic("sw_version_patch", std::to_string(sensor_status.sw_version_patch)); - add_diagnostic("longitudinal", std::to_string(sensor_status.longitudinal)); - add_diagnostic("lateral", std::to_string(sensor_status.lateral)); - add_diagnostic("vertical", std::to_string(sensor_status.vertical)); - add_diagnostic("yaw", std::to_string(sensor_status.yaw)); - add_diagnostic("pitch", std::to_string(sensor_status.pitch)); - add_diagnostic("plug_orientation", sensor_status.plug_orientation); - add_diagnostic("length", std::to_string(sensor_status.length)); - add_diagnostic("width", std::to_string(sensor_status.width)); - add_diagnostic("height", std::to_string(sensor_status.height)); - add_diagnostic("wheel_base", std::to_string(sensor_status.wheel_base)); - add_diagnostic("max_distance", std::to_string(sensor_status.max_distance)); - add_diagnostic("frequency_slot", sensor_status.frequency_slot); - add_diagnostic("cycle_time", std::to_string(sensor_status.cycle_time)); - add_diagnostic("time_slot", std::to_string(sensor_status.time_slot)); - add_diagnostic("hcc", sensor_status.hcc); - add_diagnostic("power_save_standstill", sensor_status.power_save_standstill); - add_diagnostic("sensor_ip_address0", sensor_status.sensor_ip_address0); - add_diagnostic("sensor_ip_address1", sensor_status.sensor_ip_address1); - add_diagnostic("configuration_counter", std::to_string(sensor_status.configuration_counter)); - add_diagnostic("longitudinal_velocity_status", sensor_status.longitudinal_velocity_status); - add_diagnostic( - "longitudinal_acceleration_status", sensor_status.longitudinal_acceleration_status); - add_diagnostic("lateral_acceleration_status", sensor_status.lateral_acceleration_status); - add_diagnostic("yaw_rate_status", sensor_status.yaw_rate_status); - add_diagnostic("steering_angle_status", sensor_status.steering_angle_status); - add_diagnostic("driving_direction_status", sensor_status.driving_direction_status); - add_diagnostic("characteristic_speed_status", sensor_status.characteristic_speed_status); - add_diagnostic("radar_status", sensor_status.radar_status); - add_diagnostic("voltage_status", sensor_status.voltage_status); - add_diagnostic("temperature_status", sensor_status.temperature_status); - add_diagnostic("blockage_status", sensor_status.blockage_status); - - double detection_total_time_sec = - (sensor_status.detection_last_stamp - sensor_status.detection_first_stamp) * 1e-9; - uint64_t expected_total_detection = - static_cast(detection_total_time_sec / (sensor_status.cycle_time * 1e-3)); - uint64_t detection_count_diff = - expected_total_detection > sensor_status.detection_total_count - ? expected_total_detection - sensor_status.detection_total_count - : sensor_status.detection_total_count - expected_total_detection; - double detection_dropped_rate = - 100.0 * std::abs(detection_count_diff) / expected_total_detection; - double detection_dropped_rate_dt = - 100.0 * sensor_status.detection_dropped_dt_count / sensor_status.detection_total_count; - double detection_empty_rate = - 100.0 * sensor_status.detection_empty_count / sensor_status.detection_total_count; - - add_diagnostic("detection_total_time", std::to_string(detection_total_time_sec)); - add_diagnostic("detection_dropped_rate", std::to_string(detection_dropped_rate)); - add_diagnostic("detection_dropped_rate_dt", std::to_string(detection_dropped_rate_dt)); - add_diagnostic("detection_empty_rate", std::to_string(detection_empty_rate)); - add_diagnostic( - "detection_dropped_dt_count", std::to_string(sensor_status.detection_dropped_dt_count)); - add_diagnostic("detection_empty_count", std::to_string(sensor_status.detection_empty_count)); - - double object_total_time_sec = - (sensor_status.object_last_stamp - sensor_status.object_first_stamp) * 1e-9; - uint64_t expected_total_object = - static_cast(object_total_time_sec / (sensor_status.cycle_time * 1e-3)); - uint64_t object_count_diff = expected_total_object > sensor_status.object_total_count - ? expected_total_object - sensor_status.object_total_count - : sensor_status.object_total_count - expected_total_object; - double object_dropped_rate = 100.0 * std::abs(object_count_diff) / expected_total_object; - double object_dropped_rate_dt = - 100.0 * sensor_status.object_dropped_dt_count / sensor_status.object_total_count; - double object_empty_rate = - 100.0 * sensor_status.object_empty_count / sensor_status.object_total_count; - - add_diagnostic("sensor_status.expected_total_object", std::to_string(expected_total_object)); - add_diagnostic( - "sensor_status.detection_total_count", std::to_string(sensor_status.detection_total_count)); - - add_diagnostic("object_total_time", std::to_string(object_total_time_sec)); - add_diagnostic("object_dropped_rate", std::to_string(object_dropped_rate)); - add_diagnostic("object_dropped_rate_dt", std::to_string(object_dropped_rate_dt)); - add_diagnostic("object_empty_rate", std::to_string(object_empty_rate)); - add_diagnostic("object_dropped_dt_count", std::to_string(sensor_status.object_dropped_dt_count)); - add_diagnostic("object_empty_count", std::to_string(sensor_status.object_empty_count)); - - add_diagnostic("status_total_count", std::to_string(sensor_status.status_total_count)); - add_diagnostic("radar_invalid_count", std::to_string(sensor_status.radar_invalid_count)); - - diagnostics_pub_->publish(diagnostic_array_msg); -} - -pcl::PointCloud::Ptr -ContinentalArs548DriverRosWrapper::ConvertToPointcloud( - const continental_msgs::msg::ContinentalArs548DetectionList & msg) -{ - pcl::PointCloud::Ptr output_pointcloud( - new pcl::PointCloud); - output_pointcloud->reserve(msg.detections.size()); - - nebula::drivers::continental_ars548::PointArs548Detection point{}; - for (const auto & detection : msg.detections) { - point.x = - std::cos(detection.elevation_angle) * std::cos(detection.azimuth_angle) * detection.range; - point.y = - std::cos(detection.elevation_angle) * std::sin(detection.azimuth_angle) * detection.range; - point.z = std::sin(detection.elevation_angle) * detection.range; - - point.azimuth = detection.azimuth_angle; - point.azimuth_std = detection.azimuth_angle_std; - point.elevation = detection.elevation_angle; - point.elevation_std = detection.elevation_angle_std; - point.range = detection.range; - point.range_std = detection.range_std; - point.range_rate = detection.range_rate; - point.range_rate_std = detection.range_rate_std; - point.rcs = detection.rcs; - point.measurement_id = detection.measurement_id; - point.positive_predictive_value = detection.positive_predictive_value; - point.classification = detection.classification; - point.multi_target_probability = detection.multi_target_probability; - point.object_id = detection.object_id; - point.ambiguity_flag = detection.ambiguity_flag; - - output_pointcloud->points.emplace_back(point); - } - - output_pointcloud->height = 1; - output_pointcloud->width = output_pointcloud->points.size(); - return output_pointcloud; -} - -pcl::PointCloud::Ptr -ContinentalArs548DriverRosWrapper::ConvertToPointcloud( - const continental_msgs::msg::ContinentalArs548ObjectList & msg) -{ - pcl::PointCloud::Ptr output_pointcloud( - new pcl::PointCloud); - output_pointcloud->reserve(msg.objects.size()); - - nebula::drivers::continental_ars548::PointArs548Object point{}; - for (const auto & object : msg.objects) { - point.x = static_cast(object.position.x); - point.y = static_cast(object.position.y); - point.z = static_cast(object.position.z); - - point.id = object.object_id; - point.age = object.age; - point.status_measurement = object.status_measurement; - point.status_movement = object.status_movement; - point.position_reference = object.position_reference; - point.classification_car = object.classification_car; - point.classification_truck = object.classification_truck; - point.classification_motorcycle = object.classification_motorcycle; - point.classification_bicycle = object.classification_bicycle; - point.classification_pedestrian = object.classification_pedestrian; - point.dynamics_abs_vel_x = static_cast(object.absolute_velocity.x); - point.dynamics_abs_vel_y = static_cast(object.absolute_velocity.y); - point.dynamics_rel_vel_x = static_cast(object.relative_velocity.x); - point.dynamics_rel_vel_y = static_cast(object.relative_velocity.y); - point.shape_length_edge_mean = object.shape_length_edge_mean; - point.shape_width_edge_mean = object.shape_width_edge_mean; - point.dynamics_orientation_rate_mean = object.orientation_rate_mean; - - output_pointcloud->points.emplace_back(point); - } - - output_pointcloud->height = 1; - output_pointcloud->width = output_pointcloud->points.size(); - return output_pointcloud; -} - -radar_msgs::msg::RadarScan ContinentalArs548DriverRosWrapper::ConvertToRadarScan( - const continental_msgs::msg::ContinentalArs548DetectionList & msg) -{ - radar_msgs::msg::RadarScan output_msg; - output_msg.header = msg.header; - output_msg.returns.reserve(msg.detections.size()); - - radar_msgs::msg::RadarReturn return_msg; - for (const auto & detection : msg.detections) { - if ( - detection.invalid_azimuth || detection.invalid_distance || detection.invalid_elevation || - detection.invalid_range_rate) { - continue; - } - - return_msg.range = detection.range; - return_msg.azimuth = detection.azimuth_angle; - return_msg.elevation = detection.elevation_angle; - return_msg.doppler_velocity = detection.range_rate; - return_msg.amplitude = detection.rcs; - output_msg.returns.emplace_back(return_msg); - } - - return output_msg; -} - -radar_msgs::msg::RadarTracks ContinentalArs548DriverRosWrapper::ConvertToRadarTracks( - const continental_msgs::msg::ContinentalArs548ObjectList & msg) -{ - radar_msgs::msg::RadarTracks output_msg; - output_msg.tracks.reserve(msg.objects.size()); - output_msg.header = msg.header; - - constexpr int16_t UNKNOWN_ID = 32000; - constexpr int16_t CAR_ID = 32001; - constexpr int16_t TRUCK_ID = 32002; - constexpr int16_t MOTORCYCLE_ID = 32005; - constexpr int16_t BICYCLE_ID = 32006; - constexpr int16_t PEDESTRIAN_ID = 32007; - constexpr float INVALID_COVARIANCE = 1e6; - - radar_msgs::msg::RadarTrack track_msg; - for (const auto & object : msg.objects) { - track_msg.uuid.uuid[0] = static_cast(object.object_id & 0xff); - track_msg.uuid.uuid[1] = static_cast((object.object_id >> 8) & 0xff); - track_msg.uuid.uuid[2] = static_cast((object.object_id >> 16) & 0xff); - track_msg.uuid.uuid[3] = static_cast((object.object_id >> 24) & 0xff); - - const double half_length = 0.5 * object.shape_length_edge_mean; - const double half_width = 0.5 * object.shape_width_edge_mean; - // There are 9 possible reference points. In the case of an invalid reference point, we fall - // back to the center - const int reference_index = std::min(object.position_reference, 8); - const double & yaw = object.orientation; - track_msg.position.x = object.position.x + - std::cos(yaw) * half_length * reference_to_center_[reference_index][0] - - std::sin(yaw) * half_width * reference_to_center_[reference_index][1]; - track_msg.position.y = object.position.y + - std::sin(yaw) * half_length * reference_to_center_[reference_index][0] + - std::cos(yaw) * half_width * reference_to_center_[reference_index][1]; - track_msg.position.z = object.position.z; - - track_msg.velocity = object.absolute_velocity; - track_msg.acceleration = object.absolute_acceleration; - track_msg.size.x = object.shape_length_edge_mean; - track_msg.size.y = object.shape_width_edge_mean; - track_msg.size.z = 1.f; - - uint8_t max_score = object.classification_unknown; - track_msg.classification = UNKNOWN_ID; - - if (object.classification_car > max_score) { - max_score = object.classification_car; - track_msg.classification = CAR_ID; - } - if (object.classification_truck > max_score) { - max_score = object.classification_truck; - track_msg.classification = TRUCK_ID; - } - if (object.classification_motorcycle > max_score) { - max_score = object.classification_motorcycle; - track_msg.classification = MOTORCYCLE_ID; - } - if (object.classification_bicycle > max_score) { - max_score = object.classification_bicycle; - track_msg.classification = BICYCLE_ID; - } - if (object.classification_pedestrian > max_score) { - max_score = object.classification_pedestrian; - track_msg.classification = PEDESTRIAN_ID; - } - - track_msg.position_covariance[0] = - static_cast(object.position_std.x * object.position_std.x); - track_msg.position_covariance[1] = object.position_covariance_xy; - track_msg.position_covariance[2] = 0.f; - track_msg.position_covariance[3] = - static_cast(object.position_std.y * object.position_std.y); - track_msg.position_covariance[4] = 0.f; - track_msg.position_covariance[5] = - static_cast(object.position_std.z * object.position_std.z); - - track_msg.velocity_covariance[0] = - static_cast(object.absolute_velocity_std.x * object.absolute_velocity_std.x); - track_msg.velocity_covariance[1] = object.absolute_velocity_covariance_xy; - track_msg.velocity_covariance[2] = 0.f; - track_msg.velocity_covariance[3] = - static_cast(object.absolute_velocity_std.y * object.absolute_velocity_std.y); - track_msg.velocity_covariance[4] = 0.f; - track_msg.velocity_covariance[5] = - static_cast(object.absolute_velocity_std.z * object.absolute_velocity_std.z); - - track_msg.acceleration_covariance[0] = - static_cast(object.absolute_acceleration_std.x * object.absolute_acceleration_std.x); - track_msg.acceleration_covariance[1] = object.absolute_acceleration_covariance_xy; - track_msg.acceleration_covariance[2] = 0.f; - track_msg.acceleration_covariance[3] = - static_cast(object.absolute_acceleration_std.y * object.absolute_acceleration_std.y); - track_msg.acceleration_covariance[4] = 0.f; - track_msg.acceleration_covariance[5] = - static_cast(object.absolute_acceleration_std.z * object.absolute_acceleration_std.z); - - track_msg.size_covariance[0] = INVALID_COVARIANCE; - track_msg.size_covariance[1] = 0.f; - track_msg.size_covariance[2] = 0.f; - track_msg.size_covariance[3] = INVALID_COVARIANCE; - track_msg.size_covariance[4] = 0.f; - track_msg.size_covariance[5] = INVALID_COVARIANCE; - - output_msg.tracks.emplace_back(track_msg); - } - - return output_msg; -} - -visualization_msgs::msg::MarkerArray ContinentalArs548DriverRosWrapper::ConvertToMarkers( - const continental_msgs::msg::ContinentalArs548ObjectList & msg) -{ - visualization_msgs::msg::MarkerArray marker_array; - marker_array.markers.reserve(4 * msg.objects.size()); - - constexpr int LINE_STRIP_CORNERS_NUM = 17; - constexpr std::array, LINE_STRIP_CORNERS_NUM> cube_corners = { - {{{-1.0, -1.0, -1.0}}, - {{-1.0, -1.0, 1.0}}, - {{-1.0, 1.0, 1.0}}, - {{-1.0, 1.0, -1.0}}, - {{-1.0, -1.0, -1.0}}, - {{1.0, -1.0, -1.0}}, - {{1.0, -1.0, 1.0}}, - {{1.0, 1.0, 1.0}}, - {{1.0, 1.0, -1.0}}, - {{1.0, -1.0, -1.0}}, - {{-1.0, -1.0, -1.0}}, - {{-1.0, -1.0, 1.0}}, - {{1.0, -1.0, 1.0}}, - {{1.0, 1.0, 1.0}}, - {{-1.0, 1.0, 1.0}}, - {{-1.0, 1.0, -1.0}}, - {{1.0, 1.0, -1.0}}}}; - - constexpr int PALETTE_SIZE = 32; - constexpr std::array, PALETTE_SIZE> color_array = {{ - {{1.0, 0.0, 0.0}}, {{0.0, 1.0, 0.0}}, - {{0.0, 0.0, 1.0}}, // Red, Green, Blue - {{1.0, 1.0, 0.0}}, {{0.0, 1.0, 1.0}}, - {{1.0, 0.0, 1.0}}, // Yellow, Cyan, Magenta - {{1.0, 0.647, 0.0}}, {{0.749, 1.0, 0.0}}, - {{0.0, 0.502, 0.502}}, // Orange, Lime, Teal - {{0.502, 0.0, 0.502}}, {{1.0, 0.753, 0.796}}, - {{0.647, 0.165, 0.165}}, // Purple, Pink, Brown - {{0.502, 0.0, 0.0}}, {{0.502, 0.502, 0.0}}, - {{0.0, 0.0, 0.502}}, // Maroon, Olive, Navy - {{0.502, 0.502, 0.502}}, {{1.0, 0.4, 0.4}}, - {{0.4, 1.0, 0.4}}, // Grey, Light Red, Light Green - {{0.4, 0.4, 1.0}}, {{1.0, 1.0, 0.4}}, - {{0.4, 1.0, 1.0}}, // Light Blue, Light Yellow, Light Cyan - {{1.0, 0.4, 1.0}}, {{1.0, 0.698, 0.4}}, - {{0.698, 0.4, 1.0}}, // Light Magenta, Light Orange, Light Purple - {{1.0, 0.6, 0.8}}, {{0.71, 0.396, 0.114}}, - {{0.545, 0.0, 0.0}}, // Light Pink, Light Brown, Dark Red - {{0.0, 0.392, 0.0}}, {{0.0, 0.0, 0.545}}, - {{0.545, 0.545, 0.0}}, // Dark Green, Dark Blue, Dark Yellow - {{0.0, 0.545, 0.545}}, {{0.545, 0.0, 0.545}} // Dark Cyan, Dark Magenta - }}; - - std::unordered_set current_ids; - - radar_msgs::msg::RadarTrack track_msg; - for (const auto & object : msg.objects) { - const double half_length = 0.5 * object.shape_length_edge_mean; - const double half_width = 0.5 * object.shape_width_edge_mean; - constexpr double DEFAULT_HALF_SIZE = 1.0; - const int reference_index = std::min(object.position_reference, 8); - const double & yaw = object.orientation; - current_ids.emplace(object.object_id); - - visualization_msgs::msg::Marker box_marker; - box_marker.header.frame_id = sensor_cfg_ptr_->object_frame; - box_marker.header.stamp = msg.header.stamp; - box_marker.ns = "boxes"; - box_marker.id = object.object_id; - box_marker.action = visualization_msgs::msg::Marker::ADD; - box_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; - box_marker.lifetime = rclcpp::Duration::from_seconds(0); - box_marker.color.r = color_array[object.object_id % PALETTE_SIZE][0]; - box_marker.color.g = color_array[object.object_id % PALETTE_SIZE][1]; - box_marker.color.b = color_array[object.object_id % PALETTE_SIZE][2]; - box_marker.color.a = 1.0; - box_marker.scale.x = 0.1; - - box_marker.pose.position.x = - object.position.x + std::cos(yaw) * half_length * reference_to_center_[reference_index][0] - - std::sin(yaw) * half_width * reference_to_center_[reference_index][1]; - box_marker.pose.position.y = - object.position.y + std::sin(yaw) * half_length * reference_to_center_[reference_index][0] + - std::cos(yaw) * half_width * reference_to_center_[reference_index][1]; - box_marker.pose.position.z = object.position.z; - box_marker.pose.orientation.w = std::cos(0.5 * yaw); - box_marker.pose.orientation.z = std::sin(0.5 * yaw); - - for (const auto & corner : cube_corners) { - geometry_msgs::msg::Point p; - p.x = half_length * corner[0]; - p.y = half_width * corner[1]; - p.z = DEFAULT_HALF_SIZE * corner[2]; - box_marker.points.emplace_back(p); - } - - marker_array.markers.emplace_back(box_marker); - - visualization_msgs::msg::Marker text_marker = box_marker; - text_marker.ns = "object_age"; - text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; - text_marker.color.r = 1.0; - text_marker.color.g = 1.0; - text_marker.color.b = 1.0; - text_marker.color.a = 1.0; - text_marker.scale.x = 0.3; - text_marker.scale.y = 0.3; - text_marker.scale.z = 0.3; - text_marker.pose.position.z += 0.5; - text_marker.text = - "ID=" + std::to_string(object.object_id) + " Age=" + std::to_string(object.age) + "ms"; - - marker_array.markers.emplace_back(text_marker); - - std::stringstream object_status_ss; - object_status_ss << std::fixed << std::setprecision(3) << "ID=" << object.object_id << "\n" - << static_cast(object.status_measurement) << "/" - << static_cast(object.status_movement) << "/" - << static_cast(object.position_reference); - - text_marker.ns = "object_status"; - text_marker.text = object_status_ss.str(); - - marker_array.markers.emplace_back(text_marker); - - std::stringstream object_dynamics_ss; - object_dynamics_ss << std::fixed << std::setprecision(3) << "ID=" << object.object_id - << "\nyaw=" << object.orientation - << "\nyaw_rate=" << object.orientation_rate_mean - << "\nvx=" << object.absolute_velocity.x - << "\nvy=" << object.absolute_velocity.y - << "\nax=" << object.absolute_acceleration.x - << "\nay=" << object.absolute_acceleration.y; - - text_marker.ns = "object_dynamics"; - text_marker.text = object_dynamics_ss.str(); - - marker_array.markers.emplace_back(text_marker); - } - - for (const auto & previous_id : previous_ids_) { - if (current_ids.find(previous_id) != current_ids.end()) { - continue; - } - - visualization_msgs::msg::Marker delete_marker; - delete_marker.header.frame_id = sensor_cfg_ptr_->object_frame; - delete_marker.header.stamp = msg.header.stamp; - delete_marker.ns = "boxes"; - delete_marker.id = previous_id; - delete_marker.action = visualization_msgs::msg::Marker::DELETE; - - marker_array.markers.push_back(delete_marker); - - delete_marker.ns = "object_age"; - marker_array.markers.push_back(delete_marker); - - delete_marker.ns = "object_status"; - marker_array.markers.push_back(delete_marker); - - delete_marker.ns = "object_dynamics"; - marker_array.markers.push_back(delete_marker); - } - - previous_ids_.clear(); - previous_ids_ = current_ids; - - return marker_array; -} - -RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalArs548DriverRosWrapper) -} // namespace ros -} // namespace nebula diff --git a/nebula_ros/continental_ars548_decoder_ros_wrapper.hpp b/nebula_ros/continental_ars548_decoder_ros_wrapper.hpp deleted file mode 100644 index eae105c25..000000000 --- a/nebula_ros/continental_ars548_decoder_ros_wrapper.hpp +++ /dev/null @@ -1,157 +0,0 @@ -// 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. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef NEBULA_ContinentalArs548DriverRosWrapper_H -#define NEBULA_ContinentalArs548DriverRosWrapper_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -namespace nebula -{ -namespace ros -{ -/// @brief Ros wrapper of continental radar ethernet driver -class ContinentalArs548DriverRosWrapper final : public rclcpp::Node, NebulaDriverRosWrapperBase -{ - std::shared_ptr decoder_ptr_; - Status wrapper_status_; - - rclcpp::Subscription::SharedPtr packets_sub_; - - rclcpp::Publisher::SharedPtr - detection_list_pub_; - rclcpp::Publisher::SharedPtr object_list_pub_; - rclcpp::Publisher::SharedPtr object_pointcloud_pub_; - rclcpp::Publisher::SharedPtr detection_pointcloud_pub_; - rclcpp::Publisher::SharedPtr scan_raw_pub_; - rclcpp::Publisher::SharedPtr objects_raw_pub_; - rclcpp::Publisher::SharedPtr objects_markers_pub_; - rclcpp::Publisher::SharedPtr diagnostics_pub_; - - std::unordered_set previous_ids_; - - constexpr static int REFERENCE_POINTS_NUM = 9; - constexpr static std::array, 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 - sensor_cfg_ptr_; - - drivers::continental_ars548::ContinentalArs548HwInterface hw_interface_; - - /// @brief Initializing ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @return Resulting status - Status InitializeDriver(std::shared_ptr sensor_configuration); - - /// @brief Get configurations from ros parameters - /// @param sensor_configuration Output of SensorConfiguration - /// @param calibration_configuration Output of CalibrationConfiguration - /// @param correction_configuration Output of CorrectionConfiguration (for AT) - /// @return Resulting status - Status GetParameters( - drivers::continental_ars548::ContinentalArs548SensorConfiguration & sensor_configuration); - - /// @brief Callback to process new ContinentalArs548DetectionList from the driver - /// @param msg The new ContinentalArs548DetectionList from the driver - void DetectionListCallback( - std::unique_ptr msg); - - /// @brief Callback to process new ContinentalArs548ObjectList from the driver - /// @param msg The new ContinentalArs548ObjectList from the driver - void ObjectListCallback(std::unique_ptr msg); - - /// @brief Callback to process new ContinentalArs548Status from the driver - /// @param msg The new ContinentalArs548ObjectList from the driver - void SensorStatusCallback( - const drivers::continental_ars548::ContinentalArs548Status & sensor_status); - -public: - explicit ContinentalArs548DriverRosWrapper(const rclcpp::NodeOptions & options); - - /// @brief Callback for NebulaPackets subscriber - /// @param scan_msg Received NebulaPackets - void ReceivePacketsMsgCallback(const nebula_msgs::msg::NebulaPackets::SharedPtr scan_msg); - - /// @brief Get current status of this driver - /// @return Current status - Status GetStatus(); - - /// @brief Convert ARS548 detections to a pointcloud - /// @param msg The ARS548 detection list msg - /// @return Resulting detection pointcloud - pcl::PointCloud::Ptr - ConvertToPointcloud(const continental_msgs::msg::ContinentalArs548DetectionList & msg); - - /// @brief Convert ARS548 objects to a pointcloud - /// @param msg The ARS548 object list msg - /// @return Resulting object pointcloud - pcl::PointCloud::Ptr ConvertToPointcloud( - const continental_msgs::msg::ContinentalArs548ObjectList & msg); - - /// @brief Convert ARS548 detections to a standard RadarScan msg - /// @param msg The ARS548 detection list msg - /// @return Resulting RadarScan msg - radar_msgs::msg::RadarScan ConvertToRadarScan( - const continental_msgs::msg::ContinentalArs548DetectionList & msg); - - /// @brief Convert ARS548 objects to a standard RadarTracks msg - /// @param msg The ARS548 object list msg - /// @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 -} // namespace nebula - -#endif // NEBULA_ContinentalArs548DriverRosWrapper_H diff --git a/nebula_ros/continental_ars548_hw_interface_ros_wrapper.cpp b/nebula_ros/continental_ars548_hw_interface_ros_wrapper.cpp deleted file mode 100644 index 9d7fb2901..000000000 --- a/nebula_ros/continental_ars548_hw_interface_ros_wrapper.cpp +++ /dev/null @@ -1,561 +0,0 @@ -// 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. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include -#include - -#include -#include - -namespace nebula -{ -namespace ros -{ -ContinentalArs548HwInterfaceRosWrapper::ContinentalArs548HwInterfaceRosWrapper( - const rclcpp::NodeOptions & options) -: rclcpp::Node("continental_ars548_hw_interface_ros_wrapper", options), hw_interface_() -{ - if (mtx_config_.try_lock()) { - interface_status_ = GetParameters(sensor_configuration_); - mtx_config_.unlock(); - } - if (Status::OK != interface_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); - return; - } - hw_interface_.SetLogger(std::make_shared(this->get_logger())); - std::shared_ptr - sensor_cfg_ptr = - std::make_shared( - sensor_configuration_); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - - hw_interface_.RegisterScanCallback(std::bind( - &ContinentalArs548HwInterfaceRosWrapper::ReceivePacketsDataCallback, this, - std::placeholders::_1)); - packets_pub_ = this->create_publisher( - "nebula_packets", rclcpp::SensorDataQoS()); - - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&ContinentalArs548HwInterfaceRosWrapper::paramCallback, this, std::placeholders::_1)); - - StreamStart(); -} - -ContinentalArs548HwInterfaceRosWrapper::~ContinentalArs548HwInterfaceRosWrapper() -{ -} - -Status ContinentalArs548HwInterfaceRosWrapper::StreamStart() -{ - if (Status::OK == interface_status_) { - interface_status_ = hw_interface_.SensorInterfaceStart(); - } - - if (Status::OK == interface_status_) { - odometry_sub_ = this->create_subscription( - "odometry_input", rclcpp::QoS{1}, - std::bind( - &ContinentalArs548HwInterfaceRosWrapper::OdometryCallback, this, std::placeholders::_1)); - - acceleration_sub_ = create_subscription( - "acceleration_input", rclcpp::QoS{1}, - std::bind( - &ContinentalArs548HwInterfaceRosWrapper::AccelerationCallback, this, - std::placeholders::_1)); - - steering_angle_sub_ = create_subscription( - "steering_angle_input", rclcpp::SensorDataQoS(), - std::bind( - &ContinentalArs548HwInterfaceRosWrapper::SteeringAngleCallback, this, - std::placeholders::_1)); - - set_network_configuration_service_server_ = - this->create_service( - "set_network_configuration", - std::bind( - &ContinentalArs548HwInterfaceRosWrapper::SetNetworkConfigurationRequestCallback, this, - std::placeholders::_1, std::placeholders::_2)); - - set_sensor_mounting_service_server_ = - this->create_service( - "set_sensor_mounting", - std::bind( - &ContinentalArs548HwInterfaceRosWrapper::SetSensorMountingRequestCallback, this, - std::placeholders::_1, std::placeholders::_2)); - - set_vehicle_parameters_service_server_ = - this->create_service( - "set_vehicle_parameters", - std::bind( - &ContinentalArs548HwInterfaceRosWrapper::SetVehicleParametersRequestCallback, this, - std::placeholders::_1, std::placeholders::_2)); - - set_radar_parameters_service_server_ = - this->create_service( - "set_radar_parameters", - std::bind( - &ContinentalArs548HwInterfaceRosWrapper::SetRadarParametersRequestCallback, this, - std::placeholders::_1, std::placeholders::_2)); - } - - return interface_status_; -} - -Status ContinentalArs548HwInterfaceRosWrapper::StreamStop() -{ - return Status::OK; -} -Status ContinentalArs548HwInterfaceRosWrapper::Shutdown() -{ - return Status::OK; -} - -Status ContinentalArs548HwInterfaceRosWrapper::InitializeHwInterface( // todo: don't think - // this is needed - const drivers::SensorConfigurationBase & sensor_configuration) -{ - std::stringstream ss; - ss << sensor_configuration; - RCLCPP_DEBUG_STREAM(this->get_logger(), ss.str()); - return Status::OK; -} - -Status ContinentalArs548HwInterfaceRosWrapper::GetParameters( - drivers::continental_ars548::ContinentalArs548SensorConfiguration & sensor_configuration) -{ - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", descriptor); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("host_ip", descriptor); - sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_ip", descriptor); - sensor_configuration.sensor_ip = this->get_parameter("sensor_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("multicast_ip", descriptor); - sensor_configuration.multicast_ip = this->get_parameter("multicast_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("base_frame", descriptor); - sensor_configuration.base_frame = this->get_parameter("base_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("object_frame", descriptor); - sensor_configuration.object_frame = this->get_parameter("object_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("data_port", descriptor); - sensor_configuration.data_port = this->get_parameter("data_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_host_port", descriptor); - sensor_configuration.configuration_host_port = - this->get_parameter("configuration_host_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_sensor_port", descriptor); - sensor_configuration.configuration_sensor_port = - this->get_parameter("configuration_sensor_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("use_sensor_time", descriptor); - sensor_configuration.use_sensor_time = this->get_parameter("use_sensor_time").as_bool(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_length", descriptor); - sensor_configuration.configuration_vehicle_length = - static_cast(this->get_parameter("configuration_vehicle_length").as_double()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_width", descriptor); - sensor_configuration.configuration_vehicle_width = - static_cast(this->get_parameter("configuration_vehicle_width").as_double()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_height", descriptor); - sensor_configuration.configuration_vehicle_height = - static_cast(this->get_parameter("configuration_vehicle_height").as_double()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_wheelbase", descriptor); - sensor_configuration.configuration_vehicle_wheelbase = - static_cast(this->get_parameter("configuration_vehicle_wheelbase").as_double()); - } - - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; -} - -void ContinentalArs548HwInterfaceRosWrapper::ReceivePacketsDataCallback( - std::unique_ptr scan_buffer) -{ - packets_pub_->publish(std::move(scan_buffer)); -} - -rcl_interfaces::msg::SetParametersResult ContinentalArs548HwInterfaceRosWrapper::paramCallback( - const std::vector & p) -{ - std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback"); - RCLCPP_DEBUG_STREAM(this->get_logger(), p); - RCLCPP_DEBUG_STREAM(this->get_logger(), sensor_configuration_); - RCLCPP_INFO_STREAM(this->get_logger(), p); - - drivers::continental_ars548::ContinentalArs548SensorConfiguration new_param{ - sensor_configuration_}; - RCLCPP_INFO_STREAM(this->get_logger(), new_param); - std::string sensor_model_str; - - if ( - get_param(p, "sensor_model", sensor_model_str) | get_param(p, "host_ip", new_param.host_ip) | - get_param(p, "sensor_ip", new_param.sensor_ip) | get_param(p, "frame_id", new_param.frame_id) | - get_param(p, "data_port", new_param.data_port) | - get_param(p, "multicast_ip", new_param.multicast_ip) | - get_param(p, "base_frame", new_param.base_frame) | - get_param(p, "object_frame", new_param.object_frame) | - get_param(p, "configuration_host_port", new_param.configuration_host_port) | - get_param(p, "configuration_sensor_port", new_param.configuration_sensor_port) | - get_param(p, "configuration_vehicle_length", new_param.configuration_vehicle_length) | - get_param(p, "configuration_vehicle_width", new_param.configuration_vehicle_width) | - get_param(p, "configuration_vehicle_height", new_param.configuration_vehicle_height) | - get_param(p, "configuration_vehicle_wheelbase", new_param.configuration_vehicle_wheelbase) | - get_param(p, "configuration_host_port", new_param.configuration_host_port) | - get_param(p, "configuration_sensor_port", new_param.configuration_sensor_port)) { - if (0 < sensor_model_str.length()) - new_param.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_str); - - sensor_configuration_ = new_param; - RCLCPP_INFO_STREAM(this->get_logger(), "Update sensor_configuration"); - std::shared_ptr sensor_cfg_ptr = - std::make_shared( - sensor_configuration_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - hw_interface_.CheckAndSetConfig(); - } - - auto result = std::make_shared(); - result->successful = true; - result->reason = "success"; - - RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback success"); - - return *result; -} - -void ContinentalArs548HwInterfaceRosWrapper::OdometryCallback( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) -{ - std::scoped_lock lock(mtx_config_); - - constexpr float speed_to_standstill = 0.5f; - constexpr float speed_to_moving = 2.f; - - if (standstill_ && std::abs(msg->twist.twist.linear.x) > speed_to_moving) { - standstill_ = false; - } else if (!standstill_ && std::abs(msg->twist.twist.linear.x) < speed_to_standstill) { - standstill_ = true; - } - - if (standstill_) { - hw_interface_.SetDrivingDirection(0); - } else { - hw_interface_.SetDrivingDirection(msg->twist.twist.linear.x > 0.f ? 1 : -1); - } - - constexpr float ms_to_kmh = 3.6f; - hw_interface_.SetVelocityVehicle(ms_to_kmh * std::abs(msg->twist.twist.linear.x)); - - constexpr float rad_to_deg = 180.f / M_PI; - hw_interface_.SetYawRate(rad_to_deg * msg->twist.twist.angular.z); -} - -void ContinentalArs548HwInterfaceRosWrapper::AccelerationCallback( - const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg) -{ - std::scoped_lock lock(mtx_config_); - hw_interface_.SetAccelerationLateralCog(msg->accel.accel.linear.y); - hw_interface_.SetAccelerationLongitudinalCog(msg->accel.accel.linear.x); -} - -void ContinentalArs548HwInterfaceRosWrapper::SteeringAngleCallback( - const std_msgs::msg::Float32::SharedPtr msg) -{ - std::scoped_lock lock(mtx_config_); - constexpr float rad_to_deg = 180.f / M_PI; - hw_interface_.SetSteeringAngleFrontAxle(rad_to_deg * msg->data); -} - -void ContinentalArs548HwInterfaceRosWrapper::SetNetworkConfigurationRequestCallback( - const std::shared_ptr - request, - const std::shared_ptr - response) -{ - std::scoped_lock lock(mtx_config_); - auto result = hw_interface_.SetSensorIPAddress(request->sensor_ip.data); - response->success = result == Status::OK; - response->message = (std::stringstream() << result).str(); -} - -void ContinentalArs548HwInterfaceRosWrapper::SetSensorMountingRequestCallback( - const std::shared_ptr request, - const std::shared_ptr - response) -{ - std::scoped_lock lock(mtx_config_); - - auto tf_buffer = std::make_unique(this->get_clock()); - auto tf_listener = std::make_unique(*tf_buffer); - - float longitudinal = request->longitudinal; - float lateral = request->lateral; - float vertical = request->vertical; - float yaw = request->yaw; - float pitch = request->pitch; - - if (request->autoconfigure_extrinsics) { - RCLCPP_INFO( - this->get_logger(), - "autoconfigure_extrinsics was set to true, so the mounting position will be derived from the " - "tfs"); - - geometry_msgs::msg::TransformStamped base_to_sensor_tf; - try { - base_to_sensor_tf = tf_buffer->lookupTransform( - sensor_configuration_.base_frame, sensor_configuration_.frame_id, rclcpp::Time(0), - rclcpp::Duration::from_seconds(0.5)); - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR( - this->get_logger(), "Could not obtain the transform from the base frame to %s (%s)", - sensor_configuration_.frame_id.c_str(), ex.what()); - response->success = false; - response->message = ex.what(); - return; - } - - const auto & quat = base_to_sensor_tf.transform.rotation; - geometry_msgs::msg::Vector3 rpy; - tf2::Matrix3x3(tf2::Quaternion(quat.x, quat.y, quat.z, quat.w)).getRPY(rpy.x, rpy.y, rpy.z); - - longitudinal = base_to_sensor_tf.transform.translation.x - - sensor_configuration_.configuration_vehicle_wheelbase; - lateral = base_to_sensor_tf.transform.translation.y; - vertical = base_to_sensor_tf.transform.translation.z; - yaw = rpy.z; - pitch = rpy.y; - } - - auto result = hw_interface_.SetSensorMounting( - longitudinal, lateral, vertical, yaw, pitch, request->plug_orientation); - - response->success = result == Status::OK; - response->message = (std::stringstream() << result).str(); -} - -void ContinentalArs548HwInterfaceRosWrapper::SetVehicleParametersRequestCallback( - [[maybe_unused]] const std::shared_ptr< - continental_srvs::srv::ContinentalArs548SetVehicleParameters::Request> - request, - const std::shared_ptr - response) -{ - float vehicle_length = request->vehicle_length; - float vehicle_width = request->vehicle_width; - float vehicle_height = request->vehicle_height; - float vehicle_wheelbase = request->vehicle_wheelbase; - - if (vehicle_length < 0.f) { - RCLCPP_INFO( - this->get_logger(), - "Service vehicle_length is invalid. Falling back to configuration value (%.2f)", - sensor_configuration_.configuration_vehicle_length); - vehicle_length = sensor_configuration_.configuration_vehicle_length; - } - - if (vehicle_width < 0.f) { - RCLCPP_INFO( - this->get_logger(), - "Service vehicle_width is invalid. Falling back to configuration value (%.2f)", - sensor_configuration_.configuration_vehicle_width); - vehicle_width = sensor_configuration_.configuration_vehicle_width; - } - - if (vehicle_height < 0.f) { - RCLCPP_INFO( - this->get_logger(), - "Service vehicle_height is invalid. Falling back to configuration value (%.2f)", - sensor_configuration_.configuration_vehicle_height); - vehicle_height = sensor_configuration_.configuration_vehicle_height; - } - - if (vehicle_wheelbase < 0.f) { - RCLCPP_INFO( - this->get_logger(), - "Service vehicle_wheelbase is invalid. Falling back to configuration value (%.2f)", - sensor_configuration_.configuration_vehicle_wheelbase); - vehicle_wheelbase = sensor_configuration_.configuration_vehicle_wheelbase; - } - - std::scoped_lock lock(mtx_config_); - auto result = hw_interface_.SetVehicleParameters( - vehicle_length, vehicle_width, vehicle_height, vehicle_wheelbase); - - response->success = result == Status::OK; - response->message = (std::stringstream() << result).str(); -} - -void ContinentalArs548HwInterfaceRosWrapper::SetRadarParametersRequestCallback( - const std::shared_ptr - request, - const std::shared_ptr - response) -{ - std::scoped_lock lock(mtx_config_); - auto result = hw_interface_.SetRadarParameters( - request->maximum_distance, request->frequency_slot, request->cycle_time, request->time_slot, - request->country_code, request->powersave_standstill); - - response->success = result == Status::OK; - response->message = (std::stringstream() << result).str(); -} - -std::vector -ContinentalArs548HwInterfaceRosWrapper::updateParameters() -{ - std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters start"); - std::ostringstream os_sensor_model; - os_sensor_model << sensor_configuration_.sensor_model; - RCLCPP_INFO_STREAM(this->get_logger(), "set_parameters"); - auto results = set_parameters( - {rclcpp::Parameter("sensor_model", os_sensor_model.str()), - rclcpp::Parameter("host_ip", sensor_configuration_.host_ip), - rclcpp::Parameter("sensor_ip", sensor_configuration_.sensor_ip), - rclcpp::Parameter("frame_id", sensor_configuration_.frame_id), - rclcpp::Parameter("data_port", sensor_configuration_.data_port), - rclcpp::Parameter("multicast_ip", sensor_configuration_.multicast_ip), - rclcpp::Parameter("base_frame", sensor_configuration_.base_frame), - rclcpp::Parameter("object_frame", sensor_configuration_.object_frame), - rclcpp::Parameter("configuration_host_port", sensor_configuration_.configuration_host_port), - rclcpp::Parameter( - "configuration_sensor_port", sensor_configuration_.configuration_sensor_port), - rclcpp::Parameter( - "configuration_vehicle_length", sensor_configuration_.configuration_vehicle_length), - rclcpp::Parameter( - "configuration_vehicle_width", sensor_configuration_.configuration_vehicle_width), - rclcpp::Parameter( - "configuration_vehicle_height", sensor_configuration_.configuration_vehicle_height), - rclcpp::Parameter( - "configuration_vehicle_wheelbase", sensor_configuration_.configuration_vehicle_wheelbase)}); - RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters end"); - return results; -} - -RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalArs548HwInterfaceRosWrapper) -} // namespace ros -} // namespace nebula diff --git a/nebula_ros/continental_ars548_hw_interface_ros_wrapper.hpp b/nebula_ros/continental_ars548_hw_interface_ros_wrapper.hpp deleted file mode 100644 index fc8ff6f57..000000000 --- a/nebula_ros/continental_ars548_hw_interface_ros_wrapper.hpp +++ /dev/null @@ -1,188 +0,0 @@ -// 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. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef NEBULA_ContinentalArs548HwInterfaceRosWrapper_H -#define NEBULA_ContinentalArs548HwInterfaceRosWrapper_H - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include - -namespace nebula -{ -namespace ros -{ -/// @brief Get parameter from rclcpp::Parameter -/// @tparam T -/// @param p Parameter from rclcpp parameter callback -/// @param name Target parameter name -/// @param value Corresponding value -/// @return Whether the target name existed -template -bool get_param(const std::vector & p, const std::string & name, T & value) -{ - auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { - return parameter.get_name() == name; - }); - if (it != p.cend()) { - value = it->template get_value(); - return true; - } - return false; -} - -/// @brief Hardware interface ros wrapper of continental radar ethernet driver -class ContinentalArs548HwInterfaceRosWrapper final : public rclcpp::Node, - NebulaHwInterfaceWrapperBase -{ - drivers::continental_ars548::ContinentalArs548HwInterface hw_interface_; - Status interface_status_; - - drivers::continental_ars548::ContinentalArs548SensorConfiguration sensor_configuration_; - - /// @brief Received Continental Radar message publisher - rclcpp::Publisher::SharedPtr packets_pub_; - rclcpp::Subscription::SharedPtr odometry_sub_; - rclcpp::Subscription::SharedPtr acceleration_sub_; - rclcpp::Subscription::SharedPtr steering_angle_sub_; - - bool standstill_{true}; - - rclcpp::Service::SharedPtr - set_network_configuration_service_server_; - rclcpp::Service::SharedPtr - set_sensor_mounting_service_server_; - rclcpp::Service::SharedPtr - set_vehicle_parameters_service_server_; - rclcpp::Service::SharedPtr - set_radar_parameters_service_server_; - - /// @brief Initializing hardware interface ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @return Resulting status - Status InitializeHwInterface( - const drivers::SensorConfigurationBase & sensor_configuration) override; - /// @brief Callback for receiving NebulaPackets - /// @param packets_buffer Received NebulaPackets - void ReceivePacketsDataCallback(std::unique_ptr packets_buffer); - - /// @brief Callback to send the odometry information to the radar device - /// @param msg The odometry message - void OdometryCallback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); - - /// @brief Callback to send the acceleration information to the radar device - /// @param msg The acceleration message - void AccelerationCallback(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg); - - /// @brief Callback to send the steering angle information to the radar device - /// @param msg The steering angle message - void SteeringAngleCallback(const std_msgs::msg::Float32::SharedPtr msg); - - /// @brief Service callback to set the new sensor ip - /// @param request service request - /// @param response service response - void SetNetworkConfigurationRequestCallback( - const std::shared_ptr - request, - const std::shared_ptr - response); - - /// @brief Service callback to set the new sensor mounting position - /// @param request service request - /// @param response service response - void SetSensorMountingRequestCallback( - const std::shared_ptr - request, - const std::shared_ptr - response); - - /// @brief Service callback to set the new vehicle parameters - /// @param request service request - /// @param response service response - void SetVehicleParametersRequestCallback( - const std::shared_ptr - request, - const std::shared_ptr - response); - - /// @brief Service callback to set the new radar parameters - /// @param request service request - /// @param response service response - void SetRadarParametersRequestCallback( - const std::shared_ptr - request, - const std::shared_ptr - response); - -public: - explicit ContinentalArs548HwInterfaceRosWrapper(const rclcpp::NodeOptions & options); - ~ContinentalArs548HwInterfaceRosWrapper() noexcept override; - - /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) - /// @return Resulting status - Status StreamStart() override; - /// @brief Stop point cloud streaming (not used) - /// @return Resulting status - Status StreamStop() override; - /// @brief Shutdown (not used) - /// @return Resulting status - Status Shutdown() override; - /// @brief Get configurations from ros parameters - /// @param sensor_configuration Output of SensorConfiguration - /// @return Resulting status - Status GetParameters( - drivers::continental_ars548::ContinentalArs548SensorConfiguration & sensor_configuration); - -private: - std::mutex mtx_config_; - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - - /// @brief rclcpp parameter callback - /// @param parameters Received parameters - /// @return SetParametersResult - rcl_interfaces::msg::SetParametersResult paramCallback( - const std::vector & parameters); - - /// @brief Updating rclcpp parameter - /// @return SetParametersResult - std::vector updateParameters(); -}; - -} // namespace ros -} // namespace nebula - -#endif // NEBULA_ContinentalArs548HwInterfaceRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp index 8046fbbfd..89d664429 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp @@ -43,20 +43,20 @@ namespace nebula { namespace ros { -class ContinentalArs548DecoderWrapper +class ContinentalARS548DecoderWrapper { public: - ContinentalArs548DecoderWrapper( + ContinentalARS548DecoderWrapper( rclcpp::Node * const parent_node, std::shared_ptr< - const nebula::drivers::continental_ars548::ContinentalArs548SensorConfiguration> & config, + const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration> & config, bool launch_hw); void ProcessPacket(std::unique_ptr packet_msg); void OnConfigChange( const std::shared_ptr< - const nebula::drivers::continental_ars548::ContinentalArs548SensorConfiguration> & + const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration> & new_config); rcl_interfaces::msg::SetParametersResult OnParameterChange( @@ -73,30 +73,30 @@ class ContinentalArs548DecoderWrapper /// @param msg The new ContinentalArs548ObjectList from the driver void ObjectListCallback(std::unique_ptr msg); - /// @brief Callback to process new ContinentalArs548Status from the driver + /// @brief Callback to process new ContinentalARS548Status from the driver /// @param msg The new ContinentalArs548ObjectList from the driver void SensorStatusCallback( - const drivers::continental_ars548::ContinentalArs548Status & sensor_status); + const drivers::continental_ars548::ContinentalARS548Status & sensor_status); - /// @brief Callback to process new ContinentalArs548Status from the driver + /// @brief Callback to process new ContinentalARS548Status from the driver /// @param msg The new ContinentalArs548ObjectList from the driver void PacketsCallback(std::unique_ptr msg); private: nebula::Status InitializeDriver( const std::shared_ptr< - const nebula::drivers::continental_ars548::ContinentalArs548SensorConfiguration> & config); + const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration> & config); /// @brief Convert ARS548 detections to a pointcloud /// @param msg The ARS548 detection list msg /// @return Resulting detection pointcloud - pcl::PointCloud::Ptr + pcl::PointCloud::Ptr ConvertToPointcloud(const continental_msgs::msg::ContinentalArs548DetectionList & msg); /// @brief Convert ARS548 objects to a pointcloud /// @param msg The ARS548 object list msg /// @return Resulting object pointcloud - pcl::PointCloud::Ptr ConvertToPointcloud( + pcl::PointCloud::Ptr ConvertToPointcloud( const continental_msgs::msg::ContinentalArs548ObjectList & msg); /// @brief Convert ARS548 detections to a standard RadarScan msg @@ -120,10 +120,10 @@ class ContinentalArs548DecoderWrapper nebula::Status status_; rclcpp::Logger logger_; - std::shared_ptr + std::shared_ptr config_ptr_{}; - std::shared_ptr driver_ptr_{}; + std::shared_ptr driver_ptr_{}; std::mutex mtx_driver_ptr_; rclcpp::Publisher::SharedPtr packets_pub_{}; diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp index e29d44068..071b67126 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp @@ -32,26 +32,26 @@ namespace nebula { namespace ros { -class ContinentalArs548HwInterfaceWrapper +class ContinentalARS548HwInterfaceWrapper { public: - ContinentalArs548HwInterfaceWrapper( + ContinentalARS548HwInterfaceWrapper( rclcpp::Node * const parent_node, - std::shared_ptr & + std::shared_ptr & config); /// @brief Starts the hw interface and subscribes to the input topics void SensorInterfaceStart(); void OnConfigChange( - const std::shared_ptr & + const std::shared_ptr & new_config_ptr); /// @brief Get current status of the hw interface /// @return Current status nebula::Status Status(); - std::shared_ptr HwInterface() const; + std::shared_ptr HwInterface() const; private: /// @brief Callback to send the odometry information to the radar device @@ -103,10 +103,10 @@ class ContinentalArs548HwInterfaceWrapper response); rclcpp::Node * const parent_node_; - std::shared_ptr hw_interface_{}; + std::shared_ptr hw_interface_{}; rclcpp::Logger logger_; nebula::Status status_{}; - std::shared_ptr + std::shared_ptr config_ptr_{}; rclcpp::Subscription::SharedPtr odometry_sub_{}; diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp index c2c0b85e4..f136f6a11 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp @@ -41,11 +41,11 @@ namespace ros { /// @brief Ros wrapper of continental ars548 driver -class ContinentalArs548RosWrapper final : public rclcpp::Node +class ContinentalARS548RosWrapper final : public rclcpp::Node { public: - explicit ContinentalArs548RosWrapper(const rclcpp::NodeOptions & options); - ~ContinentalArs548RosWrapper() noexcept {}; + explicit ContinentalARS548RosWrapper(const rclcpp::NodeOptions & options); + ~ContinentalARS548RosWrapper() noexcept {}; /// @brief Get current status of this driver /// @return Current status @@ -73,12 +73,12 @@ class ContinentalArs548RosWrapper final : public rclcpp::Node const std::vector & p); Status ValidateAndSetConfig( - std::shared_ptr & + std::shared_ptr & new_config); Status wrapper_status_; - std::shared_ptr + std::shared_ptr config_ptr_{}; /// @brief Stores received packets that have not been processed yet by the decoder thread @@ -90,8 +90,8 @@ class ContinentalArs548RosWrapper final : public rclcpp::Node bool launch_hw_{}; - std::optional hw_interface_wrapper_{}; - std::optional decoder_wrapper_{}; + std::optional hw_interface_wrapper_{}; + std::optional decoder_wrapper_{}; std::mutex mtx_config_; diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp index 7b5e09f70..787203d88 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp @@ -44,20 +44,20 @@ namespace nebula { namespace ros { -class ContinentalSrr520DecoderWrapper +class ContinentalSRR520DecoderWrapper { public: - ContinentalSrr520DecoderWrapper( + ContinentalSRR520DecoderWrapper( rclcpp::Node * const parent_node, - std::shared_ptr & + std::shared_ptr & config, - std::shared_ptr hw_interface_ptr); + std::shared_ptr hw_interface_ptr); void ProcessPacket(std::unique_ptr packet_msg); void OnConfigChange( const std::shared_ptr< - const nebula::drivers::continental_srr520::ContinentalSrr520SensorConfiguration> & + const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration> & new_config_ptr); rcl_interfaces::msg::SetParametersResult OnParameterChange( @@ -93,18 +93,18 @@ class ContinentalSrr520DecoderWrapper private: nebula::Status InitializeDriver( const std::shared_ptr< - const nebula::drivers::continental_srr520::ContinentalSrr520SensorConfiguration> & config); + const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration> & config); /// @brief Convert SRR520 detections to a pointcloud /// @param msg The SRR520 detection list msg /// @return Resulting detection pointcloud - pcl::PointCloud::Ptr + pcl::PointCloud::Ptr ConvertToPointcloud(const continental_msgs::msg::ContinentalSrr520DetectionList & msg); /// @brief Convert SRR520 objects to a pointcloud /// @param msg The SRR520 object list msg /// @return Resulting object pointcloud - pcl::PointCloud::Ptr ConvertToPointcloud( + pcl::PointCloud::Ptr ConvertToPointcloud( const continental_msgs::msg::ContinentalSrr520ObjectList & msg); /// @brief Convert SRR520 detections to a standard RadarScan msg @@ -129,11 +129,11 @@ class ContinentalSrr520DecoderWrapper nebula::Status status_; rclcpp::Logger logger_; - std::shared_ptr + std::shared_ptr sensor_cfg_{}; - std::shared_ptr driver_ptr_{}; - std::shared_ptr hw_interface_ptr_{}; + std::shared_ptr driver_ptr_{}; + std::shared_ptr hw_interface_ptr_{}; std::mutex mtx_driver_ptr_; rclcpp::Publisher::SharedPtr packets_pub_{}; diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp index 39a008b55..ff664c968 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp @@ -33,26 +33,26 @@ namespace nebula { namespace ros { -class ContinentalSrr520HwInterfaceWrapper +class ContinentalSRR520HwInterfaceWrapper { public: - ContinentalSrr520HwInterfaceWrapper( + ContinentalSRR520HwInterfaceWrapper( rclcpp::Node * const parent_node, - std::shared_ptr & + std::shared_ptr & config); /// @brief Starts the hw interface and subscribes to the input topics void SensorInterfaceStart(); void OnConfigChange( - const std::shared_ptr & + const std::shared_ptr & new_config); /// @brief Get current status of the hw interface /// @return Current status nebula::Status Status(); - std::shared_ptr HwInterface() const; + std::shared_ptr HwInterface() const; private: /// @brief Callback to send the odometry information to the radar device @@ -75,10 +75,10 @@ class ContinentalSrr520HwInterfaceWrapper void syncTimerCallback(); rclcpp::Node * const parent_node_; - std::shared_ptr hw_interface_{}; + std::shared_ptr hw_interface_{}; rclcpp::Logger logger_; nebula::Status status_{}; - std::shared_ptr + std::shared_ptr config_ptr_{}; message_filters::Subscriber odometry_sub_; diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp index d6af211db..7b373d03a 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp @@ -39,11 +39,11 @@ namespace ros { /// @brief Ros wrapper of continental srr520 driver -class ContinentalSrr520RosWrapper final : public rclcpp::Node +class ContinentalSRR520RosWrapper final : public rclcpp::Node { public: - explicit ContinentalSrr520RosWrapper(const rclcpp::NodeOptions & options); - ~ContinentalSrr520RosWrapper() noexcept {}; + explicit ContinentalSRR520RosWrapper(const rclcpp::NodeOptions & options); + ~ContinentalSRR520RosWrapper() noexcept {}; /// @brief Get current status of this driver /// @return Current status @@ -71,12 +71,12 @@ class ContinentalSrr520RosWrapper final : public rclcpp::Node const std::vector & p); Status ValidateAndSetConfig( - std::shared_ptr & + std::shared_ptr & new_config); Status wrapper_status_; - std::shared_ptr + std::shared_ptr config_ptr_{}; /// @brief Stores received packets that have not been processed yet by the decoder thread @@ -88,8 +88,8 @@ class ContinentalSrr520RosWrapper final : public rclcpp::Node bool launch_hw_{}; - std::optional hw_interface_wrapper_{}; - std::optional decoder_wrapper_{}; + std::optional hw_interface_wrapper_{}; + std::optional decoder_wrapper_{}; std::mutex mtx_config_; diff --git a/nebula_ros/multi_continental_ars548_hw_interface_ros_wrapper.cpp b/nebula_ros/multi_continental_ars548_hw_interface_ros_wrapper.cpp deleted file mode 100644 index 828f0df94..000000000 --- a/nebula_ros/multi_continental_ars548_hw_interface_ros_wrapper.cpp +++ /dev/null @@ -1,356 +0,0 @@ -// 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. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include -#include - -#include -#include - -namespace nebula -{ -namespace ros -{ -MultiContinentalArs548HwInterfaceRosWrapper::MultiContinentalArs548HwInterfaceRosWrapper( - const rclcpp::NodeOptions & options) -: rclcpp::Node("multi_continental_ars548_hw_interface_ros_wrapper", options), hw_interface_() -{ - if (mtx_config_.try_lock()) { - interface_status_ = GetParameters(sensor_configuration_); - mtx_config_.unlock(); - } - if (Status::OK != interface_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); - return; - } - hw_interface_.SetLogger(std::make_shared(this->get_logger())); - std::shared_ptr - sensor_cfg_ptr = - std::make_shared( - sensor_configuration_); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - - assert(sensor_configuration_.sensor_ips.size() == sensor_configuration_.frame_ids.size()); - hw_interface_.RegisterScanCallback(std::bind( - &MultiContinentalArs548HwInterfaceRosWrapper::ReceivePacketsDataCallback, this, - std::placeholders::_1, std::placeholders::_2)); - - for (std::size_t sensor_id = 0; sensor_id < sensor_configuration_.sensor_ips.size(); - sensor_id++) { - const std::string sensor_ip = sensor_configuration_.sensor_ips[sensor_id]; - const std::string frame_id = sensor_configuration_.frame_ids[sensor_id]; - packets_pub_map_[sensor_ip] = this->create_publisher( - frame_id + "/nebula_packets", rclcpp::SensorDataQoS()); - } - - set_param_res_ = this->add_on_set_parameters_callback(std::bind( - &MultiContinentalArs548HwInterfaceRosWrapper::paramCallback, this, std::placeholders::_1)); - - StreamStart(); -} - -MultiContinentalArs548HwInterfaceRosWrapper::~MultiContinentalArs548HwInterfaceRosWrapper() -{ -} - -Status MultiContinentalArs548HwInterfaceRosWrapper::StreamStart() -{ - if (Status::OK == interface_status_) { - interface_status_ = hw_interface_.SensorInterfaceStart(); - } - - if (Status::OK == interface_status_) { - odometry_sub_ = this->create_subscription( - "odometry_input", rclcpp::QoS{1}, - std::bind( - &MultiContinentalArs548HwInterfaceRosWrapper::OdometryCallback, this, - std::placeholders::_1)); - - acceleration_sub_ = create_subscription( - "acceleration_input", rclcpp::QoS{1}, - std::bind( - &MultiContinentalArs548HwInterfaceRosWrapper::AccelerationCallback, this, - std::placeholders::_1)); - - steering_angle_sub_ = create_subscription( - "steering_angle_input", rclcpp::QoS{1}, - std::bind( - &MultiContinentalArs548HwInterfaceRosWrapper::SteeringAngleCallback, this, - std::placeholders::_1)); - } - - return interface_status_; -} - -Status MultiContinentalArs548HwInterfaceRosWrapper::StreamStop() -{ - return Status::OK; -} -Status MultiContinentalArs548HwInterfaceRosWrapper::Shutdown() -{ - return Status::OK; -} - -Status MultiContinentalArs548HwInterfaceRosWrapper::InitializeHwInterface( // todo: don't think - // this is needed - const drivers::SensorConfigurationBase & sensor_configuration) -{ - std::stringstream ss; - ss << sensor_configuration; - RCLCPP_DEBUG_STREAM(this->get_logger(), ss.str()); - return Status::OK; -} - -Status MultiContinentalArs548HwInterfaceRosWrapper::GetParameters( - drivers::continental_ars548::MultiContinentalArs548SensorConfiguration & sensor_configuration) -{ - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", descriptor); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("host_ip", descriptor); - sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter>("sensor_ips", descriptor); - sensor_configuration.sensor_ips = this->get_parameter("sensor_ips").as_string_array(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("multicast_ip", descriptor); - sensor_configuration.multicast_ip = this->get_parameter("multicast_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter>("frame_ids", descriptor); - sensor_configuration.frame_ids = this->get_parameter("frame_ids").as_string_array(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("base_frame", descriptor); - sensor_configuration.base_frame = this->get_parameter("base_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("object_frame", descriptor); - sensor_configuration.object_frame = this->get_parameter("object_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("data_port", descriptor); - sensor_configuration.data_port = this->get_parameter("data_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_host_port", descriptor); - sensor_configuration.configuration_host_port = - this->get_parameter("configuration_host_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_sensor_port", descriptor); - sensor_configuration.configuration_sensor_port = - this->get_parameter("configuration_sensor_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("use_sensor_time", descriptor); - sensor_configuration.use_sensor_time = this->get_parameter("use_sensor_time").as_bool(); - } - - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; -} - -void MultiContinentalArs548HwInterfaceRosWrapper::ReceivePacketsDataCallback( - std::unique_ptr scan_buffer, const std::string & sensor_ip) -{ - packets_pub_map_[sensor_ip]->publish(std::move(scan_buffer)); -} - -rcl_interfaces::msg::SetParametersResult MultiContinentalArs548HwInterfaceRosWrapper::paramCallback( - const std::vector & p) -{ - std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback"); - RCLCPP_DEBUG_STREAM(this->get_logger(), p); - RCLCPP_DEBUG_STREAM(this->get_logger(), sensor_configuration_); - RCLCPP_INFO_STREAM(this->get_logger(), p); - - drivers::continental_ars548::MultiContinentalArs548SensorConfiguration new_param{ - sensor_configuration_}; - RCLCPP_INFO_STREAM(this->get_logger(), new_param); - std::string sensor_model_str; - - if ( - get_param(p, "sensor_model", sensor_model_str) | get_param(p, "host_ip", new_param.host_ip) | - get_param(p, "sensor_ips", new_param.sensor_ips) | - get_param(p, "frame_ids", new_param.frame_ids) | - get_param(p, "data_port", new_param.data_port) | - get_param(p, "multicast_ip", new_param.multicast_ip) | - get_param(p, "base_frame", new_param.base_frame) | - get_param(p, "object_frame", new_param.object_frame) | - get_param(p, "configuration_host_port", new_param.configuration_host_port) | - get_param(p, "configuration_sensor_port", new_param.configuration_sensor_port) | - get_param(p, "configuration_host_port", new_param.configuration_host_port) | - get_param(p, "configuration_sensor_port", new_param.configuration_sensor_port)) { - if (0 < sensor_model_str.length()) - new_param.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_str); - - sensor_configuration_ = new_param; - RCLCPP_INFO_STREAM(this->get_logger(), "Update sensor_configuration"); - std::shared_ptr sensor_cfg_ptr = - std::make_shared( - sensor_configuration_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - hw_interface_.CheckAndSetConfig(); - } - - auto result = std::make_shared(); - result->successful = true; - result->reason = "success"; - - RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback success"); - - return *result; -} - -void MultiContinentalArs548HwInterfaceRosWrapper::OdometryCallback( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) -{ - std::scoped_lock lock(mtx_config_); - - constexpr float speed_to_standstill = 0.5f; - constexpr float speed_to_moving = 2.f; - - if (standstill_ && std::abs(msg->twist.twist.linear.x) > speed_to_moving) { - standstill_ = false; - } else if (!standstill_ && std::abs(msg->twist.twist.linear.x) < speed_to_standstill) { - standstill_ = true; - } - - if (standstill_) { - hw_interface_.SetDrivingDirection(0); - } else { - hw_interface_.SetDrivingDirection(msg->twist.twist.linear.x > 0.f ? 1 : -1); - } - - constexpr float ms_to_kmh = 3.6f; - hw_interface_.SetVelocityVehicle(ms_to_kmh * std::abs(msg->twist.twist.linear.x)); - - constexpr float rad_to_deg = 180.f / M_PI; - hw_interface_.SetYawRate(rad_to_deg * msg->twist.twist.angular.z); -} - -void MultiContinentalArs548HwInterfaceRosWrapper::AccelerationCallback( - const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg) -{ - std::scoped_lock lock(mtx_config_); - hw_interface_.SetAccelerationLateralCog(msg->accel.accel.linear.y); - hw_interface_.SetAccelerationLongitudinalCog(msg->accel.accel.linear.x); -} - -void MultiContinentalArs548HwInterfaceRosWrapper::SteeringAngleCallback( - const std_msgs::msg::Float32::SharedPtr msg) -{ - std::scoped_lock lock(mtx_config_); - constexpr float rad_to_deg = 180.f / M_PI; - hw_interface_.SetSteeringAngleFrontAxle(rad_to_deg * msg->data); -} - -std::vector -MultiContinentalArs548HwInterfaceRosWrapper::updateParameters() -{ - std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters start"); - std::ostringstream os_sensor_model; - os_sensor_model << sensor_configuration_.sensor_model; - RCLCPP_INFO_STREAM(this->get_logger(), "set_parameters"); - auto results = set_parameters( - {rclcpp::Parameter("sensor_model", os_sensor_model.str()), - rclcpp::Parameter("host_ip", sensor_configuration_.host_ip), - rclcpp::Parameter("sensor_ips", sensor_configuration_.sensor_ips), - rclcpp::Parameter("frame_ids", sensor_configuration_.frame_ids), - rclcpp::Parameter("data_port", sensor_configuration_.data_port), - rclcpp::Parameter("multicast_ip", sensor_configuration_.multicast_ip), - rclcpp::Parameter("base_frame", sensor_configuration_.base_frame), - rclcpp::Parameter("object_frame", sensor_configuration_.object_frame), - rclcpp::Parameter("configuration_host_port", sensor_configuration_.configuration_host_port), - rclcpp::Parameter( - "configuration_sensor_port", sensor_configuration_.configuration_sensor_port)}); - RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters end"); - return results; -} - -RCLCPP_COMPONENTS_REGISTER_NODE(MultiContinentalArs548HwInterfaceRosWrapper) -} // namespace ros -} // namespace nebula diff --git a/nebula_ros/multi_continental_ars548_hw_interface_ros_wrapper.hpp b/nebula_ros/multi_continental_ars548_hw_interface_ros_wrapper.hpp deleted file mode 100644 index a1fb7645c..000000000 --- a/nebula_ros/multi_continental_ars548_hw_interface_ros_wrapper.hpp +++ /dev/null @@ -1,146 +0,0 @@ -// 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. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef NEBULA_ContinentalArs548HwInterfaceRosWrapper_H -#define NEBULA_ContinentalArs548HwInterfaceRosWrapper_H - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include - -namespace nebula -{ -namespace ros -{ -/// @brief Get parameter from rclcpp::Parameter -/// @tparam T -/// @param p Parameter from rclcpp parameter callback -/// @param name Target parameter name -/// @param value Corresponding value -/// @return Whether the target name existed -template -bool get_param(const std::vector & p, const std::string & name, T & value) -{ - auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { - return parameter.get_name() == name; - }); - if (it != p.cend()) { - value = it->template get_value(); - return true; - } - return false; -} - -/// @brief Hardware interface ros wrapper of continental radar ethernet driver -/// NOTE: this is a temporary class that acts as a single hw interface for multiple devices -/// The reason behind this is a not-so-efficient multicasting and package processing when having N -/// interfaces for N devices If we end up having problems because of that we may switch to this -/// implementation. Otherwise this implementation will be removed at a later date -class MultiContinentalArs548HwInterfaceRosWrapper final : public rclcpp::Node, - NebulaHwInterfaceWrapperBase -{ - drivers::continental_ars548::MultiContinentalArs548HwInterface hw_interface_; - Status interface_status_; - - drivers::continental_ars548::MultiContinentalArs548SensorConfiguration sensor_configuration_; - - /// @brief Received Continental Radar message publisher - std::unordered_map::SharedPtr> - packets_pub_map_; - rclcpp::Subscription::SharedPtr odometry_sub_; - rclcpp::Subscription::SharedPtr acceleration_sub_; - rclcpp::Subscription::SharedPtr steering_angle_sub_; - - bool standstill_{true}; - - /// @brief Initializing hardware interface ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @return Resulting status - Status InitializeHwInterface( - const drivers::SensorConfigurationBase & sensor_configuration) override; - /// @brief Callback for receiving NebulaPackets - /// @param packets_buffer Received NebulaPackets - void ReceivePacketsDataCallback( - std::unique_ptr packets_buffer, const std::string & sensor_ip); - - /// @brief Callback to send the odometry information to the radar device - /// @param msg The odometry message - void OdometryCallback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); - - /// @brief Callback to send the acceleration information to the radar device - /// @param msg The acceleration message - void AccelerationCallback(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg); - - /// @brief Callback to send the steering angle information to the radar device - /// @param msg The steering angle message - void SteeringAngleCallback(const std_msgs::msg::Float32::SharedPtr msg); - -public: - explicit MultiContinentalArs548HwInterfaceRosWrapper(const rclcpp::NodeOptions & options); - ~MultiContinentalArs548HwInterfaceRosWrapper() noexcept override; - - /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) - /// @return Resulting status - Status StreamStart() override; - /// @brief Stop point cloud streaming (not used) - /// @return Resulting status - Status StreamStop() override; - /// @brief Shutdown (not used) - /// @return Resulting status - Status Shutdown() override; - /// @brief Get configurations from ros parameters - /// @param sensor_configuration Output of SensorConfiguration - /// @return Resulting status - Status GetParameters( - drivers::continental_ars548::MultiContinentalArs548SensorConfiguration & sensor_configuration); - -private: - std::mutex mtx_config_; - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - - /// @brief rclcpp parameter callback - /// @param parameters Received parameters - /// @return SetParametersResult - rcl_interfaces::msg::SetParametersResult paramCallback( - const std::vector & parameters); - - /// @brief Updating rclcpp parameter - /// @return SetParametersResult - std::vector updateParameters(); -}; - -} // namespace ros -} // namespace nebula - -#endif // NEBULA_ContinentalArs548HwInterfaceRosWrapper_H diff --git a/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp index fcdc78e0f..853837fdd 100644 --- a/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp @@ -20,19 +20,19 @@ namespace nebula { namespace ros { -ContinentalArs548DecoderWrapper::ContinentalArs548DecoderWrapper( +ContinentalARS548DecoderWrapper::ContinentalARS548DecoderWrapper( rclcpp::Node * const parent_node, - std::shared_ptr & + std::shared_ptr & config_ptr, bool launch_hw) : status_(nebula::Status::NOT_INITIALIZED), - logger_(parent_node->get_logger().get_child("ContinentalArs548Decoder")), + logger_(parent_node->get_logger().get_child("ContinentalARS548Decoder")), config_ptr_(config_ptr) { using std::chrono_literals::operator""us; if (!config_ptr) { throw std::runtime_error( - "ContinentalArs548DecoderWrapper cannot be instantiated without a valid config!"); + "ContinentalARS548DecoderWrapper cannot be instantiated without a valid config!"); } RCLCPP_INFO(logger_, "Starting Decoder"); @@ -88,28 +88,28 @@ ContinentalArs548DecoderWrapper::ContinentalArs548DecoderWrapper( }); } -Status ContinentalArs548DecoderWrapper::InitializeDriver( +Status ContinentalARS548DecoderWrapper::InitializeDriver( const std::shared_ptr< - const nebula::drivers::continental_ars548::ContinentalArs548SensorConfiguration> & config) + const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration> & config) { driver_ptr_.reset(); - driver_ptr_ = std::make_shared(config); + driver_ptr_ = std::make_shared(config); driver_ptr_->RegisterDetectionListCallback(std::bind( - &ContinentalArs548DecoderWrapper::DetectionListCallback, this, std::placeholders::_1)); + &ContinentalARS548DecoderWrapper::DetectionListCallback, this, std::placeholders::_1)); driver_ptr_->RegisterObjectListCallback( - std::bind(&ContinentalArs548DecoderWrapper::ObjectListCallback, this, std::placeholders::_1)); + std::bind(&ContinentalARS548DecoderWrapper::ObjectListCallback, this, std::placeholders::_1)); driver_ptr_->RegisterSensorStatusCallback( - std::bind(&ContinentalArs548DecoderWrapper::SensorStatusCallback, this, std::placeholders::_1)); + std::bind(&ContinentalARS548DecoderWrapper::SensorStatusCallback, this, std::placeholders::_1)); driver_ptr_->RegisterPacketsCallback( - std::bind(&ContinentalArs548DecoderWrapper::PacketsCallback, this, std::placeholders::_1)); + std::bind(&ContinentalARS548DecoderWrapper::PacketsCallback, this, std::placeholders::_1)); return Status::OK; } -void ContinentalArs548DecoderWrapper::OnConfigChange( +void ContinentalARS548DecoderWrapper::OnConfigChange( const std::shared_ptr< - const nebula::drivers::continental_ars548::ContinentalArs548SensorConfiguration> & + const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration> & new_config_ptr) { std::lock_guard lock(mtx_driver_ptr_); @@ -117,7 +117,7 @@ void ContinentalArs548DecoderWrapper::OnConfigChange( config_ptr_ = new_config_ptr; } -void ContinentalArs548DecoderWrapper::ProcessPacket( +void ContinentalARS548DecoderWrapper::ProcessPacket( std::unique_ptr packet_msg) { driver_ptr_->ProcessPacket(std::move(packet_msg)); @@ -125,7 +125,7 @@ void ContinentalArs548DecoderWrapper::ProcessPacket( watchdog_->update(); } -void ContinentalArs548DecoderWrapper::DetectionListCallback( +void ContinentalARS548DecoderWrapper::DetectionListCallback( std::unique_ptr msg) { if ( @@ -154,7 +154,7 @@ void ContinentalArs548DecoderWrapper::DetectionListCallback( } } -void ContinentalArs548DecoderWrapper::ObjectListCallback( +void ContinentalARS548DecoderWrapper::ObjectListCallback( std::unique_ptr msg) { if ( @@ -190,8 +190,8 @@ void ContinentalArs548DecoderWrapper::ObjectListCallback( } } -void ContinentalArs548DecoderWrapper::SensorStatusCallback( - const drivers::continental_ars548::ContinentalArs548Status & sensor_status) +void ContinentalARS548DecoderWrapper::SensorStatusCallback( + const drivers::continental_ars548::ContinentalARS548Status & sensor_status) { diagnostic_msgs::msg::DiagnosticArray diagnostic_array_msg; diagnostic_array_msg.header.stamp.sec = sensor_status.timestamp_seconds; @@ -304,7 +304,7 @@ void ContinentalArs548DecoderWrapper::SensorStatusCallback( diagnostics_pub_->publish(diagnostic_array_msg); } -void ContinentalArs548DecoderWrapper::PacketsCallback( +void ContinentalARS548DecoderWrapper::PacketsCallback( std::unique_ptr msg) { if ( @@ -314,15 +314,15 @@ void ContinentalArs548DecoderWrapper::PacketsCallback( } } -pcl::PointCloud::Ptr -ContinentalArs548DecoderWrapper::ConvertToPointcloud( +pcl::PointCloud::Ptr +ContinentalARS548DecoderWrapper::ConvertToPointcloud( const continental_msgs::msg::ContinentalArs548DetectionList & msg) { - pcl::PointCloud::Ptr output_pointcloud( - new pcl::PointCloud); + pcl::PointCloud::Ptr output_pointcloud( + new pcl::PointCloud); output_pointcloud->reserve(msg.detections.size()); - nebula::drivers::continental_ars548::PointArs548Detection point{}; + nebula::drivers::continental_ars548::PointARS548Detection point{}; for (const auto & detection : msg.detections) { point.x = std::cos(detection.elevation_angle) * std::cos(detection.azimuth_angle) * detection.range; @@ -354,15 +354,15 @@ ContinentalArs548DecoderWrapper::ConvertToPointcloud( return output_pointcloud; } -pcl::PointCloud::Ptr -ContinentalArs548DecoderWrapper::ConvertToPointcloud( +pcl::PointCloud::Ptr +ContinentalARS548DecoderWrapper::ConvertToPointcloud( const continental_msgs::msg::ContinentalArs548ObjectList & msg) { - pcl::PointCloud::Ptr output_pointcloud( - new pcl::PointCloud); + pcl::PointCloud::Ptr output_pointcloud( + new pcl::PointCloud); output_pointcloud->reserve(msg.objects.size()); - nebula::drivers::continental_ars548::PointArs548Object point{}; + nebula::drivers::continental_ars548::PointARS548Object point{}; for (const auto & object : msg.objects) { point.x = static_cast(object.position.x); point.y = static_cast(object.position.y); @@ -394,7 +394,7 @@ ContinentalArs548DecoderWrapper::ConvertToPointcloud( return output_pointcloud; } -radar_msgs::msg::RadarScan ContinentalArs548DecoderWrapper::ConvertToRadarScan( +radar_msgs::msg::RadarScan ContinentalARS548DecoderWrapper::ConvertToRadarScan( const continental_msgs::msg::ContinentalArs548DetectionList & msg) { radar_msgs::msg::RadarScan output_msg; @@ -420,7 +420,7 @@ radar_msgs::msg::RadarScan ContinentalArs548DecoderWrapper::ConvertToRadarScan( return output_msg; } -radar_msgs::msg::RadarTracks ContinentalArs548DecoderWrapper::ConvertToRadarTracks( +radar_msgs::msg::RadarTracks ContinentalARS548DecoderWrapper::ConvertToRadarTracks( const continental_msgs::msg::ContinentalArs548ObjectList & msg) { radar_msgs::msg::RadarTracks output_msg; @@ -529,7 +529,7 @@ radar_msgs::msg::RadarTracks ContinentalArs548DecoderWrapper::ConvertToRadarTrac return output_msg; } -visualization_msgs::msg::MarkerArray ContinentalArs548DecoderWrapper::ConvertToMarkers( +visualization_msgs::msg::MarkerArray ContinentalARS548DecoderWrapper::ConvertToMarkers( const continental_msgs::msg::ContinentalArs548ObjectList & msg) { visualization_msgs::msg::MarkerArray marker_array; @@ -697,7 +697,7 @@ visualization_msgs::msg::MarkerArray ContinentalArs548DecoderWrapper::ConvertToM return marker_array; } -nebula::Status ContinentalArs548DecoderWrapper::Status() +nebula::Status ContinentalARS548DecoderWrapper::Status() { std::lock_guard lock(mtx_driver_ptr_); diff --git a/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp index b98828d15..17e5b5ca8 100644 --- a/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp @@ -22,13 +22,13 @@ namespace nebula namespace ros { -ContinentalArs548HwInterfaceWrapper::ContinentalArs548HwInterfaceWrapper( +ContinentalARS548HwInterfaceWrapper::ContinentalARS548HwInterfaceWrapper( rclcpp::Node * const parent_node, - std::shared_ptr & + std::shared_ptr & config_ptr) : parent_node_(parent_node), hw_interface_( - std::make_shared()), + std::make_shared()), logger_(parent_node->get_logger().get_child("HwInterfaceWrapper")), status_(Status::NOT_INITIALIZED), config_ptr_(config_ptr) @@ -45,7 +45,7 @@ ContinentalArs548HwInterfaceWrapper::ContinentalArs548HwInterfaceWrapper( status_ = Status::OK; } -void ContinentalArs548HwInterfaceWrapper::SensorInterfaceStart() +void ContinentalARS548HwInterfaceWrapper::SensorInterfaceStart() { if (Status::OK == status_) { hw_interface_->SensorInterfaceStart(); @@ -56,70 +56,70 @@ void ContinentalArs548HwInterfaceWrapper::SensorInterfaceStart() parent_node_->create_subscription( "odometry_input", rclcpp::QoS{1}, std::bind( - &ContinentalArs548HwInterfaceWrapper::OdometryCallback, this, std::placeholders::_1)); + &ContinentalARS548HwInterfaceWrapper::OdometryCallback, this, std::placeholders::_1)); acceleration_sub_ = parent_node_->create_subscription( "acceleration_input", rclcpp::QoS{1}, std::bind( - &ContinentalArs548HwInterfaceWrapper::AccelerationCallback, this, std::placeholders::_1)); + &ContinentalARS548HwInterfaceWrapper::AccelerationCallback, this, std::placeholders::_1)); steering_angle_sub_ = parent_node_->create_subscription( "steering_angle_input", rclcpp::SensorDataQoS(), std::bind( - &ContinentalArs548HwInterfaceWrapper::SteeringAngleCallback, this, std::placeholders::_1)); + &ContinentalARS548HwInterfaceWrapper::SteeringAngleCallback, this, std::placeholders::_1)); set_network_configuration_service_server_ = parent_node_->create_service( "set_network_configuration", std::bind( - &ContinentalArs548HwInterfaceWrapper::SetNetworkConfigurationRequestCallback, this, + &ContinentalARS548HwInterfaceWrapper::SetNetworkConfigurationRequestCallback, this, std::placeholders::_1, std::placeholders::_2)); set_sensor_mounting_service_server_ = parent_node_->create_service( "set_sensor_mounting", std::bind( - &ContinentalArs548HwInterfaceWrapper::SetSensorMountingRequestCallback, this, + &ContinentalARS548HwInterfaceWrapper::SetSensorMountingRequestCallback, this, std::placeholders::_1, std::placeholders::_2)); set_vehicle_parameters_service_server_ = parent_node_->create_service( "set_vehicle_parameters", std::bind( - &ContinentalArs548HwInterfaceWrapper::SetVehicleParametersRequestCallback, this, + &ContinentalARS548HwInterfaceWrapper::SetVehicleParametersRequestCallback, this, std::placeholders::_1, std::placeholders::_2)); set_radar_parameters_service_server_ = parent_node_->create_service( "set_radar_parameters", std::bind( - &ContinentalArs548HwInterfaceWrapper::SetRadarParametersRequestCallback, this, + &ContinentalARS548HwInterfaceWrapper::SetRadarParametersRequestCallback, this, std::placeholders::_1, std::placeholders::_2)); } } -void ContinentalArs548HwInterfaceWrapper::OnConfigChange( +void ContinentalARS548HwInterfaceWrapper::OnConfigChange( const std::shared_ptr< - const nebula::drivers::continental_ars548::ContinentalArs548SensorConfiguration> & + const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration> & new_config_ptr_ptr) { hw_interface_->SetSensorConfiguration(new_config_ptr_ptr); config_ptr_ = new_config_ptr_ptr; } -Status ContinentalArs548HwInterfaceWrapper::Status() +Status ContinentalARS548HwInterfaceWrapper::Status() { return status_; } -std::shared_ptr -ContinentalArs548HwInterfaceWrapper::HwInterface() const +std::shared_ptr +ContinentalARS548HwInterfaceWrapper::HwInterface() const { return hw_interface_; } -void ContinentalArs548HwInterfaceWrapper::OdometryCallback( +void ContinentalARS548HwInterfaceWrapper::OdometryCallback( const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { constexpr float speed_to_standstill = 0.5f; @@ -144,21 +144,21 @@ void ContinentalArs548HwInterfaceWrapper::OdometryCallback( hw_interface_->SetYawRate(rad_to_deg * msg->twist.twist.angular.z); } -void ContinentalArs548HwInterfaceWrapper::AccelerationCallback( +void ContinentalARS548HwInterfaceWrapper::AccelerationCallback( const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg) { hw_interface_->SetAccelerationLateralCog(msg->accel.accel.linear.y); hw_interface_->SetAccelerationLongitudinalCog(msg->accel.accel.linear.x); } -void ContinentalArs548HwInterfaceWrapper::SteeringAngleCallback( +void ContinentalARS548HwInterfaceWrapper::SteeringAngleCallback( const std_msgs::msg::Float32::SharedPtr msg) { constexpr float rad_to_deg = 180.f / M_PI; hw_interface_->SetSteeringAngleFrontAxle(rad_to_deg * msg->data); } -void ContinentalArs548HwInterfaceWrapper::SetNetworkConfigurationRequestCallback( +void ContinentalARS548HwInterfaceWrapper::SetNetworkConfigurationRequestCallback( const std::shared_ptr request, const std::shared_ptr @@ -169,7 +169,7 @@ void ContinentalArs548HwInterfaceWrapper::SetNetworkConfigurationRequestCallback response->message = (std::stringstream() << result).str(); } -void ContinentalArs548HwInterfaceWrapper::SetSensorMountingRequestCallback( +void ContinentalARS548HwInterfaceWrapper::SetSensorMountingRequestCallback( const std::shared_ptr request, const std::shared_ptr response) @@ -222,7 +222,7 @@ void ContinentalArs548HwInterfaceWrapper::SetSensorMountingRequestCallback( response->message = (std::stringstream() << result).str(); } -void ContinentalArs548HwInterfaceWrapper::SetVehicleParametersRequestCallback( +void ContinentalARS548HwInterfaceWrapper::SetVehicleParametersRequestCallback( [[maybe_unused]] const std::shared_ptr< continental_srvs::srv::ContinentalArs548SetVehicleParameters::Request> request, @@ -269,7 +269,7 @@ void ContinentalArs548HwInterfaceWrapper::SetVehicleParametersRequestCallback( response->message = (std::stringstream() << result).str(); } -void ContinentalArs548HwInterfaceWrapper::SetRadarParametersRequestCallback( +void ContinentalARS548HwInterfaceWrapper::SetRadarParametersRequestCallback( const std::shared_ptr request, const std::shared_ptr diff --git a/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp index 07f749bc0..dd78b95d0 100644 --- a/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp @@ -18,7 +18,7 @@ namespace nebula { namespace ros { -ContinentalArs548RosWrapper::ContinentalArs548RosWrapper(const rclcpp::NodeOptions & options) +ContinentalARS548RosWrapper::ContinentalARS548RosWrapper(const rclcpp::NodeOptions & options) : rclcpp::Node( "continental_ars548_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)), wrapper_status_(Status::NOT_INITIALIZED), @@ -55,12 +55,12 @@ ContinentalArs548RosWrapper::ContinentalArs548RosWrapper(const rclcpp::NodeOptio if (launch_hw_) { hw_interface_wrapper_->HwInterface()->RegisterCallback( - std::bind(&ContinentalArs548RosWrapper::ReceivePacketCallback, this, std::placeholders::_1)); + std::bind(&ContinentalARS548RosWrapper::ReceivePacketCallback, this, std::placeholders::_1)); StreamStart(); } else { packets_sub_ = create_subscription( "nebula_packets", rclcpp::SensorDataQoS(), - std::bind(&ContinentalArs548RosWrapper::ReceivePacketsCallback, this, std::placeholders::_1)); + std::bind(&ContinentalARS548RosWrapper::ReceivePacketsCallback, this, std::placeholders::_1)); RCLCPP_INFO_STREAM( get_logger(), "Hardware connection disabled, listening for packets on " << packets_sub_->get_topic_name()); @@ -69,12 +69,12 @@ ContinentalArs548RosWrapper::ContinentalArs548RosWrapper(const rclcpp::NodeOptio // Register parameter callback after all params have been declared. Otherwise it would be called // once for each declaration parameter_event_cb_ = add_on_set_parameters_callback( - std::bind(&ContinentalArs548RosWrapper::OnParameterChange, this, std::placeholders::_1)); + std::bind(&ContinentalARS548RosWrapper::OnParameterChange, this, std::placeholders::_1)); } -nebula::Status ContinentalArs548RosWrapper::DeclareAndGetSensorConfigParams() +nebula::Status ContinentalARS548RosWrapper::DeclareAndGetSensorConfigParams() { - nebula::drivers::continental_ars548::ContinentalArs548SensorConfiguration config; + nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration config; { rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); @@ -222,12 +222,12 @@ nebula::Status ContinentalArs548RosWrapper::DeclareAndGetSensorConfigParams() } auto new_config_ptr = std::make_shared< - const nebula::drivers::continental_ars548::ContinentalArs548SensorConfiguration>(config); + const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration>(config); return ValidateAndSetConfig(new_config_ptr); } -Status ContinentalArs548RosWrapper::ValidateAndSetConfig( - std::shared_ptr & +Status ContinentalARS548RosWrapper::ValidateAndSetConfig( + std::shared_ptr & new_config_ptr) { if (new_config_ptr->sensor_model == nebula::drivers::SensorModel::UNKNOWN) { @@ -249,7 +249,7 @@ Status ContinentalArs548RosWrapper::ValidateAndSetConfig( return Status::OK; } -void ContinentalArs548RosWrapper::ReceivePacketsCallback( +void ContinentalARS548RosWrapper::ReceivePacketsCallback( std::unique_ptr packets_msg_ptr) { if (hw_interface_wrapper_) { @@ -269,7 +269,7 @@ void ContinentalArs548RosWrapper::ReceivePacketsCallback( } } -void ContinentalArs548RosWrapper::ReceivePacketCallback( +void ContinentalARS548RosWrapper::ReceivePacketCallback( std::unique_ptr msg_ptr) { if (!decoder_wrapper_ || decoder_wrapper_->Status() != Status::OK) { @@ -281,12 +281,12 @@ void ContinentalArs548RosWrapper::ReceivePacketCallback( } } -Status ContinentalArs548RosWrapper::GetStatus() +Status ContinentalARS548RosWrapper::GetStatus() { return wrapper_status_; } -Status ContinentalArs548RosWrapper::StreamStart() +Status ContinentalARS548RosWrapper::StreamStart() { if (!hw_interface_wrapper_) { return Status::UDP_CONNECTION_ERROR; @@ -301,7 +301,7 @@ Status ContinentalArs548RosWrapper::StreamStart() return hw_interface_wrapper_->Status(); } -rcl_interfaces::msg::SetParametersResult ContinentalArs548RosWrapper::OnParameterChange( +rcl_interfaces::msg::SetParametersResult ContinentalARS548RosWrapper::OnParameterChange( const std::vector & p) { using rcl_interfaces::msg::SetParametersResult; @@ -314,7 +314,7 @@ rcl_interfaces::msg::SetParametersResult ContinentalArs548RosWrapper::OnParamete RCLCPP_INFO(get_logger(), "OnParameterChange"); - drivers::continental_ars548::ContinentalArs548SensorConfiguration new_config(*config_ptr_); + drivers::continental_ars548::ContinentalARS548SensorConfiguration new_config(*config_ptr_); bool got_any = get_param(p, "frame_id", new_config.frame_id) | @@ -332,7 +332,7 @@ rcl_interfaces::msg::SetParametersResult ContinentalArs548RosWrapper::OnParamete } auto new_config_ptr = std::make_shared< - const nebula::drivers::continental_ars548::ContinentalArs548SensorConfiguration>(new_config); + const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration>(new_config); auto status = ValidateAndSetConfig(new_config_ptr); if (status != Status::OK) { @@ -346,6 +346,6 @@ rcl_interfaces::msg::SetParametersResult ContinentalArs548RosWrapper::OnParamete return rcl_interfaces::build().successful(true).reason(""); } -RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalArs548RosWrapper) +RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalARS548RosWrapper) } // namespace ros } // namespace nebula diff --git a/nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp index bb556d445..f74364925 100644 --- a/nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp +++ b/nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp @@ -20,21 +20,21 @@ namespace nebula { namespace ros { -ContinentalSrr520DecoderWrapper::ContinentalSrr520DecoderWrapper( +ContinentalSRR520DecoderWrapper::ContinentalSRR520DecoderWrapper( rclcpp::Node * const parent_node, - std::shared_ptr & + std::shared_ptr & config, - std::shared_ptr hw_interface_ptr) + std::shared_ptr hw_interface_ptr) : parent_node_(parent_node), status_(nebula::Status::NOT_INITIALIZED), - logger_(parent_node->get_logger().get_child("ContinentalSrr520Decoder")), + logger_(parent_node->get_logger().get_child("ContinentalSRR520Decoder")), sensor_cfg_(config), hw_interface_ptr_(hw_interface_ptr) { using std::chrono_literals::operator""us; if (!config) { throw std::runtime_error( - "ContinentalSrr520DecoderWrapper cannot be instantiated without a valid config!"); + "ContinentalSRR520DecoderWrapper cannot be instantiated without a valid config!"); } RCLCPP_INFO(logger_, "Starting Decoder"); @@ -97,27 +97,27 @@ ContinentalSrr520DecoderWrapper::ContinentalSrr520DecoderWrapper( }); } -Status ContinentalSrr520DecoderWrapper::InitializeDriver( +Status ContinentalSRR520DecoderWrapper::InitializeDriver( const std::shared_ptr< - const nebula::drivers::continental_srr520::ContinentalSrr520SensorConfiguration> & config) + const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration> & config) { driver_ptr_.reset(); - driver_ptr_ = std::make_shared(config); + driver_ptr_ = std::make_shared(config); driver_ptr_->RegisterNearDetectionListCallback(std::bind( - &ContinentalSrr520DecoderWrapper::NearDetectionListCallback, this, std::placeholders::_1)); + &ContinentalSRR520DecoderWrapper::NearDetectionListCallback, this, std::placeholders::_1)); driver_ptr_->RegisterHRRDetectionListCallback(std::bind( - &ContinentalSrr520DecoderWrapper::HRRDetectionListCallback, this, std::placeholders::_1)); + &ContinentalSRR520DecoderWrapper::HRRDetectionListCallback, this, std::placeholders::_1)); driver_ptr_->RegisterObjectListCallback( - std::bind(&ContinentalSrr520DecoderWrapper::ObjectListCallback, this, std::placeholders::_1)); + std::bind(&ContinentalSRR520DecoderWrapper::ObjectListCallback, this, std::placeholders::_1)); driver_ptr_->RegisterStatusCallback( - std::bind(&ContinentalSrr520DecoderWrapper::StatusCallback, this, std::placeholders::_1)); + std::bind(&ContinentalSRR520DecoderWrapper::StatusCallback, this, std::placeholders::_1)); if (hw_interface_ptr_) { driver_ptr_->RegisterSyncFupCallback( - std::bind(&ContinentalSrr520DecoderWrapper::SyncFupCallback, this, std::placeholders::_1)); + std::bind(&ContinentalSRR520DecoderWrapper::SyncFupCallback, this, std::placeholders::_1)); driver_ptr_->RegisterPacketsCallback( - std::bind(&ContinentalSrr520DecoderWrapper::PacketsCallback, this, std::placeholders::_1)); + std::bind(&ContinentalSRR520DecoderWrapper::PacketsCallback, this, std::placeholders::_1)); } driver_ptr_->SetLogger( @@ -126,16 +126,16 @@ Status ContinentalSrr520DecoderWrapper::InitializeDriver( return Status::OK; } -void ContinentalSrr520DecoderWrapper::OnConfigChange( +void ContinentalSRR520DecoderWrapper::OnConfigChange( const std::shared_ptr< - const nebula::drivers::continental_srr520::ContinentalSrr520SensorConfiguration> & new_config) + const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration> & new_config) { std::lock_guard lock(mtx_driver_ptr_); InitializeDriver(new_config); sensor_cfg_ = new_config; } -void ContinentalSrr520DecoderWrapper::ProcessPacket( +void ContinentalSRR520DecoderWrapper::ProcessPacket( std::unique_ptr packet_msg) { driver_ptr_->ProcessPacket(std::move(packet_msg)); @@ -143,7 +143,7 @@ void ContinentalSrr520DecoderWrapper::ProcessPacket( watchdog_->update(); } -void ContinentalSrr520DecoderWrapper::NearDetectionListCallback( +void ContinentalSRR520DecoderWrapper::NearDetectionListCallback( std::unique_ptr msg) { if ( @@ -172,7 +172,7 @@ void ContinentalSrr520DecoderWrapper::NearDetectionListCallback( } } -void ContinentalSrr520DecoderWrapper::HRRDetectionListCallback( +void ContinentalSRR520DecoderWrapper::HRRDetectionListCallback( std::unique_ptr msg) { if ( @@ -201,7 +201,7 @@ void ContinentalSrr520DecoderWrapper::HRRDetectionListCallback( } } -void ContinentalSrr520DecoderWrapper::ObjectListCallback( +void ContinentalSRR520DecoderWrapper::ObjectListCallback( std::unique_ptr msg) { if ( @@ -237,21 +237,21 @@ void ContinentalSrr520DecoderWrapper::ObjectListCallback( } } -void ContinentalSrr520DecoderWrapper::StatusCallback( +void ContinentalSRR520DecoderWrapper::StatusCallback( std::unique_ptr status_msg_ptr) { status_pub_->publish(std::move(status_msg_ptr)); } -pcl::PointCloud::Ptr -ContinentalSrr520DecoderWrapper::ConvertToPointcloud( +pcl::PointCloud::Ptr +ContinentalSRR520DecoderWrapper::ConvertToPointcloud( const continental_msgs::msg::ContinentalSrr520DetectionList & msg) { - pcl::PointCloud::Ptr output_pointcloud( - new pcl::PointCloud); + pcl::PointCloud::Ptr output_pointcloud( + new pcl::PointCloud); output_pointcloud->reserve(msg.detections.size()); - nebula::drivers::continental_srr520::PointSrr520Detection point{}; + nebula::drivers::continental_srr520::PointSRR520Detection point{}; for (const auto & detection : msg.detections) { point.x = std::cos(detection.azimuth_angle) * detection.range; point.y = std::sin(detection.azimuth_angle) * detection.range; @@ -277,15 +277,15 @@ ContinentalSrr520DecoderWrapper::ConvertToPointcloud( return output_pointcloud; } -pcl::PointCloud::Ptr -ContinentalSrr520DecoderWrapper::ConvertToPointcloud( +pcl::PointCloud::Ptr +ContinentalSRR520DecoderWrapper::ConvertToPointcloud( const continental_msgs::msg::ContinentalSrr520ObjectList & msg) { - pcl::PointCloud::Ptr output_pointcloud( - new pcl::PointCloud); + pcl::PointCloud::Ptr output_pointcloud( + new pcl::PointCloud); output_pointcloud->reserve(msg.objects.size()); - nebula::drivers::continental_srr520::PointSrr520Object point{}; + nebula::drivers::continental_srr520::PointSRR520Object point{}; for (const auto & object : msg.objects) { point.x = object.dist_x; point.y = object.dist_y; @@ -314,7 +314,7 @@ ContinentalSrr520DecoderWrapper::ConvertToPointcloud( return output_pointcloud; } -radar_msgs::msg::RadarScan ContinentalSrr520DecoderWrapper::ConvertToRadarScan( +radar_msgs::msg::RadarScan ContinentalSRR520DecoderWrapper::ConvertToRadarScan( const continental_msgs::msg::ContinentalSrr520DetectionList & msg) { radar_msgs::msg::RadarScan output_msg; @@ -338,7 +338,7 @@ radar_msgs::msg::RadarScan ContinentalSrr520DecoderWrapper::ConvertToRadarScan( return output_msg; } -radar_msgs::msg::RadarTracks ContinentalSrr520DecoderWrapper::ConvertToRadarTracks( +radar_msgs::msg::RadarTracks ContinentalSRR520DecoderWrapper::ConvertToRadarTracks( const continental_msgs::msg::ContinentalSrr520ObjectList & msg) { radar_msgs::msg::RadarTracks output_msg; @@ -409,7 +409,7 @@ radar_msgs::msg::RadarTracks ContinentalSrr520DecoderWrapper::ConvertToRadarTrac return output_msg; } -visualization_msgs::msg::MarkerArray ContinentalSrr520DecoderWrapper::ConvertToMarkers( +visualization_msgs::msg::MarkerArray ContinentalSRR520DecoderWrapper::ConvertToMarkers( const continental_msgs::msg::ContinentalSrr520ObjectList & msg) { visualization_msgs::msg::MarkerArray marker_array; @@ -574,12 +574,12 @@ visualization_msgs::msg::MarkerArray ContinentalSrr520DecoderWrapper::ConvertToM return marker_array; } -void ContinentalSrr520DecoderWrapper::SyncFupCallback(builtin_interfaces::msg::Time stamp) +void ContinentalSRR520DecoderWrapper::SyncFupCallback(builtin_interfaces::msg::Time stamp) { hw_interface_ptr_->SensorSyncFup(stamp); } -void ContinentalSrr520DecoderWrapper::PacketsCallback( +void ContinentalSRR520DecoderWrapper::PacketsCallback( std::unique_ptr msg) { if ( @@ -589,7 +589,7 @@ void ContinentalSrr520DecoderWrapper::PacketsCallback( } } -nebula::Status ContinentalSrr520DecoderWrapper::Status() +nebula::Status ContinentalSRR520DecoderWrapper::Status() { std::lock_guard lock(mtx_driver_ptr_); diff --git a/nebula_ros/src/continental/continental_srr520_hw_interface_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_hw_interface_wrapper.cpp index 000bdf816..f76c94a06 100644 --- a/nebula_ros/src/continental/continental_srr520_hw_interface_wrapper.cpp +++ b/nebula_ros/src/continental/continental_srr520_hw_interface_wrapper.cpp @@ -22,13 +22,13 @@ namespace nebula namespace ros { -ContinentalSrr520HwInterfaceWrapper::ContinentalSrr520HwInterfaceWrapper( +ContinentalSRR520HwInterfaceWrapper::ContinentalSRR520HwInterfaceWrapper( rclcpp::Node * const parent_node, - std::shared_ptr & + std::shared_ptr & config_ptr) : parent_node_(parent_node), hw_interface_( - std::make_shared()), + std::make_shared()), logger_(parent_node->get_logger().get_child("HwInterfaceWrapper")), status_(Status::NOT_INITIALIZED), config_ptr_(config_ptr) @@ -45,7 +45,7 @@ ContinentalSrr520HwInterfaceWrapper::ContinentalSrr520HwInterfaceWrapper( status_ = Status::OK; } -void ContinentalSrr520HwInterfaceWrapper::SensorInterfaceStart() +void ContinentalSRR520HwInterfaceWrapper::SensorInterfaceStart() { using std::chrono_literals::operator""ms; @@ -59,41 +59,41 @@ void ContinentalSrr520HwInterfaceWrapper::SensorInterfaceStart() sync_ptr_ = std::make_shared(ExactTimeSyncPolicy(10), odometry_sub_, acceleration_sub_); - sync_ptr_->registerCallback(&ContinentalSrr520HwInterfaceWrapper::dynamicsCallback, this); + sync_ptr_->registerCallback(&ContinentalSRR520HwInterfaceWrapper::dynamicsCallback, this); configure_sensor_service_server_ = parent_node_->create_service( "configure_sensor", std::bind( - &ContinentalSrr520HwInterfaceWrapper::ConfigureSensorRequestCallback, + &ContinentalSRR520HwInterfaceWrapper::ConfigureSensorRequestCallback, this, std::placeholders::_1, std::placeholders::_2)); sync_timer_ = rclcpp::create_timer( parent_node_, parent_node_->get_clock(), 100ms, - std::bind(&ContinentalSrr520HwInterfaceWrapper::syncTimerCallback, this)); + std::bind(&ContinentalSRR520HwInterfaceWrapper::syncTimerCallback, this)); } } -void ContinentalSrr520HwInterfaceWrapper::OnConfigChange( +void ContinentalSRR520HwInterfaceWrapper::OnConfigChange( const std::shared_ptr< - const nebula::drivers::continental_srr520::ContinentalSrr520SensorConfiguration> & + const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration> & new_config_ptr) { hw_interface_->SetSensorConfiguration(new_config_ptr); config_ptr_ = new_config_ptr; } -Status ContinentalSrr520HwInterfaceWrapper::Status() +Status ContinentalSRR520HwInterfaceWrapper::Status() { return status_; } -std::shared_ptr -ContinentalSrr520HwInterfaceWrapper::HwInterface() const +std::shared_ptr +ContinentalSRR520HwInterfaceWrapper::HwInterface() const { return hw_interface_; } -void ContinentalSrr520HwInterfaceWrapper::dynamicsCallback( +void ContinentalSRR520HwInterfaceWrapper::dynamicsCallback( const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr & odometry_msg, const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr & acceleration_msg) { @@ -111,7 +111,7 @@ void ContinentalSrr520HwInterfaceWrapper::dynamicsCallback( odometry_msg->twist.twist.angular.z, odometry_msg->twist.twist.linear.x, standstill_); } -void ContinentalSrr520HwInterfaceWrapper::ConfigureSensorRequestCallback( +void ContinentalSRR520HwInterfaceWrapper::ConfigureSensorRequestCallback( const std::shared_ptr request, const std::shared_ptr @@ -174,7 +174,7 @@ void ContinentalSrr520HwInterfaceWrapper::ConfigureSensorRequestCallback( response->message = (std::stringstream() << result).str(); } -void ContinentalSrr520HwInterfaceWrapper::syncTimerCallback() +void ContinentalSRR520HwInterfaceWrapper::syncTimerCallback() { hw_interface_->SensorSync(); } diff --git a/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp index aa002a8c3..f0789d3e2 100644 --- a/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp @@ -18,7 +18,7 @@ namespace nebula { namespace ros { -ContinentalSrr520RosWrapper::ContinentalSrr520RosWrapper(const rclcpp::NodeOptions & options) +ContinentalSRR520RosWrapper::ContinentalSRR520RosWrapper(const rclcpp::NodeOptions & options) : rclcpp::Node( "continental_srr520_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)), wrapper_status_(Status::NOT_INITIALIZED), @@ -56,12 +56,12 @@ ContinentalSrr520RosWrapper::ContinentalSrr520RosWrapper(const rclcpp::NodeOptio if (launch_hw_) { hw_interface_wrapper_->HwInterface()->RegisterPacketCallback( - std::bind(&ContinentalSrr520RosWrapper::ReceivePacketCallback, this, std::placeholders::_1)); + std::bind(&ContinentalSRR520RosWrapper::ReceivePacketCallback, this, std::placeholders::_1)); StreamStart(); } else { packets_sub_ = create_subscription( "nebula_packets", rclcpp::SensorDataQoS(), - std::bind(&ContinentalSrr520RosWrapper::ReceivePacketsCallback, this, std::placeholders::_1)); + std::bind(&ContinentalSRR520RosWrapper::ReceivePacketsCallback, this, std::placeholders::_1)); RCLCPP_INFO_STREAM( get_logger(), "Hardware connection disabled, listening for packets on " << packets_sub_->get_topic_name()); @@ -70,12 +70,12 @@ ContinentalSrr520RosWrapper::ContinentalSrr520RosWrapper(const rclcpp::NodeOptio // Register parameter callback after all params have been declared. Otherwise it would be called // once for each declaration parameter_event_cb_ = add_on_set_parameters_callback( - std::bind(&ContinentalSrr520RosWrapper::OnParameterChange, this, std::placeholders::_1)); + std::bind(&ContinentalSRR520RosWrapper::OnParameterChange, this, std::placeholders::_1)); } -nebula::Status ContinentalSrr520RosWrapper::DeclareAndGetSensorConfigParams() +nebula::Status ContinentalSRR520RosWrapper::DeclareAndGetSensorConfigParams() { - nebula::drivers::continental_srr520::ContinentalSrr520SensorConfiguration config; + nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration config; { rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); @@ -166,12 +166,12 @@ nebula::Status ContinentalSrr520RosWrapper::DeclareAndGetSensorConfigParams() } auto new_config_ptr = std::make_shared< - const nebula::drivers::continental_srr520::ContinentalSrr520SensorConfiguration>(config); + const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration>(config); return ValidateAndSetConfig(new_config_ptr); } -Status ContinentalSrr520RosWrapper::ValidateAndSetConfig( - std::shared_ptr & +Status ContinentalSRR520RosWrapper::ValidateAndSetConfig( + std::shared_ptr & new_config) { if (new_config->sensor_model == nebula::drivers::SensorModel::UNKNOWN) { @@ -193,7 +193,7 @@ Status ContinentalSrr520RosWrapper::ValidateAndSetConfig( return Status::OK; } -void ContinentalSrr520RosWrapper::ReceivePacketsCallback( +void ContinentalSRR520RosWrapper::ReceivePacketsCallback( std::unique_ptr packets_msg) { if (hw_interface_wrapper_) { @@ -213,7 +213,7 @@ void ContinentalSrr520RosWrapper::ReceivePacketsCallback( } } -void ContinentalSrr520RosWrapper::ReceivePacketCallback( +void ContinentalSRR520RosWrapper::ReceivePacketCallback( std::unique_ptr msg_ptr) { if (!decoder_wrapper_ || decoder_wrapper_->Status() != Status::OK) { @@ -225,12 +225,12 @@ void ContinentalSrr520RosWrapper::ReceivePacketCallback( } } -Status ContinentalSrr520RosWrapper::GetStatus() +Status ContinentalSRR520RosWrapper::GetStatus() { return wrapper_status_; } -Status ContinentalSrr520RosWrapper::StreamStart() +Status ContinentalSRR520RosWrapper::StreamStart() { if (!hw_interface_wrapper_) { return Status::UDP_CONNECTION_ERROR; @@ -245,7 +245,7 @@ Status ContinentalSrr520RosWrapper::StreamStart() return hw_interface_wrapper_->Status(); } -rcl_interfaces::msg::SetParametersResult ContinentalSrr520RosWrapper::OnParameterChange( +rcl_interfaces::msg::SetParametersResult ContinentalSRR520RosWrapper::OnParameterChange( const std::vector & p) { using rcl_interfaces::msg::SetParametersResult; @@ -256,7 +256,7 @@ rcl_interfaces::msg::SetParametersResult ContinentalSrr520RosWrapper::OnParamete RCLCPP_INFO(get_logger(), "OnParameterChange"); - drivers::continental_srr520::ContinentalSrr520SensorConfiguration new_config(*config_ptr_); + drivers::continental_srr520::ContinentalSRR520SensorConfiguration new_config(*config_ptr_); bool got_any = get_param(p, "frame_id", new_config.frame_id) | @@ -269,7 +269,7 @@ rcl_interfaces::msg::SetParametersResult ContinentalSrr520RosWrapper::OnParamete } auto new_config_ptr = std::make_shared< - const nebula::drivers::continental_srr520::ContinentalSrr520SensorConfiguration>(new_config); + const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration>(new_config); auto status = ValidateAndSetConfig(new_config_ptr); if (status != Status::OK) { @@ -283,6 +283,6 @@ rcl_interfaces::msg::SetParametersResult ContinentalSrr520RosWrapper::OnParamete return rcl_interfaces::build().successful(true).reason(""); } -RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalSrr520RosWrapper) +RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalSRR520RosWrapper) } // namespace ros } // namespace nebula diff --git a/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp b/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp index 25a9c690c..1bd913f62 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp +++ b/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp @@ -40,7 +40,7 @@ ContinentalRosDecoderTest::ContinentalRosDecoderTest( const rclcpp::NodeOptions & options, const std::string & node_name) : rclcpp::Node(node_name, options) { - drivers::continental_ars548::ContinentalArs548SensorConfiguration sensor_configuration; + drivers::continental_ars548::ContinentalARS548SensorConfiguration sensor_configuration; wrapper_status_ = GetParameters(sensor_configuration); if (Status::OK != wrapper_status_) { @@ -50,12 +50,12 @@ ContinentalRosDecoderTest::ContinentalRosDecoderTest( RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); sensor_cfg_ptr_ = - std::make_shared( + std::make_shared( sensor_configuration); RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); wrapper_status_ = InitializeDriver( - std::const_pointer_cast( + std::const_pointer_cast( sensor_cfg_ptr_)); driver_ptr_->RegisterDetectionListCallback( @@ -67,12 +67,12 @@ ContinentalRosDecoderTest::ContinentalRosDecoderTest( } Status ContinentalRosDecoderTest::InitializeDriver( - std::shared_ptr + std::shared_ptr sensor_configuration) { // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared( - std::static_pointer_cast( + driver_ptr_ = std::make_shared( + std::static_pointer_cast( sensor_configuration)); return Status::OK; } @@ -83,7 +83,7 @@ Status ContinentalRosDecoderTest::GetStatus() } Status ContinentalRosDecoderTest::GetParameters( - drivers::continental_ars548::ContinentalArs548SensorConfiguration & sensor_configuration) + drivers::continental_ars548::ContinentalARS548SensorConfiguration & sensor_configuration) { std::filesystem::path bag_root_dir = _SRC_RESOURCES_DIR_PATH; // variable defined in CMakeLists.txt; diff --git a/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp b/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp index 8b7aa8c0d..f36cac677 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp +++ b/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp @@ -41,18 +41,18 @@ namespace ros class ContinentalRosDecoderTest final : public rclcpp::Node, NebulaDriverRosWrapperBase //, testing::Test { - std::shared_ptr driver_ptr_; + std::shared_ptr driver_ptr_; Status wrapper_status_; - std::shared_ptr + std::shared_ptr sensor_cfg_ptr_; Status InitializeDriver( - std::shared_ptr + std::shared_ptr sensor_configuration); Status GetParameters( - drivers::continental_ars548::ContinentalArs548SensorConfiguration & sensor_configuration); + drivers::continental_ars548::ContinentalARS548SensorConfiguration & sensor_configuration); void CheckResult(const std::string msg_as_string, const std::string & gt_path); diff --git a/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp b/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp index 3a20efb7a..c75f2e053 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp +++ b/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp @@ -40,7 +40,7 @@ ContinentalRosDecoderTest::ContinentalRosDecoderTest( const rclcpp::NodeOptions & options, const std::string & node_name) : rclcpp::Node(node_name, options) { - drivers::continental_srr520::ContinentalSrr520SensorConfiguration sensor_configuration; + drivers::continental_srr520::ContinentalSRR520SensorConfiguration sensor_configuration; wrapper_status_ = GetParameters(sensor_configuration); if (Status::OK != wrapper_status_) { @@ -50,12 +50,12 @@ ContinentalRosDecoderTest::ContinentalRosDecoderTest( RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); sensor_cfg_ptr_ = - std::make_shared( + std::make_shared( sensor_configuration); RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); wrapper_status_ = InitializeDriver( - std::const_pointer_cast( + std::const_pointer_cast( sensor_cfg_ptr_)); driver_ptr_->RegisterHRRDetectionListCallback( @@ -71,12 +71,12 @@ ContinentalRosDecoderTest::ContinentalRosDecoderTest( } Status ContinentalRosDecoderTest::InitializeDriver( - std::shared_ptr + std::shared_ptr sensor_configuration) { // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared( - std::static_pointer_cast( + driver_ptr_ = std::make_shared( + std::static_pointer_cast( sensor_configuration)); return Status::OK; } @@ -87,7 +87,7 @@ Status ContinentalRosDecoderTest::GetStatus() } Status ContinentalRosDecoderTest::GetParameters( - drivers::continental_srr520::ContinentalSrr520SensorConfiguration & sensor_configuration) + drivers::continental_srr520::ContinentalSRR520SensorConfiguration & sensor_configuration) { std::filesystem::path bag_root_dir = _SRC_RESOURCES_DIR_PATH; // variable defined in CMakeLists.txt; diff --git a/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp b/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp index e5d4299ab..463eda30d 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp +++ b/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp @@ -41,18 +41,18 @@ namespace ros class ContinentalRosDecoderTest final : public rclcpp::Node, NebulaDriverRosWrapperBase //, testing::Test { - std::shared_ptr driver_ptr_; + std::shared_ptr driver_ptr_; Status wrapper_status_; - std::shared_ptr + std::shared_ptr sensor_cfg_ptr_; Status InitializeDriver( - std::shared_ptr + std::shared_ptr sensor_configuration); Status GetParameters( - drivers::continental_srr520::ContinentalSrr520SensorConfiguration & sensor_configuration); + drivers::continental_srr520::ContinentalSRR520SensorConfiguration & sensor_configuration); void CheckResult(const std::string msg_as_string, const std::string & gt_path); From bfc14838821fb708b77a5469e88c94ecdf3cff9d Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Fri, 31 May 2024 14:53:32 +0900 Subject: [PATCH 10/47] fix: fixed srr520 changes after the latest rebased and confirmed basic driver functionalities with a live sensor Signed-off-by: Kenzo Lobos-Tsunekawa --- .../decoders/continental_srr520_decoder.cpp | 106 ++++++++++-------- .../launch/continental_launch_all_hw.xml | 58 +++++----- 2 files changed, 87 insertions(+), 77 deletions(-) diff --git a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp index e66226933..356f93398 100644 --- a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp @@ -34,6 +34,17 @@ ContinentalSRR520Decoder::ContinentalSRR520Decoder( sensor_configuration) { sensor_configuration_ = sensor_configuration; + + rdi_near_packets_ptr_ = std::make_unique(); + near_detection_list_ptr_ = + std::make_unique(); + + rdi_hrr_packets_ptr_ = std::make_unique(); + hrr_detection_list_ptr_ = + std::make_unique(); + + object_packets_ptr_ = std::make_unique(); + object_list_ptr_ = std::make_unique(); } Status ContinentalSRR520Decoder::GetStatus() @@ -168,9 +179,6 @@ bool ContinentalSRR520Decoder::ProcessPacket( void ContinentalSRR520Decoder::ProcessNearHeaderPacket( std::unique_ptr packet_msg) { - rdi_near_packets_ptr_ = std::make_unique(); - near_detection_list_ptr_ = - std::make_unique(); first_rdi_near_packet_ = false; static_assert(sizeof(ScanHeaderPacket) == RDI_NEAR_HEADER_PACKET_SIZE); @@ -230,6 +238,7 @@ void ContinentalSRR520Decoder::ProcessNearElementPacket( if ( near_detection_list_ptr_->detections.size() >= rdi_near_header_packet_.u_number_of_detections.value()) { + rdi_near_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); return; } @@ -240,7 +249,9 @@ void ContinentalSRR520Decoder::ProcessNearElementPacket( static_assert(sizeof(DetectionPacket) == RDI_NEAR_ELEMENT_PACKET_SIZE); assert(packet_msg->data.size() == RDI_NEAR_ELEMENT_PACKET_SIZE + 4); assert(rdi_near_header_packet_.u_sequence_counter == detection_packet.u_sequence_counter); - assert(rdi_near_packets_ptr_->packets.size() == detection_packet.u_message_counter + 1); + assert( + rdi_near_packets_ptr_->packets.size() == + static_cast(detection_packet.u_message_counter + 1)); for (const auto & fragment : detection_packet.fragments) { continental_msgs::msg::ContinentalSrr520Detection detection_msg; @@ -288,13 +299,10 @@ void ContinentalSRR520Decoder::ProcessNearElementPacket( void ContinentalSRR520Decoder::ProcessHRRHeaderPacket( std::unique_ptr packet_msg) { - rdi_hrr_packets_ptr_ = std::make_unique(); - hrr_detection_list_ptr_ = - std::make_unique(); first_rdi_hrr_packet_ = false; static_assert(sizeof(ScanHeaderPacket) == RDI_HRR_HEADER_PACKET_SIZE); - assert(packet_msg->data.size() == RDI_NEAR_HEADER_PACKET_SIZE + 4); + assert(packet_msg->data.size() == RDI_HRR_HEADER_PACKET_SIZE + 4); std::memcpy( &rdi_hrr_header_packet_, packet_msg->data.data() + 4 * sizeof(uint8_t), @@ -318,20 +326,20 @@ void ContinentalSRR520Decoder::ProcessHRRHeaderPacket( rdi_hrr_packets_ptr_->header.stamp = packet_msg->stamp; rdi_hrr_packets_ptr_->header.frame_id = sensor_configuration_->frame_id; - near_detection_list_ptr_->internal_time_stamp_usec = rdi_near_header_packet_.u_time_stamp.value(); - near_detection_list_ptr_->global_time_stamp_sync_status = - rdi_near_header_packet_.u_global_time_stamp_sync_status; - near_detection_list_ptr_->signal_status = rdi_near_header_packet_.u_signal_status; - near_detection_list_ptr_->sequence_counter = rdi_near_header_packet_.u_sequence_counter; - near_detection_list_ptr_->cycle_counter = rdi_near_header_packet_.u_cycle_counter.value(); - near_detection_list_ptr_->vambig = - 0.003051851f * rdi_near_header_packet_.u_vambig.value() - 100.f; // cSpell:ignore vambig - near_detection_list_ptr_->max_range = 0.1f * rdi_near_header_packet_.u_max_range.value(); + hrr_detection_list_ptr_->internal_time_stamp_usec = rdi_hrr_header_packet_.u_time_stamp.value(); + hrr_detection_list_ptr_->global_time_stamp_sync_status = + rdi_hrr_header_packet_.u_global_time_stamp_sync_status; + hrr_detection_list_ptr_->signal_status = rdi_hrr_header_packet_.u_signal_status; + hrr_detection_list_ptr_->sequence_counter = rdi_hrr_header_packet_.u_sequence_counter; + hrr_detection_list_ptr_->cycle_counter = rdi_hrr_header_packet_.u_cycle_counter.value(); + hrr_detection_list_ptr_->vambig = + 0.003051851f * rdi_hrr_header_packet_.u_vambig.value() - 100.f; // cSpell:ignore vambig + hrr_detection_list_ptr_->max_range = 0.1f * rdi_hrr_header_packet_.u_max_range.value(); - near_detection_list_ptr_->detections.reserve( - rdi_near_header_packet_.u_number_of_detections.value()); + hrr_detection_list_ptr_->detections.reserve( + rdi_hrr_header_packet_.u_number_of_detections.value()); - rdi_near_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); + rdi_hrr_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); } void ContinentalSRR520Decoder::ProcessHRRElementPacket( @@ -349,6 +357,7 @@ void ContinentalSRR520Decoder::ProcessHRRElementPacket( if ( hrr_detection_list_ptr_->detections.size() >= rdi_hrr_header_packet_.u_number_of_detections.value()) { + rdi_hrr_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); return; } @@ -359,7 +368,9 @@ void ContinentalSRR520Decoder::ProcessHRRElementPacket( static_assert(sizeof(DetectionPacket) == RDI_HRR_ELEMENT_PACKET_SIZE); assert(packet_msg->data.size() == RDI_HRR_ELEMENT_PACKET_SIZE + 4); assert(rdi_hrr_header_packet_.u_sequence_counter == detection_packet.u_sequence_counter); - assert(rdi_hrr_packets_ptr_->packets.size() == detection_packet.u_message_counter + 1); + assert( + rdi_hrr_packets_ptr_->packets.size() == + static_cast(detection_packet.u_message_counter + 1)); for (const auto & fragment : detection_packet.fragments) { continental_msgs::msg::ContinentalSrr520Detection detection_msg; @@ -407,8 +418,6 @@ void ContinentalSRR520Decoder::ProcessHRRElementPacket( void ContinentalSRR520Decoder::ProcessObjectHeaderPacket( std::unique_ptr packet_msg) { - object_packets_ptr_ = std::make_unique(); - object_list_ptr_ = std::make_unique(); first_object_packet_ = false; static_assert(sizeof(ObjectHeaderPacket) == OBJECT_HEADER_PACKET_SIZE); @@ -422,7 +431,7 @@ void ContinentalSRR520Decoder::ProcessObjectHeaderPacket( object_header_packet_.u_global_time_stamp_sync_status >= 1 && object_header_packet_.u_global_time_stamp_sync_status <= 3); - object_list_ptr_->header.frame_id = sensor_configuration_->frame_id; + object_list_ptr_->header.frame_id = sensor_configuration_->base_frame; if (object_header_packet_.u_global_time_stamp_sync_status == 1) { object_list_ptr_->header.stamp.sec = object_header_packet_.u_global_time_stamp_sec.value(); @@ -432,7 +441,7 @@ void ContinentalSRR520Decoder::ProcessObjectHeaderPacket( } object_packets_ptr_->header.stamp = packet_msg->stamp; - object_packets_ptr_->header.frame_id = sensor_configuration_->frame_id; + object_packets_ptr_->header.frame_id = sensor_configuration_->base_frame; object_list_ptr_->internal_time_stamp_usec = object_header_packet_.u_time_stamp.value(); object_list_ptr_->global_time_stamp_sync_status = @@ -461,6 +470,7 @@ void ContinentalSRR520Decoder::ProcessObjectElementPacket( } if (object_list_ptr_->objects.size() >= object_header_packet_.u_number_of_objects) { + object_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); return; } @@ -470,7 +480,9 @@ void ContinentalSRR520Decoder::ProcessObjectElementPacket( static_assert(sizeof(ObjectPacket) == OBJECT_PACKET_SIZE); assert(packet_msg->data.size() == OBJECT_PACKET_SIZE + 4); assert(object_header_packet_.u_sequence_counter == object_packet.u_sequence_counter); - assert(object_packets_ptr_->packets.size() == object_packet.u_message_counter + 1); + assert( + object_packets_ptr_->packets.size() == + static_cast(object_packet.u_message_counter + 1)); for (const auto & fragment : object_packet.fragments) { continental_msgs::msg::ContinentalSrr520Object object_msg; @@ -603,8 +615,9 @@ void ContinentalSRR520Decoder::ProcessNearCRCListPacket( PrintError("Incorrect number of RDI Near elements before CRC list"); } - near_detection_list_ptr_.reset(); - rdi_near_packets_ptr_.reset(); + rdi_near_packets_ptr_ = std::make_unique(); + near_detection_list_ptr_ = + std::make_unique(); return; } @@ -617,8 +630,9 @@ void ContinentalSRR520Decoder::ProcessNearCRCListPacket( PrintError( "RDI Near: Transmitted CRC list does not coincide with the computed one. Ignoring packet"); - near_detection_list_ptr_.reset(); - rdi_near_packets_ptr_.reset(); + rdi_near_packets_ptr_ = std::make_unique(); + near_detection_list_ptr_ = + std::make_unique(); return; } @@ -632,8 +646,9 @@ void ContinentalSRR520Decoder::ProcessNearCRCListPacket( nebula_packets_callback_(std::move(rdi_near_packets_ptr_)); } - near_detection_list_ptr_.reset(); - rdi_near_packets_ptr_.reset(); + rdi_near_packets_ptr_ = std::make_unique(); + near_detection_list_ptr_ = + std::make_unique(); } void ContinentalSRR520Decoder::ProcessHRRCRCListPacket( @@ -644,8 +659,9 @@ void ContinentalSRR520Decoder::ProcessHRRCRCListPacket( PrintError("Incorrect number of RDI HRR elements before CRC list"); } - hrr_detection_list_ptr_.reset(); - rdi_hrr_packets_ptr_.reset(); + rdi_hrr_packets_ptr_ = std::make_unique(); + hrr_detection_list_ptr_ = + std::make_unique(); return; } @@ -657,8 +673,9 @@ void ContinentalSRR520Decoder::ProcessHRRCRCListPacket( if (transmitted_crc != computed_crc) { PrintError( "RDI HRR: Transmitted CRC list does not coincide with the computed one. Ignoring packet"); - hrr_detection_list_ptr_.reset(); - rdi_hrr_packets_ptr_.reset(); + rdi_hrr_packets_ptr_ = std::make_unique(); + hrr_detection_list_ptr_ = + std::make_unique(); return; } @@ -672,8 +689,9 @@ void ContinentalSRR520Decoder::ProcessHRRCRCListPacket( nebula_packets_callback_(std::move(rdi_hrr_packets_ptr_)); } - hrr_detection_list_ptr_.reset(); - rdi_hrr_packets_ptr_.reset(); + rdi_hrr_packets_ptr_ = std::make_unique(); + hrr_detection_list_ptr_ = + std::make_unique(); } void ContinentalSRR520Decoder::ProcessObjectCRCListPacket( @@ -684,8 +702,8 @@ void ContinentalSRR520Decoder::ProcessObjectCRCListPacket( PrintError("Incorrect number of object packages before CRC list"); } - object_list_ptr_.reset(); - object_packets_ptr_.reset(); + object_packets_ptr_ = std::make_unique(); + object_list_ptr_ = std::make_unique(); return; } @@ -700,8 +718,8 @@ void ContinentalSRR520Decoder::ProcessObjectCRCListPacket( PrintError( "Object: Transmitted CRC list does not coincide with the computed one. Ignoring packet"); - object_list_ptr_.reset(); - object_packets_ptr_.reset(); + object_packets_ptr_ = std::make_unique(); + object_list_ptr_ = std::make_unique(); return; } @@ -715,8 +733,8 @@ void ContinentalSRR520Decoder::ProcessObjectCRCListPacket( nebula_packets_callback_(std::move(object_packets_ptr_)); } - object_list_ptr_.reset(); - object_packets_ptr_.reset(); + object_packets_ptr_ = std::make_unique(); + object_list_ptr_ = std::make_unique(); } void ContinentalSRR520Decoder::ProcessSensorStatusPacket( diff --git a/nebula_ros/launch/continental_launch_all_hw.xml b/nebula_ros/launch/continental_launch_all_hw.xml index 24016d89c..445875396 100644 --- a/nebula_ros/launch/continental_launch_all_hw.xml +++ b/nebula_ros/launch/continental_launch_all_hw.xml @@ -10,6 +10,11 @@ + + + + + @@ -19,10 +24,12 @@ - - - - + + + + + + @@ -54,44 +61,29 @@ - + - - - - - - - - - + - - - - - - - - - + + + - - - - - - - - - + + + + + + + + + From 6555889fe08059fa359b8f675a0f044335bff722 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Fri, 31 May 2024 17:03:26 +0900 Subject: [PATCH 11/47] feat: brought back the srr520 tests (passing) Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental/continental_ars548.hpp | 2 +- .../decoders/continental_srr520_decoder.cpp | 2 - nebula_tests/continental/CMakeLists.txt | 21 + ...ntinental_ros_decoder_test_main_srr520.cpp | 60 + .../continental_ros_decoder_test_srr520.cpp | 7 + .../continental_ros_decoder_test_srr520.hpp | 3 + .../srr520/1708578208_992410284_object.yaml | 854 +++++++ .../srr520/1708578209/1708578209.db3 | Bin 0 -> 28672 bytes .../srr520/1708578209/metadata.yaml | 26 + .../1708578209_45566935_near_detection.yaml | 2147 +++++++++++++++++ .../1708578209_76111685_hrr_detection.yaml | 365 +++ 11 files changed, 3484 insertions(+), 3 deletions(-) create mode 100644 nebula_tests/continental/continental_ros_decoder_test_main_srr520.cpp create mode 100644 nebula_tests/data/continental/srr520/1708578208_992410284_object.yaml create mode 100644 nebula_tests/data/continental/srr520/1708578209/1708578209.db3 create mode 100644 nebula_tests/data/continental/srr520/1708578209/metadata.yaml create mode 100644 nebula_tests/data/continental/srr520/1708578209_45566935_near_detection.yaml create mode 100644 nebula_tests/data/continental/srr520/1708578209_76111685_hrr_detection.yaml diff --git a/nebula_common/include/nebula_common/continental/continental_ars548.hpp b/nebula_common/include/nebula_common/continental/continental_ars548.hpp index 58eaa0c0c..00b01d996 100644 --- a/nebula_common/include/nebula_common/continental/continental_ars548.hpp +++ b/nebula_common/include/nebula_common/continental/continental_ars548.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. diff --git a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp index 356f93398..75658539e 100644 --- a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp @@ -712,8 +712,6 @@ void ContinentalSRR520Decoder::ProcessObjectCRCListPacket( uint16_t computed_crc = crc16_packets(object_packets_ptr_->packets.begin(), object_packets_ptr_->packets.end(), 4); - // uint8_t current_seq = buffer[3]; - if (transmitted_crc != computed_crc) { PrintError( "Object: Transmitted CRC list does not coincide with the computed one. Ignoring packet"); diff --git a/nebula_tests/continental/CMakeLists.txt b/nebula_tests/continental/CMakeLists.txt index 44daf8813..f35d3de44 100644 --- a/nebula_tests/continental/CMakeLists.txt +++ b/nebula_tests/continental/CMakeLists.txt @@ -18,3 +18,24 @@ target_link_libraries(continental_ros_decoder_test_main_ars548 ${PCL_LIBRARIES} continental_ros_decoder_test_ars548 ) + +# Continental SRR520 +ament_auto_add_library(continental_ros_decoder_test_srr520 SHARED +continental_ros_decoder_test_srr520.cpp + ) +ament_add_gtest(continental_ros_decoder_test_main_srr520 +continental_ros_decoder_test_main_srr520.cpp + ) +target_link_libraries(continental_ros_decoder_test_srr520 ${PCL_LIBRARIES} ${NEBULA_TEST_LIBRARIES}) + +ament_target_dependencies(continental_ros_decoder_test_main_srr520 + ${NEBULA_TEST_DEPENDENCIES} + ) +target_include_directories(continental_ros_decoder_test_main_srr520 PUBLIC + ${PROJECT_SOURCE_DIR}/src/continental + include + ) +target_link_libraries(continental_ros_decoder_test_main_srr520 + ${PCL_LIBRARIES} + continental_ros_decoder_test_srr520 + ) diff --git a/nebula_tests/continental/continental_ros_decoder_test_main_srr520.cpp b/nebula_tests/continental/continental_ros_decoder_test_main_srr520.cpp new file mode 100644 index 000000000..56b072750 --- /dev/null +++ b/nebula_tests/continental/continental_ros_decoder_test_main_srr520.cpp @@ -0,0 +1,60 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "continental_ros_decoder_test_srr520.hpp" + +#include + +#include + +#include + +std::shared_ptr continental_driver; + +TEST(TestDecoder, TestBag) +{ + std::cout << "TEST(TestDecoder, TestBag)" << std::endl; + continental_driver->ReadBag(); +} + +int main(int argc, char * argv[]) +{ + std::cout << "continental_ros_decoder_test_main_srr520.cpp" << std::endl; + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + + std::string node_name = "nebula_continental_decoder_test"; + + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + rclcpp::executors::SingleThreadedExecutor exec; + rclcpp::NodeOptions options; + + continental_driver = std::make_shared(options, node_name); + exec.add_node(continental_driver->get_node_base_interface()); + + RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Get Status"); + nebula::Status driver_status = continental_driver->GetStatus(); + int result = 0; + if (driver_status == nebula::Status::OK) { + RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Reading Started"); + result = RUN_ALL_TESTS(); + } else { + RCLCPP_ERROR_STREAM(rclcpp::get_logger(node_name), driver_status); + } + RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Ending"); + continental_driver.reset(); + rclcpp::shutdown(); + + return result; +} diff --git a/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp b/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp index c75f2e053..be79400ba 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp +++ b/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp @@ -207,6 +207,7 @@ void ContinentalRosDecoderTest::CheckResult( void ContinentalRosDecoderTest::HRRDetectionListCallback( std::unique_ptr msg) { + hrr_detection_list_count_++; EXPECT_EQ(sensor_cfg_ptr_->frame_id, msg->header.frame_id); std::string msg_as_string = continental_msgs::msg::to_yaml(*msg); @@ -223,6 +224,7 @@ void ContinentalRosDecoderTest::HRRDetectionListCallback( void ContinentalRosDecoderTest::NearDetectionListCallback( std::unique_ptr msg) { + near_detection_list_count_++; EXPECT_EQ(sensor_cfg_ptr_->frame_id, msg->header.frame_id); std::string msg_as_string = continental_msgs::msg::to_yaml(*msg); @@ -239,6 +241,7 @@ void ContinentalRosDecoderTest::NearDetectionListCallback( void ContinentalRosDecoderTest::ObjectListCallback( std::unique_ptr msg) { + object_list_count_++; EXPECT_EQ(sensor_cfg_ptr_->base_frame, msg->header.frame_id); std::string msg_as_string = continental_msgs::msg::to_yaml(*msg); @@ -297,6 +300,10 @@ void ContinentalRosDecoderTest::ReadBag() } } } + + EXPECT_EQ(1, near_detection_list_count_); + EXPECT_EQ(1, hrr_detection_list_count_); + EXPECT_EQ(1, object_list_count_); } } // namespace ros diff --git a/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp b/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp index 463eda30d..d1ad80f83 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp +++ b/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp @@ -89,6 +89,9 @@ class ContinentalRosDecoderTest final : public rclcpp::Node, std::string storage_id; std::string format; std::string target_topic; + std::size_t near_detection_list_count_{}; + std::size_t hrr_detection_list_count_{}; + std::size_t object_list_count_{}; }; } // namespace ros diff --git a/nebula_tests/data/continental/srr520/1708578208_992410284_object.yaml b/nebula_tests/data/continental/srr520/1708578208_992410284_object.yaml new file mode 100644 index 000000000..dcdd5843c --- /dev/null +++ b/nebula_tests/data/continental/srr520/1708578208_992410284_object.yaml @@ -0,0 +1,854 @@ +header: + stamp: + sec: 1708578208 + nanosec: 992410284 + frame_id: some_sensor_frame +internal_time_stamp_usec: 468502487 +global_time_stamp_sync_status: 1 +signal_status: 1 +sequence_counter: 198 +cycle_counter: 451 +ego_vx: 6.78121 +ego_yaw_rate: -0.00632930 +motion_type: 1 +objects: + - object_id: 8 + dist_x: 26.9082 + dist_y: 1.72125 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: -3.12653 + score: 26.6667 + life_cycles: 2 + box_valid: 0 + object_status: 3 + - object_id: 0 + dist_x: 9.18302 + dist_y: 4.48623 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.0714155 + dist_y_std: 0.104376 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.417582 + box_width: 1.00366 + orientation: 2.63000e-07 + rcs: 0.439667 + score: 100.000 + life_cycles: 78 + box_valid: 1 + object_status: 2 + - object_id: 4 + dist_x: 0.503561 + dist_y: 5.72223 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: 2.02736 + score: 100.000 + life_cycles: 9 + box_valid: 0 + object_status: 2 + - object_id: 11 + dist_x: 43.1043 + dist_y: -0.119017 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.203259 + dist_y_std: 1.38986 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00732601 + box_width: 0.146520 + orientation: 2.63000e-07 + rcs: 21.8857 + score: 86.6667 + life_cycles: 3 + box_valid: 1 + object_status: 2 + - object_id: 5 + dist_x: 5.29191 + dist_y: 15.8757 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.159311 + dist_y_std: 0.0878960 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 1.00366 + box_width: 0.981685 + orientation: 2.63000e-07 + rcs: 8.74450 + score: 100.000 + life_cycles: 38 + box_valid: 1 + object_status: 3 + - object_id: 9 + dist_x: 27.4392 + dist_y: -5.39262 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: 15.9257 + score: 26.6667 + life_cycles: 2 + box_valid: 0 + object_status: 3 + - object_id: 10 + dist_x: 4.79751 + dist_y: 16.0405 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: -3.59062 + score: 26.6667 + life_cycles: 2 + box_valid: 0 + object_status: 3 + - object_id: 7 + dist_x: -2.04168 + dist_y: 6.11591 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.102545 + dist_y_std: 0.0842336 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.388278 + box_width: 0.446886 + orientation: 2.63000e-07 + rcs: -3.37079 + score: 100.000 + life_cycles: 25 + box_valid: 1 + object_status: 2 + - object_id: 2 + dist_x: 4.43129 + dist_y: 16.3427 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: 6.64387 + score: 53.3333 + life_cycles: 1 + box_valid: 0 + object_status: 0 + - object_id: 6 + dist_x: 13.5502 + dist_y: 10.6754 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.355246 + dist_y_std: 0.379051 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.161172 + box_width: 0.205128 + orientation: 2.63000e-07 + rcs: 1.58769 + score: 100.000 + life_cycles: 5 + box_valid: 1 + object_status: 2 + - object_id: 14 + dist_x: 3.85449 + dist_y: 21.4057 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: -13.2145 + score: 53.3333 + life_cycles: 1 + box_valid: 0 + object_status: 0 + - object_id: 3 + dist_x: 12.7354 + dist_y: 4.01929 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: -10.4543 + score: 53.3333 + life_cycles: 1 + box_valid: 0 + object_status: 0 + - object_id: 23 + dist_x: 7.93787 + dist_y: 6.15254 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.106208 + dist_y_std: 0.119026 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.278388 + box_width: 0.271062 + orientation: 2.63000e-07 + rcs: -2.12506 + score: 100.000 + life_cycles: 46 + box_valid: 1 + object_status: 2 + - object_id: 12 + dist_x: -0.292973 + dist_y: 6.02436 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.106208 + dist_y_std: 0.0805713 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.234432 + box_width: 0.344322 + orientation: 2.63000e-07 + rcs: -4.29897 + score: 100.000 + life_cycles: 82 + box_valid: 1 + object_status: 2 + - object_id: 17 + dist_x: 29.3527 + dist_y: -5.52995 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.0293040 + box_width: 0.0586081 + orientation: 2.63000e-07 + rcs: 24.4504 + score: 73.3333 + life_cycles: 2 + box_valid: 1 + object_status: 2 + - object_id: 15 + dist_x: 3.66223 + dist_y: 19.8401 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: -4.59209 + score: 53.3333 + life_cycles: 1 + box_valid: 0 + object_status: 0 + - object_id: 16 + dist_x: 44.7249 + dist_y: 1.68463 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: 13.5076 + score: 53.3333 + life_cycles: 1 + box_valid: 0 + object_status: 0 + - object_id: 32 + dist_x: 9.12809 + dist_y: 10.6388 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.108039 + dist_y_std: 0.0842336 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 1.00366 + box_width: 0.996337 + orientation: 2.63000e-07 + rcs: 3.59062 + score: 100.000 + life_cycles: 39 + box_valid: 1 + object_status: 2 + - object_id: 20 + dist_x: 3.69885 + dist_y: 15.4180 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.172130 + dist_y_std: 0.113532 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.732601 + box_width: 0.783883 + orientation: 2.63000e-07 + rcs: -0.952614 + score: 86.6667 + life_cycles: 70 + box_valid: 1 + object_status: 3 + - object_id: 35 + dist_x: 11.0874 + dist_y: 4.29396 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.0677531 + dist_y_std: 0.119026 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 1.00366 + box_width: 1.00366 + orientation: 2.63000e-07 + rcs: -2.54030 + score: 100.000 + life_cycles: 56 + box_valid: 1 + object_status: 2 + - object_id: 18 + dist_x: 4.02845 + dist_y: 17.7709 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: -15.1685 + score: 53.3333 + life_cycles: 1 + box_valid: 0 + object_status: 0 + - object_id: 19 + dist_x: 20.6641 + dist_y: 12.0670 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.282000 + dist_y_std: 0.459623 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.212454 + box_width: 0.190476 + orientation: 2.63000e-07 + rcs: 4.71422 + score: 66.6667 + life_cycles: 11 + box_valid: 1 + object_status: 3 + - object_id: 28 + dist_x: 9.36614 + dist_y: 5.99689 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.104376 + dist_y_std: 0.135506 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.637363 + box_width: 0.256410 + orientation: 2.63000e-07 + rcs: 1.53884 + score: 100.000 + life_cycles: 36 + box_valid: 1 + object_status: 2 + - object_id: 34 + dist_x: 6.46383 + dist_y: 11.6733 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.373558 + dist_y_std: 0.146493 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.219780 + box_width: 0.336996 + orientation: 2.63000e-07 + rcs: -10.0147 + score: 100.000 + life_cycles: 5 + box_valid: 1 + object_status: 2 + - object_id: 22 + dist_x: 4.90738 + dist_y: 15.5187 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: -4.20127 + score: 53.3333 + life_cycles: 1 + box_valid: 0 + object_status: 0 + - object_id: 26 + dist_x: 4.35805 + dist_y: 16.3610 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: 6.93698 + score: 26.6667 + life_cycles: 2 + box_valid: 0 + object_status: 3 + - object_id: 31 + dist_x: 1.15360 + dist_y: 6.40889 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.0970518 + dist_y_std: 0.0677531 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 1.00366 + box_width: 0.893773 + orientation: 2.63000e-07 + rcs: -6.88813 + score: 100.000 + life_cycles: 63 + box_valid: 1 + object_status: 2 + - object_id: 21 + dist_x: 6.18916 + dist_y: 13.0558 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.267350 + dist_y_std: 0.130013 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.527472 + box_width: 0.271062 + orientation: 2.63000e-07 + rcs: -7.18124 + score: 100.000 + life_cycles: 13 + box_valid: 1 + object_status: 3 + - object_id: 33 + dist_x: 20.2338 + dist_y: 5.07218 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.126350 + dist_y_std: 0.415675 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.322344 + box_width: 0.630037 + orientation: 2.63000e-07 + rcs: -4.42110 + score: 100.000 + life_cycles: 9 + box_valid: 1 + object_status: 2 + - object_id: 27 + dist_x: 31.5226 + dist_y: -2.06915 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: 7.79189 + score: 53.3333 + life_cycles: 1 + box_valid: 0 + object_status: 0 + - object_id: 25 + dist_x: 18.7964 + dist_y: 3.58898 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.104376 + dist_y_std: 0.357077 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.534799 + box_width: 0.688645 + orientation: 2.63000e-07 + rcs: -2.95555 + score: 93.3333 + life_cycles: 17 + box_valid: 1 + object_status: 3 + - object_id: 24 + dist_x: 3.40587 + dist_y: 15.6743 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: -10.5276 + score: 53.3333 + life_cycles: 1 + box_valid: 0 + object_status: 0 + - object_id: 29 + dist_x: 24.5460 + dist_y: 3.90943 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.0769090 + dist_y_std: 0.258194 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.307692 + box_width: 0.622711 + orientation: 2.63000e-07 + rcs: 4.29897 + score: 100.000 + life_cycles: 23 + box_valid: 1 + object_status: 2 + - object_id: 30 + dist_x: 31.0099 + dist_y: -6.34479 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: 24.3283 + score: 26.6667 + life_cycles: 2 + box_valid: 0 + object_status: 3 + - object_id: 36 + dist_x: 3.23192 + dist_y: 21.4698 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: -10.9184 + score: 26.6667 + life_cycles: 2 + box_valid: 0 + object_status: 3 + - object_id: 41 + dist_x: -2.84737 + dist_y: 6.15254 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.0952206 + dist_y_std: 0.0897271 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.315018 + box_width: 0.234432 + orientation: 2.63000e-07 + rcs: 1.34343 + score: 100.000 + life_cycles: 91 + box_valid: 1 + object_status: 2 + - object_id: 39 + dist_x: 4.69680 + dist_y: 16.0039 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: 1.85637 + score: 26.6667 + life_cycles: 2 + box_valid: 0 + object_status: 3 + - object_id: 13 + dist_x: 29.0964 + dist_y: -3.08542 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.188610 + dist_y_std: 0.783739 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.117216 + box_width: 0.271062 + orientation: 2.63000e-07 + rcs: 13.2633 + score: 40.0000 + life_cycles: 7 + box_valid: 1 + object_status: 3 + - object_id: 43 + dist_x: 16.6631 + dist_y: 4.09254 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.0787401 + dist_y_std: 0.186779 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.358974 + box_width: 0.681319 + orientation: 2.63000e-07 + rcs: 6.39961 + score: 100.000 + life_cycles: 48 + box_valid: 1 + object_status: 2 + - object_id: 40 + dist_x: 16.9836 + dist_y: 11.8748 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.250870 + dist_y_std: 0.351584 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.373626 + box_width: 0.593407 + orientation: 2.63000e-07 + rcs: -4.49438 + score: 80.0000 + life_cycles: 11 + box_valid: 1 + object_status: 3 diff --git a/nebula_tests/data/continental/srr520/1708578209/1708578209.db3 b/nebula_tests/data/continental/srr520/1708578209/1708578209.db3 new file mode 100644 index 0000000000000000000000000000000000000000..1300ded382c73b3fad886c545ba569cfa8dc907c GIT binary patch literal 28672 zcmeI5dsI_bzQ@lqF9<FaRiEnK!QX=h)F=i2PC`^tRh;k zqD5ql$aEa#TDABXij;O7z1P)JZL7V*@^N+0>Fuq3ScS>m*?Co=ceN||V^-HW3r^12 zzq3F4yT9iohked3hgB<6y7s)hQNs&q{mzIG=jbyz{LldSa@-q^-%Th&&ncSB);l7^AwQR-> zw1&9+Rl*>hJ3;sI^+k1Cw30l9%(9u~l{8Z`_=1!~DFywr!7{=^MNR9F!l8eZtCiklzFyBE%mG+u2iCxa0Mx^3%HiDAYCX<_nI;V)yF|q zkgGAxiI)_a%6?9dQc@)2W+Y|;5mq5vRIe6kWNNuYAupF`<;6;|c|7}at>S_LnOY_l zZz|S^RqEmbxk9FS862d;F_V#5W+Sx{W5B>|?gK-0^(R(`rq)u78#xkjrlex)hn*c|12hkzeyq8y+c zpd6qapd6qapd6qapd6qapd6qapd6qa_$@iW#BpRwVM!6VRAe0ggPZ=;L^(h?Ksi7; zKsi7;Ksi7;Ksi7;Ksi7;Ksi7;@Edjj-v4K-5b#4ylmnCllmnCllmnCllmnCllmnCl zlmnCl4s@V~jna{|OWYfnAsU%dBUi2uQG*n7bx47_Sg92&WChv~r7W*RArY%2`5R?g zjd`s|vtAPdW+ACCnq~^-OVz>LLXg;9tlkpK4P$Y+QkhCy7|V?Wi)xudF3FQCLw=D&&@R&|!&2CN>H)=I#F)sHC)5E)^RS+l^lk0Fv@$;{0M|sZ41k zftCs+3XP04u{~j-pevL=8hHPoKo%n|uJnB5ELiweQz=11m^59ESMU4l`gW-><1+z{ zetJ{Zb!dcPc|3se)lw#cbnK$fSDTN?Zhm@{=65d=-N!{x{qL&s^tp9^VLdoKQ1MUW zR^6EK2?Hp;x57}-0IOMM;)KP`b8utB2Svt)FY)pkEDaCuce6I^2`U&HHkSFr%0!CK zS1#9$4HFIGL=+uB5Pa9+$LL{%Yla)Q@4GsiVG>HLa38sI2qAo0Tj~)a=7HNj1W!r7 z8;{@;=H4eFLUJ`DES)UQA$lv>C8a%27Ya){TS|6wN)!+9Gy)(8UgcGMnth;+R_WRJ zu+zEjJN*g812LSnv{0#X?F5DkKRQW_NmE>fpa&+l7Iew?%`OE zE1Rq0V#sU9m(Scl4i#!DTfn0ouyMT4*5J%Orx#DLsDlJTe_`kXkAV2aa|Ac_S8CJu zD{7B#2O%Ku)WQ^Nf9>Oukar-#Q9PQ1d34D8C|cs6{xVGd#W{+{bTE$vdCyLlIHIC9s{0F za4s5}g*4B>Y$vO80UvMve@|ASL3DpunKy@IPh zWC|n0*s!_1Nc@&zvly8|;*i(=uEeSj)7`6mHX(2KGqBHSH6K|3|a${PSQ;J`m3$_CNMPfz%l;??fz?)9iVH(`a2dQgE|b-%j7tnZ0+e>5=a3=JVj0Lg&7t|S}J zL$|kfuc-oBh7X%N1pdS^Y;Q)A=rgN(bwg%etJRJMa6E6RKw?NHftxhaf?8Xukor|H;3LxC6^FHVq`w#vry)O*z)nNVMe(eSCd+qGuiD{t>T z#`lLaC>#P@SRZp-bX;bi%H)HHr|waLRV+1 zfZTVs@RpGe4P{DX65U&;sr}Q0JQ)MCLg*lPBa|jsX6-n);iKSx#1`{dKtwffH7c5}-oRj*qNw#~Zk-xw*L*-B4AGyg^Qa zyk{+LR$hP}sr_U^UbS%tvYX^PPMX4K+tfZY&lT*CqyLT2>28o`9L!ig@8{O}=q{To zAn(v}%@xAPTZY|YB$N!STU_I63S<7x<|x7=y$srI;CObo@+>DSZ1x?borM}tR(*tV zWOZ(`s{%L;M^Sxs=``a|&NxMYUpTS>W&W@PulD)JTic8yE9~6zTPS)3jI5dusX?qC z#%t3{)C)@+lcq)8!_PbWY#7K<34IP;T%i{FOx4Ee$!P&kGqu90Kg{(l6Hfo=N{3Vk zMpol^GrafiDPYe5;_aDjp@M}3=Dg@yVvXAD0>mIamJYL-Wi@RvN zfu0=-BI3lA7iFTz^v3A3tcc^n=4?S^RrWHaAmY7igi06{*YtMFlBng1J%jA1Z=W}9 z06aS66%Myp`y0KhxuIJW>(kj(urfCI(&lR6g3;?-r68_6(*rI`B8*%}U0eNZnZM;R@PuCWpix_Q-GVsOstFklGWBM}^C_NryY=%a5>CEy9$wU#{+=rN(c6bCg^$x^P5|t=O7TUrwa{ zW$$1vp1LwQx?2eTCj{grEU@v?ioQ%9a!EUUDMv3@H7CMbie^kxbIYSfg;`#>QyxFtD{gudBkNvzQa||ai8JH{F0%2m6`#^BI<2h}`WASs%aMMs z!q(n=!OX$L{$ErnadB9AWIHY<4whu|bA2-E6sY8BxJbd59#ZTn=OYLk^1NFvS^Fz{ z?c?xA?821?!yh>n)(Lm1MRLz8^jv=7A!+kAY(v(){d!V?rAPNnDembImZ9o`+oYbp zsYe}E4Kh$%#__aH8TIwJ_Jwp`CARrkctRPr`KVjORlM|K)8Dk{=2-t3JA_+;o=mDI zx5VY_EJwGx&*4|%+gAETd=0L%a>nth!v&FDUbQ@duv}2LX#;QAyCLmv#3-}AwXs~q zufOUXpu$>{9y9dBe!t|1QBGTjUt~Y?NOFXq7F>>Xg*?RV6Kj8uRk8S8v}0ha-;26B zVk6Ge&bWWG@&JA=y*FV$Hu!OJLe7atC_txQ}N$dF`$r?+cI-G70h? zO}6p=bByga>hj47U$?8?Lv=il7Gg*}m3JP$z13sW5bO3gSVSK&oGgt#g5B+iX||_4MP$n6u#h;Wm!v=q zMfJw#mY7Nq56C;v?l{b%d?4>xu%md>Aa5Al^tar{u&=+qkY`H@u-Cuokf+*bNeZx+Hv{r)Ne1@vxR7T{ z3b2>=8syoM0_^4aL0 Date: Fri, 31 May 2024 17:17:24 +0900 Subject: [PATCH 12/47] chore: removed chunks of code that got in during the rebase. updated the config file Signed-off-by: Kenzo Lobos-Tsunekawa --- .../include/nebula_common/nebula_common.hpp | 47 +++++++------------ nebula_ros/config/continental/SRR520.yaml | 14 ++---- 2 files changed, 22 insertions(+), 39 deletions(-) diff --git a/nebula_common/include/nebula_common/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp index 6713fdd22..ccc1f0289 100644 --- a/nebula_common/include/nebula_common/nebula_common.hpp +++ b/nebula_common/include/nebula_common/nebula_common.hpp @@ -2,7 +2,10 @@ #define NEBULA_COMMON_H #include + #include + +#include #include #include #include @@ -324,7 +327,6 @@ enum class SensorModel { VELODYNE_HDL32, VELODYNE_VLP16, ROBOSENSE_HELIOS, - ROBOSENSE_BPEARL, ROBOSENSE_BPEARL_V3, ROBOSENSE_BPEARL_V4, CONTINENTAL_ARS548, @@ -343,24 +345,11 @@ enum class datatype { FLOAT64 = 8 }; -enum class PtpProfile { - IEEE_1588v2 = 0, - IEEE_802_1AS, - IEEE_802_1AS_AUTO, - UNKNOWN_PROFILE -}; +enum class PtpProfile { IEEE_1588v2 = 0, IEEE_802_1AS, IEEE_802_1AS_AUTO, UNKNOWN_PROFILE }; -enum class PtpTransportType { - UDP_IP = 0, - L2, - UNKNOWN_TRANSPORT -}; +enum class PtpTransportType { UDP_IP = 0, L2, UNKNOWN_TRANSPORT }; -enum class PtpSwitchType { - NON_TSN = 0, - TSN, - UNKNOWN_SWITCH -}; +enum class PtpSwitchType { NON_TSN = 0, TSN, UNKNOWN_SWITCH }; /// @brief not used? struct PointField @@ -429,9 +418,6 @@ inline std::ostream & operator<<(std::ostream & os, nebula::drivers::SensorModel case SensorModel::ROBOSENSE_HELIOS: os << "HELIOS"; break; - case SensorModel::ROBOSENSE_BPEARL: - os << "BPEARL"; - break; case SensorModel::ROBOSENSE_BPEARL_V3: os << "BPEARL V3.0"; break; @@ -567,9 +553,9 @@ inline SensorModel SensorModelFromString(const std::string & sensor_model) if (sensor_model == "VLP16") return SensorModel::VELODYNE_VLP16; // Robosense if (sensor_model == "Helios") return SensorModel::ROBOSENSE_HELIOS; - if (sensor_model == "Bpearl") return SensorModel::ROBOSENSE_BPEARL; + if (sensor_model == "Bpearl" || sensor_model == "Bpearl_V4") + return SensorModel::ROBOSENSE_BPEARL_V4; if (sensor_model == "Bpearl_V3") return SensorModel::ROBOSENSE_BPEARL_V3; - if (sensor_model == "Bpearl_V4") return SensorModel::ROBOSENSE_BPEARL_V4; // Continental if (sensor_model == "ARS548") return SensorModel::CONTINENTAL_ARS548; if (sensor_model == "SRR520") return SensorModel::CONTINENTAL_SRR520; @@ -614,8 +600,6 @@ inline std::string SensorModelToString(const SensorModel & sensor_model) // Robosense case SensorModel::ROBOSENSE_HELIOS: return "Helios"; - case SensorModel::ROBOSENSE_BPEARL: - return "Bpearl"; case SensorModel::ROBOSENSE_BPEARL_V3: return "Bpearl_V3"; case SensorModel::ROBOSENSE_BPEARL_V4: @@ -650,8 +634,9 @@ inline PtpProfile PtpProfileFromString(const std::string & ptp_profile) { // Hesai auto tmp_str = ptp_profile; - std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), - [](unsigned char c){ return std::tolower(c); }); + std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), [](unsigned char c) { + return std::tolower(c); + }); if (tmp_str == "1588v2") return PtpProfile::IEEE_1588v2; if (tmp_str == "802.1as") return PtpProfile::IEEE_802_1AS; if (tmp_str == "automotive") return PtpProfile::IEEE_802_1AS_AUTO; @@ -689,8 +674,9 @@ inline PtpTransportType PtpTransportTypeFromString(const std::string & transport { // Hesai auto tmp_str = transport_type; - std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), - [](unsigned char c){ return std::tolower(c); }); + std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), [](unsigned char c) { + return std::tolower(c); + }); if (tmp_str == "udp") return PtpTransportType::UDP_IP; if (tmp_str == "l2") return PtpTransportType::L2; @@ -724,8 +710,9 @@ inline PtpSwitchType PtpSwitchTypeFromString(const std::string & switch_type) { // Hesai auto tmp_str = switch_type; - std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), - [](unsigned char c){ return std::tolower(c); }); + std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), [](unsigned char c) { + return std::tolower(c); + }); if (tmp_str == "tsn") return PtpSwitchType::TSN; if (tmp_str == "non_tsn") return PtpSwitchType::NON_TSN; diff --git a/nebula_ros/config/continental/SRR520.yaml b/nebula_ros/config/continental/SRR520.yaml index 644d424f7..8cdfee839 100644 --- a/nebula_ros/config/continental/SRR520.yaml +++ b/nebula_ros/config/continental/SRR520.yaml @@ -1,15 +1,11 @@ /**: ros__parameters: - sensor_model: "SRR520" - frame_id: "continental" - base_frame: "base_link" - interface: "can4" + sensor_model: SRR520 + frame_id: continental + base_frame: base_link + interface: can4 receiver_timeout_sec: 0.03 sender_timeout_sec: 0.01 - filters: "0:0" + filters: 0:0 use_bus_time: false - new_sensor_id: 0 - new_plug_bottom: true - new_cover_damping: 0.0 - reset_sensor_configuration: false configuration_vehicle_wheelbase: 2.79 From 12ba32cd1df32262d388bc4b644c85e3dc45118d Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Fri, 31 May 2024 17:19:41 +0900 Subject: [PATCH 13/47] chore: kept back a package that got upgraded automatically by pre commit Signed-off-by: Kenzo Lobos-Tsunekawa --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 498a511ff..b4c2e9f2f 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -18,7 +18,7 @@ repos: args: [--markdown-linebreak-ext=md] - repo: https://github.com/igorshubovych/markdownlint-cli - rev: v0.41.0 + rev: v0.40.0 hooks: - id: markdownlint args: [-c, .markdownlint.yaml, --fix] From 85cac29c5b0831d6d00dc30657828875779907cc Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Mon, 10 Jun 2024 17:20:25 +0900 Subject: [PATCH 14/47] chore: added some comments regarding filters Signed-off-by: Kenzo Lobos-Tsunekawa --- nebula_common/include/nebula_common/nebula_common.hpp | 2 +- nebula_ros/config/radar/continental/SRR520.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nebula_common/include/nebula_common/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp index ccc1f0289..320f31f41 100644 --- a/nebula_common/include/nebula_common/nebula_common.hpp +++ b/nebula_common/include/nebula_common/nebula_common.hpp @@ -458,7 +458,7 @@ struct CANSensorConfigurationBase : SensorConfigurationBase std::string interface; float receiver_timeout_sec{}; float sender_timeout_sec{}; - std::string filters{}; + std::string filters{}; // socketcan filters bool use_bus_time{}; }; diff --git a/nebula_ros/config/radar/continental/SRR520.yaml b/nebula_ros/config/radar/continental/SRR520.yaml index 8cdfee839..a02649a35 100644 --- a/nebula_ros/config/radar/continental/SRR520.yaml +++ b/nebula_ros/config/radar/continental/SRR520.yaml @@ -6,6 +6,6 @@ interface: can4 receiver_timeout_sec: 0.03 sender_timeout_sec: 0.01 - filters: 0:0 + filters: 0:0 # socketcan filters use_bus_time: false configuration_vehicle_wheelbase: 2.79 From f7787e6ebc4cc5917f7164d21744a951a50a6cb6 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Mon, 10 Jun 2024 17:35:41 +0900 Subject: [PATCH 15/47] chore: fixed segmentation when launch_hw was true and there was no can device. Changed the name of the config file for schema purposes Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental_srr520_hw_interface.cpp | 1 + .../config/radar/continental/{SRR520.yaml => SRR520.param.yaml} | 1 + 2 files changed, 2 insertions(+) rename nebula_ros/config/radar/continental/{SRR520.yaml => SRR520.param.yaml} (93%) diff --git a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp index 5a391baf6..16ae1962e 100644 --- a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp @@ -186,6 +186,7 @@ void ContinentalSRR520HwInterface::SensorSync() { if (!can_sender_ptr_) { PrintError("Can sender is invalid so can not do sync up"); + return; } if (!sync_fup_sent_) { diff --git a/nebula_ros/config/radar/continental/SRR520.yaml b/nebula_ros/config/radar/continental/SRR520.param.yaml similarity index 93% rename from nebula_ros/config/radar/continental/SRR520.yaml rename to nebula_ros/config/radar/continental/SRR520.param.yaml index a02649a35..15ffa26ae 100644 --- a/nebula_ros/config/radar/continental/SRR520.yaml +++ b/nebula_ros/config/radar/continental/SRR520.param.yaml @@ -4,6 +4,7 @@ frame_id: continental base_frame: base_link interface: can4 + launch_hw: true receiver_timeout_sec: 0.03 sender_timeout_sec: 0.01 filters: 0:0 # socketcan filters From c095232ffd87112a05d6788d98787fc32dda3699 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Mon, 10 Jun 2024 17:40:27 +0900 Subject: [PATCH 16/47] chore: updated cspell Signed-off-by: Kenzo Lobos-Tsunekawa --- .cspell.json | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.cspell.json b/.cspell.json index 5b977fd6c..fc25bef16 100644 --- a/.cspell.json +++ b/.cspell.json @@ -27,6 +27,7 @@ "Msop", "nohup", "nproc", + "nsec", "ntoa", "pandar", "PANDAR", @@ -45,6 +46,8 @@ "struct", "structs", "UDP_SEQ", + "usec", + "vambig", "vccint", "Vccint", "Vdat", From d545d84b033550f6ae16b2aed141f18fdb24ffd0 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Mon, 10 Jun 2024 17:47:16 +0900 Subject: [PATCH 17/47] chore: changes for clang compatibility Signed-off-by: Kenzo Lobos-Tsunekawa --- .../nebula_common/continental/continental_srr520.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/nebula_common/include/nebula_common/continental/continental_srr520.hpp b/nebula_common/include/nebula_common/continental/continental_srr520.hpp index 64554086d..3f7e981cb 100644 --- a/nebula_common/include/nebula_common/continental/continental_srr520.hpp +++ b/nebula_common/include/nebula_common/continental/continental_srr520.hpp @@ -237,7 +237,7 @@ struct FollowUpPacket #pragma pack(pop) -struct PointSRR520Detection +struct EIGEN_ALIGN16 PointSRR520Detection { PCL_ADD_POINT4D; float range; @@ -252,11 +252,11 @@ struct PointSRR520Detection uint8_t pdh05; float snr; EIGEN_MAKE_ALIGNED_OPERATOR_NEW -} EIGEN_ALIGN16; +}; // Note we only use a subset of the data since POINT_CLOUD_REGISTER_POINT_STRUCT has a limit in the // number of fields -struct PointSRR520Object +struct EIGEN_ALIGN16 PointSRR520Object { PCL_ADD_POINT4D; uint32_t id; @@ -272,7 +272,7 @@ struct PointSRR520Object float box_length; float box_width; EIGEN_MAKE_ALIGNED_OPERATOR_NEW -} EIGEN_ALIGN16; +}; } // namespace continental_srr520 } // namespace drivers From a151918489d35f9901b291561e401d925803a9ab Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Mon, 10 Jun 2024 18:19:18 +0900 Subject: [PATCH 18/47] chore: added an explanation to the concept of time domain id Signed-off-by: Kenzo Lobos-Tsunekawa --- .../include/nebula_common/continental/continental_srr520.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nebula_common/include/nebula_common/continental/continental_srr520.hpp b/nebula_common/include/nebula_common/continental/continental_srr520.hpp index 3f7e981cb..e29ecae08 100644 --- a/nebula_common/include/nebula_common/continental/continental_srr520.hpp +++ b/nebula_common/include/nebula_common/continental/continental_srr520.hpp @@ -83,7 +83,8 @@ constexpr int SENSOR_CONFIG_CAN_MESSAGE_ID = 601; constexpr int NEAR_CRC_ID = 0; constexpr int HRR_CRC_ID = 1; constexpr int OBJECT_CRC_ID = 2; -constexpr int TIME_DOMAIN_ID = 0; +constexpr int TIME_DOMAIN_ID = + 0; // For details, please refer to autosar's "Time Synchronization over CAN" document constexpr int RDI_NEAR_HEADER_PACKET_SIZE = 32; constexpr int RDI_NEAR_ELEMENT_PACKET_SIZE = 64; From b775188849cbf53ac444975053d174f02a2ddd89 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos Tsunekawa Date: Mon, 10 Jun 2024 18:25:02 +0900 Subject: [PATCH 19/47] Update nebula_ros/config/radar/continental/SRR520.param.yaml Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com> --- nebula_ros/config/radar/continental/SRR520.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_ros/config/radar/continental/SRR520.param.yaml b/nebula_ros/config/radar/continental/SRR520.param.yaml index 15ffa26ae..f3e50f934 100644 --- a/nebula_ros/config/radar/continental/SRR520.param.yaml +++ b/nebula_ros/config/radar/continental/SRR520.param.yaml @@ -7,6 +7,6 @@ launch_hw: true receiver_timeout_sec: 0.03 sender_timeout_sec: 0.01 - filters: 0:0 # socketcan filters + filters: 0:0 # candump-like filters use_bus_time: false configuration_vehicle_wheelbase: 2.79 From 8e247a8325bb43018c0413bcd5d0034f2ad57f26 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos Tsunekawa Date: Mon, 10 Jun 2024 18:25:24 +0900 Subject: [PATCH 20/47] Update nebula_common/include/nebula_common/nebula_common.hpp Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com> --- nebula_common/include/nebula_common/nebula_common.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nebula_common/include/nebula_common/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp index 320f31f41..0331179c0 100644 --- a/nebula_common/include/nebula_common/nebula_common.hpp +++ b/nebula_common/include/nebula_common/nebula_common.hpp @@ -458,7 +458,8 @@ struct CANSensorConfigurationBase : SensorConfigurationBase std::string interface; float receiver_timeout_sec{}; float sender_timeout_sec{}; - std::string filters{}; // socketcan filters + /// @brief Socketcan filters, see the documentation of SocketCanReceiver::CanFilterList for details + std::string filters{}; bool use_bus_time{}; }; From 7c03d64fe79c8e33b2640560c7da846856d68d15 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Tue, 11 Jun 2024 11:32:13 +0900 Subject: [PATCH 21/47] chore: added temptative schema Signed-off-by: Kenzo Lobos-Tsunekawa --- nebula_ros/schema/SRR520.schema.json | 69 ++++++++++++++++++++++++ nebula_ros/schema/sub/communication.json | 42 +++++++++------ 2 files changed, 96 insertions(+), 15 deletions(-) create mode 100644 nebula_ros/schema/SRR520.schema.json diff --git a/nebula_ros/schema/SRR520.schema.json b/nebula_ros/schema/SRR520.schema.json new file mode 100644 index 000000000..dc808247c --- /dev/null +++ b/nebula_ros/schema/SRR520.schema.json @@ -0,0 +1,69 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Radar Continental SRR520 parameters.", + "type": "object", + "definitions": { + "ARS548": { + "type": "object", + "properties": { + "interface": { + "$ref": "sub/communication.json#/definitions/interface" + }, + "receiver_timeout_sec": { + "$ref": "sub/communication.json#/definitions/receiver_timeout_sec" + }, + "sender_timeout_sec": { + "$ref": "sub/communication.json#/definitions/sender_timeout_sec" + }, + "filters": { + "$ref": "sub/communication.json#/definitions/filters" + }, + "launch_hw": { + "$ref": "sub/hardware.json#/definitions/launch_hw" + }, + "frame_id": { + "$ref": "sub/topic.json#/definitions/frame_id" + }, + "base_frame": { + "$ref": "sub/topic.json#/definitions/base_frame" + }, + "object_frame": { + "$ref": "sub/topic.json#/definitions/object_frame" + }, + "use_bus_time": { + "$ref": "sub/topic.json#/definitions/use_bus_time" + }, + "configuration_vehicle_wheelbase": { + "$ref": "sub/misc.json#/definitions/configuration_vehicle_wheelbase" + }, + "sensor_model": { + "$ref": "sub/radar_continental.json#/definitions/sensor_model" + } + }, + "required": [ + "interface", + "receiver_timeout_sec", + "sender_timeout_sec", + "filters", + "use_bus_time", + "launch_hw", + "configuration_vehicle_wheelbase", + "sensor_model" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/SRR520" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/nebula_ros/schema/sub/communication.json b/nebula_ros/schema/sub/communication.json index 315c6bfb6..b01c22e06 100644 --- a/nebula_ros/schema/sub/communication.json +++ b/nebula_ros/schema/sub/communication.json @@ -24,6 +24,11 @@ "readOnly": true, "description": "Sensor data port." }, + "filters": { + "type": "string", + "default": "0:0", + "description": "candump-style filters for CAN frames." + }, "gnss_port": { "type": "integer", "default": 2369, @@ -38,6 +43,11 @@ "readOnly": true, "description": "Host IPv4 address." }, + "interface": { + "type": "string", + "default": "can", + "description": "CAN interface name." + }, "multicast_ip": { "type": "string", "default": "224.0.0.2", @@ -62,11 +72,7 @@ "ptp_profile": { "type": "string", "default": "1588v2", - "enum": [ - "1588v2", - "802.1as", - "automotive" - ], + "enum": ["1588v2", "802.1as", "automotive"], "description": "PTP profile." }, "ptp_domain": { @@ -79,20 +85,26 @@ "ptp_transport_type": { "type": "string", "default": "UDP", - "enum": [ - "UDP", - "L2" - ], + "enum": ["UDP", "L2"], "description": "1588v2 supports 'UDP' or 'L2', other profiles only L2 (HW)." - }, + }, "ptp_switch_type": { "type": "string", "default": "TSN", - "enum": [ - "TSN", - "NON_TSN" - ], + "enum": ["TSN", "NON_TSN"], "description": "For automotive profile,'TSN' or 'NON_TSN'." + }, + "receiver_timeout_sec": { + "type": "number", + "default": 0.03, + "minimum": 0.0, + "description": "Timeout for reading data from the CAN bus." + }, + "sender_timeout_sec": { + "type": "number", + "default": 0.01, + "minimum": 0.0, + "description": "Timeout for sending data to the CAN bus." } } -} \ No newline at end of file +} From 2ff3256907bef99db507e8800b09c0fa1f8b4f8b Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Tue, 11 Jun 2024 11:35:49 +0900 Subject: [PATCH 22/47] chore: SRR520 schema still referenced the ARS548 radar Signed-off-by: Kenzo Lobos-Tsunekawa --- nebula_ros/schema/SRR520.schema.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_ros/schema/SRR520.schema.json b/nebula_ros/schema/SRR520.schema.json index dc808247c..19356adff 100644 --- a/nebula_ros/schema/SRR520.schema.json +++ b/nebula_ros/schema/SRR520.schema.json @@ -3,7 +3,7 @@ "title": "Radar Continental SRR520 parameters.", "type": "object", "definitions": { - "ARS548": { + "SRR520": { "type": "object", "properties": { "interface": { From 925066fa119ccd4ff035d67c54bda99d134cf2c1 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Tue, 11 Jun 2024 11:37:54 +0900 Subject: [PATCH 23/47] chore: typo in the base schema Signed-off-by: Kenzo Lobos-Tsunekawa --- nebula_ros/schema/sub/radar_continental.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_ros/schema/sub/radar_continental.json b/nebula_ros/schema/sub/radar_continental.json index 2ccaff49e..cf499f2ba 100644 --- a/nebula_ros/schema/sub/radar_continental.json +++ b/nebula_ros/schema/sub/radar_continental.json @@ -7,7 +7,7 @@ "$ref": "hardware.json#/definitions/sensor_model", "enum": [ "ARS548", - "SSR520" + "SRR520" ] } } From 777b87769c36a5c07eb26d81f46c5cb5447dfc74 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Tue, 11 Jun 2024 11:41:23 +0900 Subject: [PATCH 24/47] chore: forgot the bus time in the schema Signed-off-by: Kenzo Lobos-Tsunekawa --- nebula_ros/schema/sub/topic.json | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/nebula_ros/schema/sub/topic.json b/nebula_ros/schema/sub/topic.json index b3aa90dd0..a238c06a5 100644 --- a/nebula_ros/schema/sub/topic.json +++ b/nebula_ros/schema/sub/topic.json @@ -34,6 +34,12 @@ "readOnly": true, "description": "Tracked objects frame." }, + "use_bus_time": { + "type": "boolean", + "default": false, + "readOnly": true, + "description": "Use bus time for published sensor data." + }, "use_sensor_time": { "type": "boolean", "default": false, From c3e6ea1c2fc761b9e14fd77c20a99e12553e81fb Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Tue, 11 Jun 2024 11:53:04 +0900 Subject: [PATCH 25/47] chore: deleted unused methods Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental_ars548_hw_interface.hpp | 4 ---- .../continental_srr520_hw_interface.hpp | 4 ---- 2 files changed, 8 deletions(-) diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp index ed578b66c..8e69d66b2 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp @@ -133,10 +133,6 @@ class ContinentalARS548HwInterface /// @return Resulting status Status SetYawRate(float yaw_rate); - /// @brief Checking the current settings and changing the difference point - /// @return Resulting status - Status CheckAndSetConfig(); - /// @brief Setting rclcpp::Logger /// @param node Logger void SetLogger(std::shared_ptr node); diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp index e40cfa37a..b42a8b7dd 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp @@ -98,10 +98,6 @@ class ContinentalSRR520HwInterface float longitudinal_acceleration, float lateral_acceleration, float yaw_rate, float longitudinal_velocity, bool standstill); - /// @brief Checking the current settings and changing the difference point - /// @return Resulting status - Status CheckAndSetConfig(); - /// @brief Setting rclcpp::Logger /// @param node Logger void SetLogger(std::shared_ptr node); From 54b75c465c78af05d71bceeb3b20a46631db0da3 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Tue, 11 Jun 2024 12:00:44 +0900 Subject: [PATCH 26/47] chore: changed variable name from vambig -> v_ambiguous Signed-off-by: Kenzo Lobos-Tsunekawa --- .cspell.json | 1 - .../nebula_common/continental/continental_srr520.hpp | 2 +- .../decoders/continental_srr520_decoder.cpp | 8 ++++---- .../msg/ContinentalSrr520DetectionList.msg | 2 +- .../srr520/1708578209_45566935_near_detection.yaml | 2 +- .../srr520/1708578209_76111685_hrr_detection.yaml | 2 +- 6 files changed, 8 insertions(+), 9 deletions(-) diff --git a/.cspell.json b/.cspell.json index fc25bef16..e83fb6742 100644 --- a/.cspell.json +++ b/.cspell.json @@ -47,7 +47,6 @@ "structs", "UDP_SEQ", "usec", - "vambig", "vccint", "Vccint", "Vdat", diff --git a/nebula_common/include/nebula_common/continental/continental_srr520.hpp b/nebula_common/include/nebula_common/continental/continental_srr520.hpp index e29ecae08..aee19625b 100644 --- a/nebula_common/include/nebula_common/continental/continental_srr520.hpp +++ b/nebula_common/include/nebula_common/continental/continental_srr520.hpp @@ -157,7 +157,7 @@ struct ScanHeaderPacket uint8_t u_signal_status; // 13 uint8_t u_sequence_counter; // 14 big_uint32_buf_t u_cycle_counter; // 15 - big_uint16_buf_t u_vambig; // 19. cSpell:ignore vambig + big_uint16_buf_t u_v_ambiguous; // 19 big_uint16_buf_t u_max_range; // 21 big_uint16_buf_t u_number_of_detections; // 23 uint8_t reserved[7]; // 25 diff --git a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp index 75658539e..46015ece8 100644 --- a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp @@ -213,8 +213,8 @@ void ContinentalSRR520Decoder::ProcessNearHeaderPacket( near_detection_list_ptr_->signal_status = rdi_near_header_packet_.u_signal_status; near_detection_list_ptr_->sequence_counter = rdi_near_header_packet_.u_sequence_counter; near_detection_list_ptr_->cycle_counter = rdi_near_header_packet_.u_cycle_counter.value(); - near_detection_list_ptr_->vambig = - 0.003051851f * rdi_near_header_packet_.u_vambig.value() - 100.f; // cSpell:ignore vambig + near_detection_list_ptr_->v_ambiguous = + 0.003051851f * rdi_near_header_packet_.u_v_ambiguous.value() - 100.f; near_detection_list_ptr_->max_range = 0.1f * rdi_near_header_packet_.u_max_range.value(); near_detection_list_ptr_->detections.reserve( @@ -332,8 +332,8 @@ void ContinentalSRR520Decoder::ProcessHRRHeaderPacket( hrr_detection_list_ptr_->signal_status = rdi_hrr_header_packet_.u_signal_status; hrr_detection_list_ptr_->sequence_counter = rdi_hrr_header_packet_.u_sequence_counter; hrr_detection_list_ptr_->cycle_counter = rdi_hrr_header_packet_.u_cycle_counter.value(); - hrr_detection_list_ptr_->vambig = - 0.003051851f * rdi_hrr_header_packet_.u_vambig.value() - 100.f; // cSpell:ignore vambig + hrr_detection_list_ptr_->v_ambiguous = + 0.003051851f * rdi_hrr_header_packet_.u_v_ambiguous.value() - 100.f; hrr_detection_list_ptr_->max_range = 0.1f * rdi_hrr_header_packet_.u_max_range.value(); hrr_detection_list_ptr_->detections.reserve( diff --git a/nebula_messages/continental_msgs/msg/ContinentalSrr520DetectionList.msg b/nebula_messages/continental_msgs/msg/ContinentalSrr520DetectionList.msg index e2c159901..2bd8f85be 100644 --- a/nebula_messages/continental_msgs/msg/ContinentalSrr520DetectionList.msg +++ b/nebula_messages/continental_msgs/msg/ContinentalSrr520DetectionList.msg @@ -4,7 +4,7 @@ uint8 global_time_stamp_sync_status uint8 signal_status uint8 sequence_counter uint32 cycle_counter -float32 vambig # cSpell:ignore vambig +float32 v_ambiguous float32 max_range ContinentalSrr520Detection[] detections diff --git a/nebula_tests/data/continental/srr520/1708578209_45566935_near_detection.yaml b/nebula_tests/data/continental/srr520/1708578209_45566935_near_detection.yaml index 40e4a50b1..10b233b89 100644 --- a/nebula_tests/data/continental/srr520/1708578209_45566935_near_detection.yaml +++ b/nebula_tests/data/continental/srr520/1708578209_45566935_near_detection.yaml @@ -8,7 +8,7 @@ global_time_stamp_sync_status: 1 signal_status: 1 sequence_counter: 195 cycle_counter: 909 -vambig: 21.4454 +v_ambiguous: 21.4454 max_range: 0.00000 detections: - range: 4.81074 diff --git a/nebula_tests/data/continental/srr520/1708578209_76111685_hrr_detection.yaml b/nebula_tests/data/continental/srr520/1708578209_76111685_hrr_detection.yaml index 34339e3c8..1becd4f61 100644 --- a/nebula_tests/data/continental/srr520/1708578209_76111685_hrr_detection.yaml +++ b/nebula_tests/data/continental/srr520/1708578209_76111685_hrr_detection.yaml @@ -8,7 +8,7 @@ global_time_stamp_sync_status: 1 signal_status: 1 sequence_counter: 196 cycle_counter: 910 -vambig: 23.3772 +v_ambiguous: 23.3772 max_range: 41.3000 detections: - range: 7.76557 From dc457112cd3ae7488289021fc6cd818461f9b866 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos Tsunekawa Date: Tue, 11 Jun 2024 13:16:16 +0900 Subject: [PATCH 27/47] Update nebula_common/include/nebula_common/continental/continental_srr520.hpp Co-authored-by: David Wong <33114676+drwnz@users.noreply.github.com> --- .../include/nebula_common/continental/continental_srr520.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_common/include/nebula_common/continental/continental_srr520.hpp b/nebula_common/include/nebula_common/continental/continental_srr520.hpp index aee19625b..d8b48ccf3 100644 --- a/nebula_common/include/nebula_common/continental/continental_srr520.hpp +++ b/nebula_common/include/nebula_common/continental/continental_srr520.hpp @@ -84,7 +84,7 @@ constexpr int NEAR_CRC_ID = 0; constexpr int HRR_CRC_ID = 1; constexpr int OBJECT_CRC_ID = 2; constexpr int TIME_DOMAIN_ID = - 0; // For details, please refer to autosar's "Time Synchronization over CAN" document + 0; // For details, please refer to AUTOSAR's "Time Synchronization over CAN" document constexpr int RDI_NEAR_HEADER_PACKET_SIZE = 32; constexpr int RDI_NEAR_ELEMENT_PACKET_SIZE = 64; From 3cedea4fb09afa72bb9779ac611a262eec3f8d53 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos Tsunekawa Date: Tue, 11 Jun 2024 13:16:26 +0900 Subject: [PATCH 28/47] Update nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp Co-authored-by: David Wong <33114676+drwnz@users.noreply.github.com> --- .../continental_srr520_hw_interface.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp index b42a8b7dd..3bc0279d8 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp @@ -73,7 +73,7 @@ class ContinentalSRR520HwInterface /// @brief Configure the sensor /// @param sensor_id Desired sensor id /// @param longitudinal_autosar Desired longitudinal value in autosar coordinates - /// @param lateral_autosar Desired lateral value in autosar coordinates + /// @param lateral_autosar Desired lateral value in AUTOSAR coordinates /// @param vertical_autosar Desired vertical value in autosar coordinates /// @param yaw_autosar Desired yaw value in autosar coordinates /// @param longitudinal_cog Desired longitudinal cog From b50cc28636ec9d8597622ec54da35fe29439b827 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Tue, 11 Jun 2024 14:51:11 +0900 Subject: [PATCH 29/47] chore: changed magic numbers for constexprs Signed-off-by: Kenzo Lobos-Tsunekawa --- .../decoders/continental_srr520_decoder.cpp | 158 +++++++++++++----- 1 file changed, 113 insertions(+), 45 deletions(-) diff --git a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp index 46015ece8..b8a2c912c 100644 --- a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp @@ -179,6 +179,10 @@ bool ContinentalSRR520Decoder::ProcessPacket( void ContinentalSRR520Decoder::ProcessNearHeaderPacket( std::unique_ptr packet_msg) { + constexpr float V_AMBIGUOUS_RESOLUTION = 0.003051851f; + constexpr float V_AMBIGUOUS_MIN_VALUE = -100.f; + constexpr float MAX_RANGE_RESOLUTION = 0.1f; + first_rdi_near_packet_ = false; static_assert(sizeof(ScanHeaderPacket) == RDI_NEAR_HEADER_PACKET_SIZE); @@ -214,8 +218,9 @@ void ContinentalSRR520Decoder::ProcessNearHeaderPacket( near_detection_list_ptr_->sequence_counter = rdi_near_header_packet_.u_sequence_counter; near_detection_list_ptr_->cycle_counter = rdi_near_header_packet_.u_cycle_counter.value(); near_detection_list_ptr_->v_ambiguous = - 0.003051851f * rdi_near_header_packet_.u_v_ambiguous.value() - 100.f; - near_detection_list_ptr_->max_range = 0.1f * rdi_near_header_packet_.u_max_range.value(); + V_AMBIGUOUS_RESOLUTION * rdi_near_header_packet_.u_v_ambiguous.value() + V_AMBIGUOUS_MIN_VALUE; + near_detection_list_ptr_->max_range = + MAX_RANGE_RESOLUTION * rdi_near_header_packet_.u_max_range.value(); near_detection_list_ptr_->detections.reserve( rdi_near_header_packet_.u_number_of_detections.value()); @@ -226,6 +231,17 @@ void ContinentalSRR520Decoder::ProcessNearHeaderPacket( void ContinentalSRR520Decoder::ProcessNearElementPacket( std::unique_ptr packet_msg) { + constexpr auto RANGE_RESOLUTION = 0.024420024; + constexpr auto AZIMUTH_RESOLUTION = 0.006159986; + constexpr auto RANGE_RATE_RESOLUTION = 0.014662757; + constexpr auto RCS_RESOLUTION = 0.476190476; + constexpr auto SNR_RESOLUTION = 1.7; + + constexpr auto AZIMUTH_MIN_VALUE = -1.570796327; + constexpr auto RANGE_RATE_MIN_VALUE = -15.f; + constexpr auto RCS_MIN_VALUE = -40.f; + constexpr auto SNR_MIN_VALUE = 11.f; + if (rdi_near_packets_ptr_->packets.size() == 0) { if (!first_rdi_near_packet_) { PrintError("Near element before header. This can happen during the first iteration"); @@ -264,21 +280,21 @@ void ContinentalSRR520Decoder::ProcessNearElementPacket( uint16_t u_range = (static_cast(data[0]) << 4) | (static_cast(data[1] & 0xF0) >> 4); assert(u_range <= 4095); - detection_msg.range = 0.024420024 * u_range; + detection_msg.range = RANGE_RESOLUTION * u_range; uint16_t u_azimuth = (static_cast(data[1] & 0x0f) << 5) | (static_cast(data[2] & 0xF8) >> 3); assert(u_azimuth <= 510); - detection_msg.azimuth_angle = 0.006159986 * u_azimuth - 1.570796327; + detection_msg.azimuth_angle = AZIMUTH_RESOLUTION * u_azimuth + AZIMUTH_MIN_VALUE; uint16_t u_range_rate = (static_cast(data[2] & 0x07) << 8) | static_cast(data[3]); assert(u_range_rate <= 2046); - detection_msg.range_rate = 0.014662757 * u_range_rate - 15.f; + detection_msg.range_rate = RANGE_RATE_RESOLUTION * u_range_rate + RANGE_RATE_MIN_VALUE; uint16_t u_rcs = (data[4] & 0xFE) >> 1; assert(u_rcs <= 126); - detection_msg.rcs = 0.476190476 * u_rcs - 40.f; + detection_msg.rcs = RCS_RESOLUTION * u_rcs + RCS_MIN_VALUE; detection_msg.pdh00 = 100 * (data[4] & 0x01); detection_msg.pdh01 = 100 * ((data[5] & 0x80) >> 7); @@ -287,7 +303,7 @@ void ContinentalSRR520Decoder::ProcessNearElementPacket( detection_msg.pdh04 = 100 * ((data[5] & 0x10) >> 4); uint8_t u_snr = data[5] & 0x0f; - detection_msg.snr = 1.7 * u_snr + 11.f; + detection_msg.snr = SNR_RESOLUTION * u_snr + SNR_MIN_VALUE; near_detection_list_ptr_->detections.push_back(detection_msg); parsed_detections++; @@ -299,6 +315,10 @@ void ContinentalSRR520Decoder::ProcessNearElementPacket( void ContinentalSRR520Decoder::ProcessHRRHeaderPacket( std::unique_ptr packet_msg) { + constexpr float V_AMBIGUOUS_RESOLUTION = 0.003051851f; + constexpr float V_AMBIGUOUS_MIN_VALUE = -100.f; + constexpr float MAX_RANGE_RESOLUTION = 0.1f; + first_rdi_hrr_packet_ = false; static_assert(sizeof(ScanHeaderPacket) == RDI_HRR_HEADER_PACKET_SIZE); @@ -333,8 +353,9 @@ void ContinentalSRR520Decoder::ProcessHRRHeaderPacket( hrr_detection_list_ptr_->sequence_counter = rdi_hrr_header_packet_.u_sequence_counter; hrr_detection_list_ptr_->cycle_counter = rdi_hrr_header_packet_.u_cycle_counter.value(); hrr_detection_list_ptr_->v_ambiguous = - 0.003051851f * rdi_hrr_header_packet_.u_v_ambiguous.value() - 100.f; - hrr_detection_list_ptr_->max_range = 0.1f * rdi_hrr_header_packet_.u_max_range.value(); + V_AMBIGUOUS_RESOLUTION * rdi_hrr_header_packet_.u_v_ambiguous.value() + V_AMBIGUOUS_MIN_VALUE; + hrr_detection_list_ptr_->max_range = + MAX_RANGE_RESOLUTION * rdi_hrr_header_packet_.u_max_range.value(); hrr_detection_list_ptr_->detections.reserve( rdi_hrr_header_packet_.u_number_of_detections.value()); @@ -345,6 +366,17 @@ void ContinentalSRR520Decoder::ProcessHRRHeaderPacket( void ContinentalSRR520Decoder::ProcessHRRElementPacket( std::unique_ptr packet_msg) { + constexpr auto RANGE_RESOLUTION = 0.024420024; + constexpr auto AZIMUTH_RESOLUTION = 0.006159986; + constexpr auto RANGE_RATE_RESOLUTION = 0.014662757; + constexpr auto RCS_RESOLUTION = 0.476190476; + constexpr auto SNR_RESOLUTION = 1.7f; + + constexpr auto AZIMUTH_MIN_VALUE = -1.570796327; + constexpr auto RANGE_RATE_MIN_VALUE = -15.f; + constexpr auto RCS_MIN_VALUE = -40.f; + constexpr auto SNR_MIN_VALUE = 11.f; + if (rdi_hrr_packets_ptr_->packets.size() == 0) { if (!first_rdi_hrr_packet_) { PrintError("HRR element before header. This can happen during the first iteration"); @@ -383,21 +415,21 @@ void ContinentalSRR520Decoder::ProcessHRRElementPacket( uint16_t u_range = (static_cast(data[0]) << 4) | (static_cast(data[1] & 0xF0) >> 4); assert(u_range <= 4095); - detection_msg.range = 0.024420024 * u_range; + detection_msg.range = RANGE_RESOLUTION * u_range; uint16_t u_azimuth = (static_cast(data[1] & 0x0f) << 5) | (static_cast(data[2] & 0xF8) >> 3); assert(u_azimuth <= 510); - detection_msg.azimuth_angle = 0.006159986 * u_azimuth - 1.570796327; + detection_msg.azimuth_angle = AZIMUTH_RESOLUTION * u_azimuth + AZIMUTH_MIN_VALUE; uint16_t u_range_rate = (static_cast(data[2] & 0x07) << 8) | static_cast(data[3]); assert(u_range_rate <= 2046); - detection_msg.range_rate = 0.014662757 * u_range_rate - 15.f; + detection_msg.range_rate = RANGE_RATE_RESOLUTION * u_range_rate + RANGE_RATE_MIN_VALUE; uint16_t u_rcs = (data[4] & 0xFE) >> 1; assert(u_rcs <= 126); - detection_msg.rcs = 0.476190476 * u_rcs - 40.f; + detection_msg.rcs = RCS_RESOLUTION * u_rcs + RCS_MIN_VALUE; detection_msg.pdh00 = 100 * (data[4] & 0x01); detection_msg.pdh01 = 100 * ((data[5] & 0x80) >> 7); @@ -406,7 +438,7 @@ void ContinentalSRR520Decoder::ProcessHRRElementPacket( detection_msg.pdh04 = 100 * ((data[5] & 0x10) >> 4); uint8_t u_snr = data[5] & 0x0f; - detection_msg.snr = 1.7 * u_snr + 11.f; + detection_msg.snr = SNR_RESOLUTION * u_snr + SNR_MIN_VALUE; hrr_detection_list_ptr_->detections.push_back(detection_msg); parsed_detections++; @@ -418,6 +450,11 @@ void ContinentalSRR520Decoder::ProcessHRRElementPacket( void ContinentalSRR520Decoder::ProcessObjectHeaderPacket( std::unique_ptr packet_msg) { + constexpr auto VX_RESOLUTION = 0.003051851; + constexpr auto VX_MIN_VALUE = -100.f; + constexpr auto YAW_RATE_RESOLUTION = 9.58766e-05; + constexpr auto YAW_RATE_MIN_VALUE = -3.14159; + first_object_packet_ = false; static_assert(sizeof(ObjectHeaderPacket) == OBJECT_HEADER_PACKET_SIZE); @@ -449,9 +486,9 @@ void ContinentalSRR520Decoder::ProcessObjectHeaderPacket( object_list_ptr_->signal_status = object_header_packet_.u_signal_status; object_list_ptr_->sequence_counter = object_header_packet_.u_sequence_counter; object_list_ptr_->cycle_counter = object_header_packet_.u_cycle_counter.value(); - object_list_ptr_->ego_vx = 0.003051851 * object_header_packet_.u_ego_vx.value() - 100.f; + object_list_ptr_->ego_vx = VX_RESOLUTION * object_header_packet_.u_ego_vx.value() + VX_MIN_VALUE; object_list_ptr_->ego_yaw_rate = - 9.58766e-05 * object_header_packet_.u_ego_yaw_rate.value() - 3.14159; + YAW_RATE_RESOLUTION * object_header_packet_.u_ego_yaw_rate.value() + YAW_RATE_MIN_VALUE; object_list_ptr_->motion_type = object_header_packet_.u_motion_type; object_list_ptr_->objects.reserve(object_header_packet_.u_number_of_objects); @@ -462,6 +499,23 @@ void ContinentalSRR520Decoder::ProcessObjectHeaderPacket( void ContinentalSRR520Decoder::ProcessObjectElementPacket( std::unique_ptr packet_msg) { + constexpr auto DIST_RESOLUTION = 0.009155553; + constexpr auto V_ABS_RESOLUTION = 0.009156391; + constexpr auto A_ABS_RESOLUTION = 0.019569472; + constexpr auto DIST_STD_RESOLUTION = 0.001831166; + constexpr auto V_ABS_STD_RESOLUTION = 0.001831166; + constexpr auto A_ABS_STD_RESOLUTION = 0.001831166; + constexpr auto OBJECT_BOX_RESOLUTION = 0.007326007; + constexpr auto OBJECT_ORIENTATION_RESOLUTION = 0.001534729; + constexpr auto OBJECT_RCS_RESOLUTION = 0.024425989; + constexpr auto OBJECT_SCORE_RESOLUTION = 6.666666667; + + constexpr auto DIST_MIN_VALUE = -300.f; + constexpr auto V_ABS_MIN_VALUE = -75.f; + constexpr auto A_ABS_MIN_VALUE = -10.f; + constexpr auto OBJECT_ORIENTATION_MIN_VALUE = -3.14159; + constexpr auto OBJECT_RCS_MIN_VALUE = -50.f; + if (object_packets_ptr_->packets.size() == 0) { if (!first_object_packet_) { PrintError("Object element before header. This can happen during the first iteration"); @@ -498,86 +552,87 @@ void ContinentalSRR520Decoder::ProcessObjectElementPacket( uint16_t u_dist_y = ((static_cast(data[3]) << 8) | data[4]); assert(u_dist_x <= 65534); assert(u_dist_y <= 65534); - object_msg.dist_x = 0.009155553 * u_dist_x - 300.f; - object_msg.dist_y = 0.009155553 * u_dist_y - 300.f; + object_msg.dist_x = DIST_RESOLUTION * u_dist_x + DIST_MIN_VALUE; + object_msg.dist_y = DIST_RESOLUTION * u_dist_y + DIST_MIN_VALUE; uint16_t u_v_abs_x = (static_cast(data[5]) << 6) | (static_cast(data[6] & 0xfc) >> 2); assert(u_v_abs_x <= 16382); - object_msg.v_abs_x = 0.009156391 * u_v_abs_x - 75.f; + object_msg.v_abs_x = V_ABS_RESOLUTION * u_v_abs_x + V_ABS_MIN_VALUE; uint16_t u_v_abs_y = (static_cast(data[6] & 0x03) << 12) | (static_cast(data[7]) << 4) | (static_cast(data[8] & 0xF0) >> 4); assert(u_v_abs_y <= 16382); - object_msg.v_abs_y = 0.009156391 * u_v_abs_y - 75.f; + object_msg.v_abs_y = V_ABS_RESOLUTION * u_v_abs_y + V_ABS_MIN_VALUE; uint16_t u_a_abs_x = (static_cast(data[8] & 0x0f) << 6) | (static_cast(data[9] & 0xfc) >> 2); assert(u_a_abs_x <= 1022); - object_msg.a_abs_x = 0.019569472 * u_a_abs_x - 10.f; + object_msg.a_abs_x = A_ABS_RESOLUTION * u_a_abs_x + A_ABS_MIN_VALUE; uint16_t u_a_abs_y = (static_cast(data[9] & 0x03) << 8) | static_cast(data[10]); assert(u_a_abs_y <= 1022); - object_msg.a_abs_y = 0.019569472 * u_a_abs_y - 10.f; + object_msg.a_abs_y = A_ABS_RESOLUTION * u_a_abs_y + A_ABS_MIN_VALUE; uint16_t u_dist_x_std = (static_cast(data[11]) << 6) | (static_cast(data[12] & 0xfc) >> 2); assert(u_dist_x_std <= 16383); - object_msg.dist_x_std = 0.001831166 * u_dist_x_std; + object_msg.dist_x_std = DIST_STD_RESOLUTION * u_dist_x_std; uint16_t u_dist_y_std = (static_cast(data[12] & 0x03) << 12) | (static_cast(data[13]) << 4) | (static_cast(data[14] & 0xF0) >> 4); assert(u_dist_y_std <= 16383); - object_msg.dist_y_std = 0.001831166 * u_dist_y_std; + object_msg.dist_y_std = DIST_STD_RESOLUTION * u_dist_y_std; uint16_t u_v_abs_x_std = (static_cast(data[14] & 0x0f) << 10) | (static_cast(data[15]) << 2) | (static_cast(data[15] & 0x03) >> 6); assert(u_v_abs_x_std <= 16383); - object_msg.v_abs_x_std = 0.001831166 * u_v_abs_x_std; + object_msg.v_abs_x_std = V_ABS_STD_RESOLUTION * u_v_abs_x_std; uint16_t u_v_abs_y_std = (static_cast(data[16] & 0x3f) << 8) | static_cast(data[17]); assert(u_v_abs_y_std <= 16383); - object_msg.v_abs_y_std = 0.001831166 * u_v_abs_y_std; + object_msg.v_abs_y_std = V_ABS_STD_RESOLUTION * u_v_abs_y_std; uint16_t u_a_abs_x_std = (static_cast(data[18]) << 6) | (static_cast(data[19] & 0xfc) >> 2); assert(u_a_abs_x_std <= 16383); - object_msg.a_abs_x_std = 0.001831166 * u_a_abs_x_std; + object_msg.a_abs_x_std = A_ABS_STD_RESOLUTION * u_a_abs_x_std; uint16_t u_a_abs_y_std = (static_cast(data[19] & 0x03) << 12) | (static_cast(data[20]) << 4) | (static_cast(data[21] & 0xF0) >> 4); assert(u_a_abs_y_std <= 16383); - object_msg.a_abs_y_std = 0.001831166 * u_a_abs_y_std; + object_msg.a_abs_y_std = A_ABS_STD_RESOLUTION * u_a_abs_y_std; uint16_t u_box_length = (static_cast(data[21] & 0x0f) << 8) | static_cast(data[22]); assert(u_box_length <= 4095); - object_msg.box_length = 0.007326007 * u_box_length; + object_msg.box_length = OBJECT_BOX_RESOLUTION * u_box_length; uint16_t u_box_width = (static_cast(data[23]) << 4) | (static_cast(data[24] & 0xF0) >> 4); assert(u_box_width <= 4095); - object_msg.box_width = 0.007326007 * u_box_width; + object_msg.box_width = OBJECT_BOX_RESOLUTION * u_box_width; uint16_t u_orientation = (static_cast(data[24] & 0x0f) << 8) | static_cast(data[25]); assert(u_orientation <= 4094); - object_msg.orientation = 0.001534729 * u_orientation - 3.14159; + object_msg.orientation = + OBJECT_ORIENTATION_RESOLUTION * u_orientation + OBJECT_ORIENTATION_MIN_VALUE; uint16_t u_rcs = (static_cast(data[26]) << 4) | (static_cast(data[27] & 0xF0) >> 4); assert(u_rcs <= 4094); - object_msg.rcs = 0.024425989 * u_rcs - 50.f; + object_msg.rcs = OBJECT_RCS_RESOLUTION * u_rcs + OBJECT_RCS_MIN_VALUE; uint8_t u_score = data[27] & 0x0f; assert(u_score <= 15); - object_msg.score = 6.666666667 * u_score; + object_msg.score = OBJECT_SCORE_RESOLUTION * u_score; object_msg.life_cycles = (static_cast(data[28]) << 8) | data[29]; assert(object_msg.life_cycles <= 65535); @@ -740,6 +795,12 @@ void ContinentalSRR520Decoder::ProcessSensorStatusPacket( { static_assert(sizeof(StatusPacket) == STATUS_PACKET_SIZE); + constexpr float STATUS_DISTANCE_RESOLUTION = 1e-3f; + constexpr float STATUS_DISTANCE_MIN_VALUE = -32.767; + constexpr float STATUS_ANGLE_RESOLUTION = 9.58766f; + constexpr float STATUS_ANGLE_MIN_VALUE = -3.14159f; + constexpr auto STATUS_ANGLE_STD_RESOLUTION = 1.52593e-05; + StatusPacket status_packet; std::memcpy(&status_packet, packet_msg->data.data() + 4 * sizeof(uint8_t), sizeof(status_packet)); @@ -791,31 +852,37 @@ void ContinentalSRR520Decoder::ProcessSensorStatusPacket( diagnostic_values.push_back(key_value); key_value.key = "long_pos"; - key_value.value = std::to_string(1e-3 * status_packet.u_long_pos.value() - 32.767); + key_value.value = std::to_string( + STATUS_DISTANCE_RESOLUTION * status_packet.u_long_pos.value() + STATUS_DISTANCE_MIN_VALUE); diagnostic_values.push_back(key_value); key_value.key = "lat_pos"; - key_value.value = std::to_string(1e-3 * status_packet.u_lat_pos.value() - 32.767); + key_value.value = std::to_string( + STATUS_DISTANCE_RESOLUTION * status_packet.u_lat_pos.value() + STATUS_DISTANCE_MIN_VALUE); diagnostic_values.push_back(key_value); key_value.key = "vert_pos"; - key_value.value = std::to_string(1e-3 * status_packet.u_vert_pos.value() - 32.767); + key_value.value = std::to_string( + STATUS_DISTANCE_RESOLUTION * status_packet.u_vert_pos.value() + STATUS_DISTANCE_MIN_VALUE); diagnostic_values.push_back(key_value); key_value.key = "long_pos_cog"; - key_value.value = std::to_string(1e-3 * status_packet.u_long_pos_cog.value() - 32.767); + key_value.value = std::to_string( + STATUS_DISTANCE_RESOLUTION * status_packet.u_long_pos_cog.value() + STATUS_DISTANCE_MIN_VALUE); diagnostic_values.push_back(key_value); key_value.key = "wheelbase"; - key_value.value = std::to_string(1e-3 * status_packet.u_wheelbase.value()); + key_value.value = std::to_string(STATUS_DISTANCE_RESOLUTION * status_packet.u_wheelbase.value()); diagnostic_values.push_back(key_value); key_value.key = "yaw_angle"; - key_value.value = std::to_string(9.58766e-05 * status_packet.u_yaw_angle.value() - 3.14159); + key_value.value = std::to_string( + STATUS_ANGLE_RESOLUTION * status_packet.u_yaw_angle.value() + STATUS_ANGLE_MIN_VALUE); diagnostic_values.push_back(key_value); key_value.key = "cover_damping"; - key_value.value = std::to_string(1e-3 * status_packet.u_cover_damping.value() - 32.767); + key_value.value = std::to_string( + STATUS_DISTANCE_RESOLUTION * status_packet.u_cover_damping.value() + STATUS_DISTANCE_MIN_VALUE); diagnostic_values.push_back(key_value); uint8_t plug_orientation = status_packet.u_plug_orientation & 0b1; @@ -1052,17 +1119,18 @@ void ContinentalSRR520Decoder::ProcessSensorStatusPacket( diagnostic_values.push_back(key_value); key_value.key = "aln_current_azimuth_std"; - key_value.value = std::to_string(1.52593e-05 * status_packet.u_aln_current_azimuth_std.value()); + key_value.value = + std::to_string(STATUS_ANGLE_STD_RESOLUTION * status_packet.u_aln_current_azimuth_std.value()); diagnostic_values.push_back(key_value); key_value.key = "aln_current_azimuth"; - key_value.value = - std::to_string(9.58766e-05 * status_packet.u_aln_current_azimuth.value() - 3.14159); + key_value.value = std::to_string( + STATUS_ANGLE_RESOLUTION * status_packet.u_aln_current_azimuth.value() + STATUS_ANGLE_MIN_VALUE); diagnostic_values.push_back(key_value); key_value.key = "aln_current_delta"; - key_value.value = - std::to_string(9.58766e-05 * status_packet.u_aln_current_delta.value() - 3.14159); + key_value.value = std::to_string( + STATUS_ANGLE_RESOLUTION * status_packet.u_aln_current_delta.value() + STATUS_ANGLE_MIN_VALUE); diagnostic_values.push_back(key_value); uint16_t computed_crc = crc16_packet(packet_msg->data.begin() + 4, packet_msg->data.end() - 3); From b7439445d61416a63125f539b36ee2399842d19c Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Tue, 11 Jun 2024 14:56:57 +0900 Subject: [PATCH 30/47] chore: removed pandar mention in continental packages and changed callback name to be consistent among continental sensors Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental_ars548_hw_interface.hpp | 2 +- .../continental_srr520_hw_interface.hpp | 4 ++-- .../continental_ars548_hw_interface.cpp | 2 +- nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp index 8e69d66b2..605fe8fe0 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp @@ -56,7 +56,7 @@ class ContinentalARS548HwInterface /// @brief Registering callback /// @param callback Callback function /// @return Resulting status - Status RegisterCallback( + Status RegisterPacketCallback( std::function)> packet_callback); /// @brief Set the sensor mounting parameters diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp index 3bc0279d8..7eba34ee0 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp @@ -58,8 +58,8 @@ class ContinentalSRR520HwInterface const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration> new_config_ptr); - /// @brief Registering callback for PandarScan - /// @param scan_callback Callback function + /// @brief Registering callback + /// @param callback Callback function /// @return Resulting status Status RegisterPacketCallback( std::function)> packet_callback); diff --git a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp index bf05631b5..b115ea637 100644 --- a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp @@ -66,7 +66,7 @@ Status ContinentalARS548HwInterface::SensorInterfaceStart() return Status::OK; } -Status ContinentalARS548HwInterface::RegisterCallback( +Status ContinentalARS548HwInterface::RegisterPacketCallback( std::function)> packet_callback) { packet_callback_ = std::move(packet_callback); diff --git a/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp index dd78b95d0..5713caf99 100644 --- a/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp @@ -54,7 +54,7 @@ ContinentalARS548RosWrapper::ContinentalARS548RosWrapper(const rclcpp::NodeOptio }); if (launch_hw_) { - hw_interface_wrapper_->HwInterface()->RegisterCallback( + hw_interface_wrapper_->HwInterface()->RegisterPacketCallback( std::bind(&ContinentalARS548RosWrapper::ReceivePacketCallback, this, std::placeholders::_1)); StreamStart(); } else { From 9a141a3fd55b218f0ea8da4c25aa3a5602454daa Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Tue, 11 Jun 2024 14:59:43 +0900 Subject: [PATCH 31/47] chore: capitalized acronyms Signed-off-by: Kenzo Lobos-Tsunekawa --- .../decoders/continental_srr520_decoder.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp index 535ef1144..646882c6b 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp @@ -53,14 +53,14 @@ class ContinentalSRR520Decoder : public ContinentalPacketsDecoder /// @return Resulting flag bool ProcessPacket(std::unique_ptr packet_msg) override; - /// @brief Register function to call whenever a new rdi near detection list is processed + /// @brief Register function to call whenever a new RDI near detection list is processed /// @param detection_list_callback /// @return Resulting status Status RegisterNearDetectionListCallback( std::function)> detection_list_callback); - /// @brief Register function to call whenever a new rdi hrr detection list is processed + /// @brief Register function to call whenever a new RDI HRR detection list is processed /// @param detection_list_callback /// @return Resulting status Status RegisterHRRDetectionListCallback( From 46dde25f3a0e8fcf46c1995d3526283dcccb9178 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Tue, 11 Jun 2024 15:07:20 +0900 Subject: [PATCH 32/47] chore: changed fup for follow up Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental/continental_srr520.hpp | 4 +-- .../decoders/continental_srr520_decoder.hpp | 12 +++---- .../decoders/continental_srr520_decoder.cpp | 20 +++++------ .../continental_srr520_hw_interface.hpp | 6 ++-- .../continental_srr520_hw_interface.cpp | 34 ++++++++++--------- .../continental_srr520_decoder_wrapper.hpp | 4 +-- .../continental_srr520_decoder_wrapper.cpp | 8 ++--- 7 files changed, 45 insertions(+), 43 deletions(-) diff --git a/nebula_common/include/nebula_common/continental/continental_srr520.hpp b/nebula_common/include/nebula_common/continental/continental_srr520.hpp index d8b48ccf3..f5f254f4f 100644 --- a/nebula_common/include/nebula_common/continental/continental_srr520.hpp +++ b/nebula_common/include/nebula_common/continental/continental_srr520.hpp @@ -75,7 +75,7 @@ constexpr int OBJECT_HEADER_CAN_MESSAGE_ID = 1200; constexpr int OBJECT_CAN_MESSAGE_ID = 1201; constexpr int CRC_LIST_CAN_MESSAGE_ID = 800; constexpr int STATUS_CAN_MESSAGE_ID = 700; -constexpr int SYNC_FUP_CAN_MESSAGE_ID = 53; +constexpr int SYNC_FOLLOW_UP_CAN_MESSAGE_ID = 53; constexpr int VEH_DYN_CAN_MESSAGE_ID = 600; constexpr int SENSOR_CONFIG_CAN_MESSAGE_ID = 601; @@ -94,7 +94,7 @@ constexpr int OBJECT_HEADER_PACKET_SIZE = 32; constexpr int OBJECT_PACKET_SIZE = 64; constexpr int CRC_LIST_PACKET_SIZE = 4; constexpr int STATUS_PACKET_SIZE = 64; -constexpr int SYNC_FUP_CAN_PACKET_SIZE = 8; +constexpr int SYNC_FOLLOW_UP_CAN_PACKET_SIZE = 8; constexpr int VEH_DYN_CAN_PACKET_SIZE = 8; constexpr int CONFIGURATION_PACKET_SIZE = 16; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp index 646882c6b..a3dfd07b9 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp @@ -80,11 +80,11 @@ class ContinentalSRR520Decoder : public ContinentalPacketsDecoder Status RegisterStatusCallback( std::function)> status_callback); - /// @brief Register function to call whenever a sync fup packet is processed - /// @param sync_fup_callback + /// @brief Register function to call whenever a sync follow-up packet is processed + /// @param sync_follow_up_callback /// @return Resulting status - Status RegisterSyncFupCallback( - std::function sync_fup_callback); + Status RegisterSyncFollowUpCallback( + std::function sync_follow_up_callback); /// @brief Register function to call whenever enough packets have been processed /// @param object_list_callback @@ -156,7 +156,7 @@ class ContinentalSRR520Decoder : public ContinentalPacketsDecoder /// @brief Process a new sensor status packet /// @param buffer The buffer containing the status packet /// @param stamp The stamp in nanoseconds - void ProcessSyncFupPacket(std::unique_ptr packet_msg); + void ProcessSyncFollowUpPacket(std::unique_ptr packet_msg); /// @brief Printing the string to RCLCPP_INFO_STREAM /// @param info Target string @@ -178,7 +178,7 @@ class ContinentalSRR520Decoder : public ContinentalPacketsDecoder object_list_callback_{}; std::function msg)> status_callback_{}; - std::function sync_fup_callback_{}; + std::function sync_follow_up_callback_{}; std::function msg)> nebula_packets_callback_{}; diff --git a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp index b8a2c912c..d5c2d8c83 100644 --- a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp @@ -83,10 +83,10 @@ Status ContinentalSRR520Decoder::RegisterStatusCallback( return Status::OK; } -Status ContinentalSRR520Decoder::RegisterSyncFupCallback( - std::function sync_fup_callback) +Status ContinentalSRR520Decoder::RegisterSyncFollowUpCallback( + std::function sync_follow_up_callback) { - sync_fup_callback_ = std::move(sync_fup_callback); + sync_follow_up_callback_ = std::move(sync_follow_up_callback); return Status::OK; } @@ -160,13 +160,13 @@ bool ContinentalSRR520Decoder::ProcessPacket( } ProcessSensorStatusPacket(std::move(packet_msg)); - } else if (can_message_id == SYNC_FUP_CAN_MESSAGE_ID) { - if (payload_size != SYNC_FUP_CAN_PACKET_SIZE) { - PrintError("SYNC_FUP_CAN_MESSAGE_ID message with invalid size"); + } else if (can_message_id == SYNC_FOLLOW_UP_CAN_MESSAGE_ID) { + if (payload_size != SYNC_FOLLOW_UP_CAN_PACKET_SIZE) { + PrintError("SYNC_FOLLOW_UP_CAN_MESSAGE_ID message with invalid size"); return false; } - ProcessSyncFupPacket(std::move(packet_msg)); + ProcessSyncFollowUpPacket(std::move(packet_msg)); } else if ( can_message_id != VEH_DYN_CAN_MESSAGE_ID && can_message_id != SENSOR_CONFIG_CAN_MESSAGE_ID) { PrintError("Unrecognized message ID=" + std::to_string(can_message_id)); @@ -1157,11 +1157,11 @@ void ContinentalSRR520Decoder::ProcessSensorStatusPacket( } } -void ContinentalSRR520Decoder::ProcessSyncFupPacket( +void ContinentalSRR520Decoder::ProcessSyncFollowUpPacket( std::unique_ptr packet_msg) { - if (sync_fup_callback_) { - sync_fup_callback_(packet_msg->stamp); + if (sync_follow_up_callback_) { + sync_follow_up_callback_(packet_msg->stamp); } auto nebula_packets = std::make_unique(); diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp index 7eba34ee0..df3f28bb0 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp @@ -67,8 +67,8 @@ class ContinentalSRR520HwInterface /// @brief Sensor synchronization routine void SensorSync(); - /// @brief Process a new Sync Fup request - void SensorSyncFup(builtin_interfaces::msg::Time stamp); + /// @brief Process a new Sync Follow-up request + void SensorSyncFollowUp(builtin_interfaces::msg::Time stamp); /// @brief Configure the sensor /// @param sensor_id Desired sensor id @@ -135,7 +135,7 @@ class ContinentalSRR520HwInterface bool sensor_interface_active_{}; uint8_t sync_counter_{0}; - bool sync_fup_sent_{true}; + bool sync_follow_up_sent_{true}; builtin_interfaces::msg::Time last_sync_stamp_; std::shared_ptr parent_node_logger_ptr_; diff --git a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp index 16ae1962e..d019fdf5d 100644 --- a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp @@ -144,13 +144,13 @@ Status ContinentalSRR520HwInterface::RegisterPacketCallback( return Status::OK; } -void ContinentalSRR520HwInterface::SensorSyncFup(builtin_interfaces::msg::Time stamp) +void ContinentalSRR520HwInterface::SensorSyncFollowUp(builtin_interfaces::msg::Time stamp) { if (!can_sender_ptr_) { PrintError("Can sender is invalid so can not do follow up"); } - if (!config_ptr_->sync_use_bus_time || sync_fup_sent_) { + if (!config_ptr_->sync_use_bus_time || sync_follow_up_sent_) { return; } @@ -172,13 +172,14 @@ void ContinentalSRR520HwInterface::SensorSyncFup(builtin_interfaces::msg::Time s data[6] = (t4r_nanoseconds & 0x0000FF00) >> 8; data[7] = (t4r_nanoseconds & 0x000000FF) >> 0; - std::array fup_crc_array{data[2], data[3], data[4], data[5], data[6], data[7], 0x00}; - uint8_t fup_crc = crc8h2f(fup_crc_array.begin(), fup_crc_array.end()); - data[1] = fup_crc; + std::array follow_up_crc_array{data[2], data[3], data[4], data[5], + data[6], data[7], 0x00}; + uint8_t follow_up_crc = crc8h2f(follow_up_crc_array.begin(), follow_up_crc_array.end()); + data[1] = follow_up_crc; - SendFrame(data, SYNC_FUP_CAN_MESSAGE_ID); + SendFrame(data, SYNC_FOLLOW_UP_CAN_MESSAGE_ID); - sync_fup_sent_ = true; + sync_follow_up_sent_ = true; sync_counter_ = sync_counter_ == 15 ? 0 : sync_counter_ + 1; } @@ -189,8 +190,8 @@ void ContinentalSRR520HwInterface::SensorSync() return; } - if (!sync_fup_sent_) { - PrintError("We will send a SYNC message without having sent a FUP message first!"); + if (!sync_follow_up_sent_) { + PrintError("We will send a SYNC message without having sent a FollowUp message first!"); } auto now = std::chrono::system_clock::now(); @@ -217,10 +218,10 @@ void ContinentalSRR520HwInterface::SensorSync() uint8_t sync_crc = crc8h2f(sync_crc_array.begin(), sync_crc_array.end()); data[1] = sync_crc; - SendFrame(data, SYNC_FUP_CAN_MESSAGE_ID); + SendFrame(data, SYNC_FOLLOW_UP_CAN_MESSAGE_ID); if (config_ptr_->sync_use_bus_time) { - sync_fup_sent_ = false; + sync_follow_up_sent_ = false; return; } @@ -233,14 +234,15 @@ void ContinentalSRR520HwInterface::SensorSync() data[6] = (stamp.nanosec & 0x0000FF00) >> 8; data[7] = (stamp.nanosec & 0x000000FF) >> 0; - std::array fup_crc_array{data[2], data[3], data[4], data[5], data[6], data[7], 0x00}; - uint8_t fup_crc = crc8h2f(fup_crc_array.begin(), fup_crc_array.end()); - data[1] = fup_crc; + std::array follow_up_crc_array{data[2], data[3], data[4], data[5], + data[6], data[7], 0x00}; + uint8_t follow_up_crc = crc8h2f(follow_up_crc_array.begin(), follow_up_crc_array.end()); + data[1] = follow_up_crc; - SendFrame(data, SYNC_FUP_CAN_MESSAGE_ID); + SendFrame(data, SYNC_FOLLOW_UP_CAN_MESSAGE_ID); sync_counter_ = sync_counter_ == 15 ? 0 : sync_counter_ + 1; - sync_fup_sent_ = true; + sync_follow_up_sent_ = true; } Status ContinentalSRR520HwInterface::SensorInterfaceStop() diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp index 787203d88..89b9759fe 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp @@ -83,8 +83,8 @@ class ContinentalSRR520DecoderWrapper /// @param msg The new DiagnosticArray from the driver void StatusCallback(std::unique_ptr msg); - /// @brief Callback to process a new SyncFup message from the driver - void SyncFupCallback(builtin_interfaces::msg::Time stamp); + /// @brief Callback to process a new SyncFollowUp message from the driver + void SyncFollowUpCallback(builtin_interfaces::msg::Time stamp); /// @brief Callback to process a new NebulaPackets message from the driver /// @param msg The new NebulaPackets from the driver diff --git a/nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp index f74364925..d5355eb69 100644 --- a/nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp +++ b/nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp @@ -114,8 +114,8 @@ Status ContinentalSRR520DecoderWrapper::InitializeDriver( std::bind(&ContinentalSRR520DecoderWrapper::StatusCallback, this, std::placeholders::_1)); if (hw_interface_ptr_) { - driver_ptr_->RegisterSyncFupCallback( - std::bind(&ContinentalSRR520DecoderWrapper::SyncFupCallback, this, std::placeholders::_1)); + driver_ptr_->RegisterSyncFollowUpCallback(std::bind( + &ContinentalSRR520DecoderWrapper::SyncFollowUpCallback, this, std::placeholders::_1)); driver_ptr_->RegisterPacketsCallback( std::bind(&ContinentalSRR520DecoderWrapper::PacketsCallback, this, std::placeholders::_1)); } @@ -574,9 +574,9 @@ visualization_msgs::msg::MarkerArray ContinentalSRR520DecoderWrapper::ConvertToM return marker_array; } -void ContinentalSRR520DecoderWrapper::SyncFupCallback(builtin_interfaces::msg::Time stamp) +void ContinentalSRR520DecoderWrapper::SyncFollowUpCallback(builtin_interfaces::msg::Time stamp) { - hw_interface_ptr_->SensorSyncFup(stamp); + hw_interface_ptr_->SensorSyncFollowUp(stamp); } void ContinentalSRR520DecoderWrapper::PacketsCallback( From 64cd6df9f62d3ca62ea36dc5ef3dd6d680b73fbe Mon Sep 17 00:00:00 2001 From: Kenzo Lobos Tsunekawa Date: Tue, 11 Jun 2024 15:09:59 +0900 Subject: [PATCH 33/47] Update nebula_common/include/nebula_common/continental/crc.hpp Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com> --- nebula_common/include/nebula_common/continental/crc.hpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/nebula_common/include/nebula_common/continental/crc.hpp b/nebula_common/include/nebula_common/continental/crc.hpp index c66d050e1..733f5d0fd 100644 --- a/nebula_common/include/nebula_common/continental/crc.hpp +++ b/nebula_common/include/nebula_common/continental/crc.hpp @@ -12,6 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#pragma once + +#include + template int crc16_packets(Iterator begin, Iterator end, int payload_offset) { From 3dc3bf9bc71e2433bebdc27c6300e8c994672d99 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Tue, 11 Jun 2024 15:18:50 +0900 Subject: [PATCH 34/47] chore: removed unnecessary moves Signed-off-by: Kenzo Lobos-Tsunekawa --- nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp | 2 +- nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp index 5713caf99..a4983b083 100644 --- a/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp @@ -49,7 +49,7 @@ ContinentalARS548RosWrapper::ContinentalARS548RosWrapper(const rclcpp::NodeOptio decoder_thread_ = std::thread([this]() { while (true) { - decoder_wrapper_->ProcessPacket(std::move(packet_queue_.pop())); + decoder_wrapper_->ProcessPacket(packet_queue_.pop()); } }); diff --git a/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp index f0789d3e2..08e20a926 100644 --- a/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp @@ -50,7 +50,7 @@ ContinentalSRR520RosWrapper::ContinentalSRR520RosWrapper(const rclcpp::NodeOptio decoder_thread_ = std::thread([this]() { while (true) { - decoder_wrapper_->ProcessPacket(std::move(packet_queue_.pop())); + decoder_wrapper_->ProcessPacket(packet_queue_.pop()); } }); From 78996f6fd4a346213afc1c5a90fb5155ea61a68b Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Tue, 11 Jun 2024 15:37:53 +0900 Subject: [PATCH 35/47] chore: updated parameter loading scheme Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental_ars548_ros_wrapper.cpp | 162 +++--------------- .../continental_srr520_ros_wrapper.cpp | 96 ++--------- 2 files changed, 35 insertions(+), 223 deletions(-) diff --git a/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp index a4983b083..505c9ec7d 100644 --- a/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp @@ -76,146 +76,28 @@ nebula::Status ContinentalARS548RosWrapper::DeclareAndGetSensorConfigParams() { nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration config; - { - rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model"); - config.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("host_ip", descriptor); - config.host_ip = this->get_parameter("host_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_ip", descriptor); - config.sensor_ip = this->get_parameter("sensor_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("multicast_ip", descriptor); - config.multicast_ip = this->get_parameter("multicast_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", descriptor); - config.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("base_frame", descriptor); - config.base_frame = this->get_parameter("base_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("object_frame", descriptor); - config.object_frame = this->get_parameter("object_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("data_port", descriptor); - config.data_port = this->get_parameter("data_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_host_port", descriptor); - config.configuration_host_port = this->get_parameter("configuration_host_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_sensor_port", descriptor); - config.configuration_sensor_port = this->get_parameter("configuration_sensor_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("use_sensor_time", descriptor); - config.use_sensor_time = this->get_parameter("use_sensor_time").as_bool(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_length", descriptor); - config.configuration_vehicle_length = - static_cast(this->get_parameter("configuration_vehicle_length").as_double()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_width", descriptor); - config.configuration_vehicle_width = - static_cast(this->get_parameter("configuration_vehicle_width").as_double()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_height", descriptor); - config.configuration_vehicle_height = - static_cast(this->get_parameter("configuration_vehicle_height").as_double()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_wheelbase", descriptor); - config.configuration_vehicle_wheelbase = - static_cast(this->get_parameter("configuration_vehicle_wheelbase").as_double()); - } + config.sensor_model = nebula::drivers::SensorModelFromString( + declare_parameter("sensor_model", param_read_only())); + config.host_ip = declare_parameter("host_ip", param_read_only()); + config.sensor_ip = declare_parameter("sensor_ip", param_read_only()); + config.multicast_ip = declare_parameter("multicast_ip", param_read_only()); + config.frame_id = declare_parameter("frame_id", param_read_write()); + config.base_frame = declare_parameter("base_frame", param_read_write()); + config.object_frame = declare_parameter("object_frame", param_read_write()); + config.data_port = static_cast(declare_parameter("data_port", param_read_only())); + config.configuration_host_port = + static_cast(declare_parameter("configuration_host_port", param_read_only())); + config.configuration_sensor_port = + static_cast(declare_parameter("configuration_sensor_port", param_read_only())); + config.use_sensor_time = declare_parameter("use_sensor_time", param_read_write()); + config.configuration_vehicle_length = static_cast( + declare_parameter("configuration_vehicle_length", param_read_write())); + config.configuration_vehicle_width = static_cast( + declare_parameter("configuration_vehicle_width", param_read_write())); + config.configuration_vehicle_height = static_cast( + declare_parameter("configuration_vehicle_height", param_read_write())); + config.configuration_vehicle_wheelbase = static_cast( + declare_parameter("configuration_vehicle_wheelbase", param_read_write())); if (config.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { return Status::INVALID_SENSOR_MODEL; diff --git a/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp index 08e20a926..2f3591ff9 100644 --- a/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp @@ -77,89 +77,19 @@ nebula::Status ContinentalSRR520RosWrapper::DeclareAndGetSensorConfigParams() { nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration config; - { - rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", descriptor); - config.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("interface", descriptor); - config.interface = this->get_parameter("interface").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("receiver_timeout_sec", descriptor); - config.receiver_timeout_sec = this->get_parameter("receiver_timeout_sec").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sender_timeout_sec", descriptor); - config.sender_timeout_sec = this->get_parameter("sender_timeout_sec").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("filters", descriptor); - config.filters = this->get_parameter("filters").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", descriptor); - config.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("base_frame", descriptor); - config.base_frame = this->get_parameter("base_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("use_bus_time", descriptor); - config.use_bus_time = this->get_parameter("use_bus_time").as_bool(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_wheelbase", descriptor); - config.configuration_vehicle_wheelbase = - static_cast(this->get_parameter("configuration_vehicle_wheelbase").as_double()); - } + config.sensor_model = nebula::drivers::SensorModelFromString( + declare_parameter("sensor_model", param_read_only())); + config.interface = declare_parameter("interface", param_read_only()); + config.receiver_timeout_sec = + static_cast(declare_parameter("receiver_timeout_sec", param_read_only())); + config.sender_timeout_sec = + static_cast(declare_parameter("sender_timeout_sec", param_read_only())); + config.filters = declare_parameter("filters", param_read_only()); + config.frame_id = declare_parameter("frame_id", param_read_write()); + config.base_frame = declare_parameter("base_frame", param_read_write()); + config.use_bus_time = declare_parameter("use_bus_time", param_read_only()); + config.configuration_vehicle_wheelbase = static_cast( + declare_parameter("configuration_vehicle_wheelbase", param_read_only())); if (config.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { return Status::INVALID_SENSOR_MODEL; From 4afd4799ff0293ceac2c93f1312976c4963f999f Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 13 Jun 2024 00:29:16 +0900 Subject: [PATCH 36/47] chore: updated srr test includes to match google code style Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental_ros_decoder_test_main_srr520.cpp | 6 ++---- .../continental_ros_decoder_test_srr520.cpp | 3 +-- .../continental_ros_decoder_test_srr520.hpp | 16 +++++++--------- 3 files changed, 10 insertions(+), 15 deletions(-) diff --git a/nebula_tests/continental/continental_ros_decoder_test_main_srr520.cpp b/nebula_tests/continental/continental_ros_decoder_test_main_srr520.cpp index 56b072750..4830b6862 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_main_srr520.cpp +++ b/nebula_tests/continental/continental_ros_decoder_test_main_srr520.cpp @@ -13,10 +13,8 @@ // limitations under the License. #include "continental_ros_decoder_test_srr520.hpp" - -#include - -#include +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" #include diff --git a/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp b/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp index be79400ba..dca69659d 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp +++ b/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp @@ -14,6 +14,7 @@ #include "continental_ros_decoder_test_srr520.hpp" +#include "gtest/gtest.h" #include "rclcpp/serialization.hpp" #include "rclcpp/serialized_message.hpp" #include "rcpputils/filesystem_helper.hpp" @@ -24,8 +25,6 @@ #include "rosbag2_cpp/writers/sequential_writer.hpp" #include "rosbag2_storage/storage_options.hpp" -#include - #include #include #include diff --git a/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp b/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp index 17954293e..b56dd43ca 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp +++ b/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp @@ -15,19 +15,17 @@ #ifndef NEBULA_ContinentalRosDecoderTestsrr520_H #define NEBULA_ContinentalRosDecoderTestsrr520_H +#include "diagnostic_updater/diagnostic_updater.hpp" +#include "gtest/gtest.h" #include "nebula_common/nebula_common.hpp" #include "nebula_common/nebula_status.hpp" #include "nebula_common/velodyne/velodyne_common.hpp" #include "nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp" +#include "rclcpp/rclcpp.hpp" -#include -#include - -#include -#include -#include - -#include +#include "continental_msgs/msg/continental_srr520_detection_list.hpp" +#include "continental_msgs/msg/continental_srr520_object_list.hpp" +#include "nebula_msgs/msg/nebula_packets.hpp" #include #include @@ -36,7 +34,7 @@ namespace nebula { namespace ros { -class ContinentalRosDecoderTest final : public rclcpp::Node //, testing::Test +class ContinentalRosDecoderTest final : public rclcpp::Node //, testing::Test { std::shared_ptr driver_ptr_; Status wrapper_status_; From fdb4c121f3f6179f063118e306c28ecd66b4a47a Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 13 Jun 2024 00:34:01 +0900 Subject: [PATCH 37/47] chore: added uniform initialization in srr tests Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental/continental_ros_decoder_test_srr520.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp b/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp index b56dd43ca..7ec697ee3 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp +++ b/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp @@ -80,10 +80,10 @@ class ContinentalRosDecoderTest final : public rclcpp::Node //, testing::Test void ReadBag(); private: - std::string bag_path; - std::string storage_id; - std::string format; - std::string target_topic; + std::string bag_path{}; + std::string storage_id{}; + std::string format{}; + std::string target_topic{}; std::size_t near_detection_list_count_{}; std::size_t hrr_detection_list_count_{}; std::size_t object_list_count_{}; From 899bf7a0931f19ce7945f90ba9a8bb2a75ec96d7 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 13 Jun 2024 00:36:59 +0900 Subject: [PATCH 38/47] chore: missing underscore in class variables for srr tests Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental_ros_decoder_test_srr520.cpp | 36 +++++++++---------- .../continental_ros_decoder_test_srr520.hpp | 8 ++--- 2 files changed, 22 insertions(+), 22 deletions(-) diff --git a/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp b/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp index dca69659d..5f80bb20a 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp +++ b/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp @@ -127,8 +127,8 @@ Status ContinentalRosDecoderTest::GetParameters( descriptor.additional_constraints = ""; this->declare_parameter( "bag_path", (bag_root_dir / "srr520" / "1708578209").string(), descriptor); - bag_path = this->get_parameter("bag_path").as_string(); - std::cout << bag_path << std::endl; + bag_path_ = this->get_parameter("bag_path").as_string(); + std::cout << bag_path_ << std::endl; } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -137,7 +137,7 @@ Status ContinentalRosDecoderTest::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; this->declare_parameter("storage_id", "sqlite3", descriptor); - storage_id = this->get_parameter("storage_id").as_string(); + storage_id_ = this->get_parameter("storage_id").as_string(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -146,7 +146,7 @@ Status ContinentalRosDecoderTest::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; this->declare_parameter("format", "cdr", descriptor); - format = this->get_parameter("format").as_string(); + format_ = this->get_parameter("format").as_string(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -156,7 +156,7 @@ Status ContinentalRosDecoderTest::GetParameters( descriptor.additional_constraints = ""; this->declare_parameter( "target_topic", "/sensing/radar/front_left/nebula_packets", descriptor); - target_topic = this->get_parameter("target_topic").as_string(); + target_topic_ = this->get_parameter("target_topic").as_string(); } if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { @@ -214,7 +214,7 @@ void ContinentalRosDecoderTest::HRRDetectionListCallback( detection_path << msg->header.stamp.sec << "_" << msg->header.stamp.nanosec << "_hrr_detection.yaml"; - auto gt_path = rcpputils::fs::path(bag_path).parent_path() / detection_path.str(); + auto gt_path = rcpputils::fs::path(bag_path_).parent_path() / detection_path.str(); ASSERT_TRUE(gt_path.exists()); CheckResult(msg_as_string, gt_path.string()); @@ -231,7 +231,7 @@ void ContinentalRosDecoderTest::NearDetectionListCallback( detection_path << msg->header.stamp.sec << "_" << msg->header.stamp.nanosec << "_near_detection.yaml"; - auto gt_path = rcpputils::fs::path(bag_path).parent_path() / detection_path.str(); + auto gt_path = rcpputils::fs::path(bag_path_).parent_path() / detection_path.str(); ASSERT_TRUE(gt_path.exists()); CheckResult(msg_as_string, gt_path.string()); @@ -247,7 +247,7 @@ void ContinentalRosDecoderTest::ObjectListCallback( std::stringstream detection_path; detection_path << msg->header.stamp.sec << "_" << msg->header.stamp.nanosec << "_object.yaml"; - auto gt_path = rcpputils::fs::path(bag_path).parent_path() / detection_path.str(); + auto gt_path = rcpputils::fs::path(bag_path_).parent_path() / detection_path.str(); ASSERT_TRUE(gt_path.exists()); CheckResult(msg_as_string, gt_path.string()); @@ -258,22 +258,22 @@ void ContinentalRosDecoderTest::ReadBag() rosbag2_storage::StorageOptions storage_options; rosbag2_cpp::ConverterOptions converter_options; - std::cout << bag_path << std::endl; - std::cout << storage_id << std::endl; - std::cout << format << std::endl; - std::cout << target_topic << std::endl; + std::cout << bag_path_ << std::endl; + std::cout << storage_id_ << std::endl; + std::cout << format_ << std::endl; + std::cout << target_topic_ << std::endl; - auto target_topic_name = target_topic; + auto target_topic_name = target_topic_; if (target_topic_name.substr(0, 1) == "/") { target_topic_name = target_topic_name.substr(1); } target_topic_name = std::regex_replace(target_topic_name, std::regex("/"), "_"); - rcpputils::fs::path bag_dir(bag_path); + rcpputils::fs::path bag_dir(bag_path_); - storage_options.uri = bag_path; - storage_options.storage_id = storage_id; - converter_options.output_serialization_format = format; // "cdr"; + storage_options.uri = bag_path_; + storage_options.storage_id = storage_id_; + converter_options.output_serialization_format = format_; // "cdr"; rclcpp::Serialization serialization; { @@ -284,7 +284,7 @@ void ContinentalRosDecoderTest::ReadBag() std::cout << "Found topic name " << bag_message->topic_name << std::endl; - if (bag_message->topic_name == target_topic) { + if (bag_message->topic_name == target_topic_) { nebula_msgs::msg::NebulaPackets extracted_msg; rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); serialization.deserialize_message(&extracted_serialized_msg, &extracted_msg); diff --git a/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp b/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp index 7ec697ee3..f6cdb1d50 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp +++ b/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp @@ -80,10 +80,10 @@ class ContinentalRosDecoderTest final : public rclcpp::Node //, testing::Test void ReadBag(); private: - std::string bag_path{}; - std::string storage_id{}; - std::string format{}; - std::string target_topic{}; + std::string bag_path_{}; + std::string storage_id_{}; + std::string format_{}; + std::string target_topic_{}; std::size_t near_detection_list_count_{}; std::size_t hrr_detection_list_count_{}; std::size_t object_list_count_{}; From b94fef272d42fea4321bcd0e6afdeb7184705e40 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 13 Jun 2024 09:29:28 +0900 Subject: [PATCH 39/47] chore: updated includes in ARS tests to keep consistency Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental_ros_decoder_test_ars548.hpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp b/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp index bf79912b7..e860d53dc 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp +++ b/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp @@ -15,19 +15,17 @@ #ifndef NEBULA_ContinentalRosDecoderTestArs548_H #define NEBULA_ContinentalRosDecoderTestArs548_H +#include "diagnostic_updater/diagnostic_updater.hpp" +#include "gtest/gtest.h" #include "nebula_common/nebula_common.hpp" #include "nebula_common/nebula_status.hpp" #include "nebula_common/velodyne/velodyne_common.hpp" #include "nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp" +#include "rclcpp/rclcpp.hpp" -#include -#include - -#include -#include -#include - -#include +#include "continental_msgs/msg/continental_ars548_detection_list.hpp" +#include "continental_msgs/msg/continental_ars548_object_list.hpp" +#include "nebula_msgs/msg/nebula_packets.hpp" #include #include From 1966b567a4403a2c2e35e648ccef557c7fbb76d7 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 13 Jun 2024 09:37:32 +0900 Subject: [PATCH 40/47] chore: updated missing ars548 tests Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental_ros_decoder_test_ars548.cpp | 37 +++++++++---------- .../continental_ros_decoder_test_ars548.hpp | 8 ++-- ...ntinental_ros_decoder_test_main_ars548.cpp | 6 +-- 3 files changed, 24 insertions(+), 27 deletions(-) diff --git a/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp b/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp index 1bd913f62..30b234092 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp +++ b/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp @@ -14,6 +14,7 @@ #include "continental_ros_decoder_test_ars548.hpp" +#include "gtest/gtest.h" #include "rclcpp/serialization.hpp" #include "rclcpp/serialized_message.hpp" #include "rcpputils/filesystem_helper.hpp" @@ -24,8 +25,6 @@ #include "rosbag2_cpp/writers/sequential_writer.hpp" #include "rosbag2_storage/storage_options.hpp" -#include - #include #include #include @@ -133,8 +132,8 @@ Status ContinentalRosDecoderTest::GetParameters( descriptor.additional_constraints = ""; this->declare_parameter( "bag_path", (bag_root_dir / "ars548" / "1708578204").string(), descriptor); - bag_path = this->get_parameter("bag_path").as_string(); - std::cout << bag_path << std::endl; + bag_path_ = this->get_parameter("bag_path").as_string(); + std::cout << bag_path_ << std::endl; } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -143,7 +142,7 @@ Status ContinentalRosDecoderTest::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; this->declare_parameter("storage_id", "sqlite3", descriptor); - storage_id = this->get_parameter("storage_id").as_string(); + storage_id_ = this->get_parameter("storage_id").as_string(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -152,7 +151,7 @@ Status ContinentalRosDecoderTest::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; this->declare_parameter("format", "cdr", descriptor); - format = this->get_parameter("format").as_string(); + format_ = this->get_parameter("format").as_string(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -162,7 +161,7 @@ Status ContinentalRosDecoderTest::GetParameters( descriptor.additional_constraints = ""; this->declare_parameter( "target_topic", "/sensing/radar/front_center/nebula_packets", descriptor); - target_topic = this->get_parameter("target_topic").as_string(); + target_topic_ = this->get_parameter("target_topic").as_string(); } if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { @@ -217,7 +216,7 @@ void ContinentalRosDecoderTest::DetectionListCallback( std::stringstream detection_path; detection_path << msg->header.stamp.sec << "_" << msg->header.stamp.nanosec << "_detection.yaml"; - auto gt_path = rcpputils::fs::path(bag_path).parent_path() / detection_path.str(); + auto gt_path = rcpputils::fs::path(bag_path_).parent_path() / detection_path.str(); ASSERT_TRUE(gt_path.exists()); CheckResult(msg_as_string, gt_path.string()); @@ -232,7 +231,7 @@ void ContinentalRosDecoderTest::ObjectListCallback( std::stringstream detection_path; detection_path << msg->header.stamp.sec << "_" << msg->header.stamp.nanosec << "_object.yaml"; - auto gt_path = rcpputils::fs::path(bag_path).parent_path() / detection_path.str(); + auto gt_path = rcpputils::fs::path(bag_path_).parent_path() / detection_path.str(); ASSERT_TRUE(gt_path.exists()); CheckResult(msg_as_string, gt_path.string()); @@ -243,22 +242,22 @@ void ContinentalRosDecoderTest::ReadBag() rosbag2_storage::StorageOptions storage_options; rosbag2_cpp::ConverterOptions converter_options; - std::cout << bag_path << std::endl; - std::cout << storage_id << std::endl; - std::cout << format << std::endl; - std::cout << target_topic << std::endl; + std::cout << bag_path_ << std::endl; + std::cout << storage_id_ << std::endl; + std::cout << format_ << std::endl; + std::cout << target_topic_ << std::endl; - auto target_topic_name = target_topic; + auto target_topic_name = target_topic_; if (target_topic_name.substr(0, 1) == "/") { target_topic_name = target_topic_name.substr(1); } target_topic_name = std::regex_replace(target_topic_name, std::regex("/"), "_"); - rcpputils::fs::path bag_dir(bag_path); + rcpputils::fs::path bag_dir(bag_path_); - storage_options.uri = bag_path; - storage_options.storage_id = storage_id; - converter_options.output_serialization_format = format; // "cdr"; + storage_options.uri = bag_path_; + storage_options.storage_id = storage_id_; + converter_options.output_serialization_format = format_; // "cdr"; rclcpp::Serialization serialization; { @@ -269,7 +268,7 @@ void ContinentalRosDecoderTest::ReadBag() std::cout << "Found topic name " << bag_message->topic_name << std::endl; - if (bag_message->topic_name == target_topic) { + if (bag_message->topic_name == target_topic_) { nebula_msgs::msg::NebulaPackets extracted_msg; rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); serialization.deserialize_message(&extracted_serialized_msg, &extracted_msg); diff --git a/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp b/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp index e860d53dc..9b88c394d 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp +++ b/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp @@ -73,10 +73,10 @@ class ContinentalRosDecoderTest final : public rclcpp::Node //, testing::Test void ReadBag(); private: - std::string bag_path; - std::string storage_id; - std::string format; - std::string target_topic; + std::string bag_path_{}; + std::string storage_id_{}; + std::string format_{}; + std::string target_topic_{}; }; } // namespace ros diff --git a/nebula_tests/continental/continental_ros_decoder_test_main_ars548.cpp b/nebula_tests/continental/continental_ros_decoder_test_main_ars548.cpp index 9935e446f..0f6b73ed7 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_main_ars548.cpp +++ b/nebula_tests/continental/continental_ros_decoder_test_main_ars548.cpp @@ -13,10 +13,8 @@ // limitations under the License. #include "continental_ros_decoder_test_ars548.hpp" - -#include - -#include +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" #include From 0c642581f80cd58f7b0ddd7c4d73d939fdf3c0e4 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos Tsunekawa Date: Thu, 13 Jun 2024 17:24:05 +0900 Subject: [PATCH 41/47] Update nebula_ros/schema/sub/topic.json Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com> --- nebula_ros/schema/sub/topic.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_ros/schema/sub/topic.json b/nebula_ros/schema/sub/topic.json index a238c06a5..b20f859d3 100644 --- a/nebula_ros/schema/sub/topic.json +++ b/nebula_ros/schema/sub/topic.json @@ -38,7 +38,7 @@ "type": "boolean", "default": false, "readOnly": true, - "description": "Use bus time for published sensor data." + "description": "Use CAN bus time for published sensor data." }, "use_sensor_time": { "type": "boolean", From 38771fb343a40c816fd5c5adabd60ac506330a47 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 13 Jun 2024 19:15:23 +0900 Subject: [PATCH 42/47] chore: updated to the new parameter loading scheme Signed-off-by: Kenzo Lobos-Tsunekawa --- nebula_tests/continental/CMakeLists.txt | 4 +- .../continental_ros_decoder_test_ars548.cpp | 93 ++++--------------- .../continental_ros_decoder_test_srr520.cpp | 82 +++------------- .../continental/parameter_descriptors.cpp | 34 +++++++ .../continental/parameter_descriptors.hpp | 44 +++++++++ .../srr520/1708578208_992410284_object.yaml | 2 +- .../1708578209_45566935_near_detection.yaml | 2 +- .../1708578209_76111685_hrr_detection.yaml | 2 +- 8 files changed, 116 insertions(+), 147 deletions(-) create mode 100644 nebula_tests/continental/parameter_descriptors.cpp create mode 100644 nebula_tests/continental/parameter_descriptors.hpp diff --git a/nebula_tests/continental/CMakeLists.txt b/nebula_tests/continental/CMakeLists.txt index 96e0c286b..10e01ca1f 100644 --- a/nebula_tests/continental/CMakeLists.txt +++ b/nebula_tests/continental/CMakeLists.txt @@ -1,6 +1,7 @@ # Continental ARS548 add_library(continental_ros_decoder_test_ars548 SHARED continental_ros_decoder_test_ars548.cpp + parameter_descriptors.cpp ) target_include_directories(continental_ros_decoder_test_ars548 PUBLIC @@ -26,6 +27,7 @@ target_link_libraries(continental_ros_decoder_test_main_ars548 # Continental SRR520 add_library(continental_ros_decoder_test_srr520 SHARED continental_ros_decoder_test_srr520.cpp + parameter_descriptors.cpp ) target_include_directories(continental_ros_decoder_test_srr520 PUBLIC @@ -47,4 +49,4 @@ target_include_directories(continental_ros_decoder_test_main_srr520 PUBLIC ) target_link_libraries(continental_ros_decoder_test_main_srr520 continental_ros_decoder_test_srr520 -) \ No newline at end of file +) diff --git a/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp b/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp index 30b234092..28ae5a9b4 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp +++ b/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp @@ -15,6 +15,7 @@ #include "continental_ros_decoder_test_ars548.hpp" #include "gtest/gtest.h" +#include "parameter_descriptors.hpp" #include "rclcpp/serialization.hpp" #include "rclcpp/serialized_message.hpp" #include "rcpputils/filesystem_helper.hpp" @@ -87,82 +88,22 @@ Status ContinentalRosDecoderTest::GetParameters( std::filesystem::path bag_root_dir = _SRC_RESOURCES_DIR_PATH; // variable defined in CMakeLists.txt; bag_root_dir /= "continental"; - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", "ARS548"); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("base_frame", "some_base_frame", descriptor); - sensor_configuration.base_frame = this->get_parameter("base_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("object_frame", "some_object_frame", descriptor); - sensor_configuration.object_frame = this->get_parameter("object_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "some_sensor_frame", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter( - "bag_path", (bag_root_dir / "ars548" / "1708578204").string(), descriptor); - bag_path_ = this->get_parameter("bag_path").as_string(); - std::cout << bag_path_ << std::endl; - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("storage_id", "sqlite3", descriptor); - storage_id_ = this->get_parameter("storage_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("format", "cdr", descriptor); - format_ = this->get_parameter("format").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter( - "target_topic", "/sensing/radar/front_center/nebula_packets", descriptor); - target_topic_ = this->get_parameter("target_topic").as_string(); - } + + sensor_configuration.sensor_model = nebula::drivers::SensorModelFromString( + declare_parameter("sensor_model", "ARS548", param_read_only())); + sensor_configuration.frame_id = + declare_parameter("frame_id", "some_sensor_frame", param_read_only()); + sensor_configuration.base_frame = + declare_parameter("base_frame", "some_base_frame", param_read_only()); + sensor_configuration.object_frame = + declare_parameter("object_frame", "some_object_frame", param_read_only()); + + bag_path_ = declare_parameter( + "bag_path", (bag_root_dir / "ars548" / "1708578204").string(), param_read_only()); + storage_id_ = declare_parameter("storage_id", "sqlite3", param_read_only()); + format_ = declare_parameter("format", "cdr", param_read_only()); + target_topic_ = declare_parameter( + "target_topic", "/sensing/radar/front_center/nebula_packets", param_read_only()); if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { return Status::INVALID_SENSOR_MODEL; diff --git a/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp b/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp index 5f80bb20a..9aa2af968 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp +++ b/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp @@ -15,6 +15,7 @@ #include "continental_ros_decoder_test_srr520.hpp" #include "gtest/gtest.h" +#include "parameter_descriptors.hpp" #include "rclcpp/serialization.hpp" #include "rclcpp/serialized_message.hpp" #include "rcpputils/filesystem_helper.hpp" @@ -91,73 +92,20 @@ Status ContinentalRosDecoderTest::GetParameters( std::filesystem::path bag_root_dir = _SRC_RESOURCES_DIR_PATH; // variable defined in CMakeLists.txt; bag_root_dir /= "continental"; - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", "SRR520"); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("base_frame", "some_base_frame", descriptor); - sensor_configuration.frame_id = this->get_parameter("base_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "some_sensor_frame", descriptor); - sensor_configuration.base_frame = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter( - "bag_path", (bag_root_dir / "srr520" / "1708578209").string(), descriptor); - bag_path_ = this->get_parameter("bag_path").as_string(); - std::cout << bag_path_ << std::endl; - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("storage_id", "sqlite3", descriptor); - storage_id_ = this->get_parameter("storage_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("format", "cdr", descriptor); - format_ = this->get_parameter("format").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter( - "target_topic", "/sensing/radar/front_left/nebula_packets", descriptor); - target_topic_ = this->get_parameter("target_topic").as_string(); - } + + sensor_configuration.sensor_model = nebula::drivers::SensorModelFromString( + declare_parameter("sensor_model", "SRR520", param_read_only())); + sensor_configuration.frame_id = + declare_parameter("frame_id", "some_sensor_frame", param_read_only()); + sensor_configuration.base_frame = + declare_parameter("base_frame", "some_base_frame", param_read_only()); + + bag_path_ = declare_parameter( + "bag_path", (bag_root_dir / "srr520" / "1708578209").string(), param_read_only()); + storage_id_ = declare_parameter("storage_id", "sqlite3", param_read_only()); + format_ = declare_parameter("format", "cdr", param_read_only()); + target_topic_ = declare_parameter( + "target_topic", "/sensing/radar/front_left/nebula_packets", param_read_only()); if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { return Status::INVALID_SENSOR_MODEL; diff --git a/nebula_tests/continental/parameter_descriptors.cpp b/nebula_tests/continental/parameter_descriptors.cpp new file mode 100644 index 000000000..d2d265780 --- /dev/null +++ b/nebula_tests/continental/parameter_descriptors.cpp @@ -0,0 +1,34 @@ +#include "parameter_descriptors.hpp" + +namespace nebula +{ +namespace ros +{ + +rcl_interfaces::msg::ParameterDescriptor param_read_write() +{ + return rcl_interfaces::msg::ParameterDescriptor{}; +}; + +rcl_interfaces::msg::ParameterDescriptor param_read_only() +{ + return rcl_interfaces::msg::ParameterDescriptor{}.set__read_only(true); +} + +rcl_interfaces::msg::ParameterDescriptor::_floating_point_range_type float_range( + double start, double stop, double step) +{ + return { + rcl_interfaces::msg::FloatingPointRange().set__from_value(start).set__to_value(stop).set__step( + step)}; +} + +rcl_interfaces::msg::ParameterDescriptor::_integer_range_type int_range( + int start, int stop, int step) +{ + return { + rcl_interfaces::msg::IntegerRange().set__from_value(start).set__to_value(stop).set__step(step)}; +} + +} // namespace ros +} // namespace nebula diff --git a/nebula_tests/continental/parameter_descriptors.hpp b/nebula_tests/continental/parameter_descriptors.hpp new file mode 100644 index 000000000..6d4e38504 --- /dev/null +++ b/nebula_tests/continental/parameter_descriptors.hpp @@ -0,0 +1,44 @@ +#pragma once + +#include +#include +#include + +#include +#include +namespace nebula +{ +namespace ros +{ + +rcl_interfaces::msg::ParameterDescriptor param_read_write(); + +rcl_interfaces::msg::ParameterDescriptor param_read_only(); + +rcl_interfaces::msg::ParameterDescriptor::_floating_point_range_type float_range( + double start, double stop, double step); + +rcl_interfaces::msg::ParameterDescriptor::_integer_range_type int_range( + int start, int stop, int step); + +/// @brief Get a parameter's value from a list of parameters, if that parameter is in the list. +/// @tparam T The parameter's expected value type +/// @param p A vector of parameters +/// @param name Target parameter name +/// @param value (out) Parameter value. Set if parameter is found, left untouched otherwise. +/// @return Whether the target name existed +template +bool get_param(const std::vector & p, const std::string & name, T & value) +{ + auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { + return parameter.get_name() == name; + }); + if (it != p.cend()) { + value = it->template get_value(); + return true; + } + return false; +} + +} // namespace ros +} // namespace nebula diff --git a/nebula_tests/data/continental/srr520/1708578208_992410284_object.yaml b/nebula_tests/data/continental/srr520/1708578208_992410284_object.yaml index dcdd5843c..f9d7cfa56 100644 --- a/nebula_tests/data/continental/srr520/1708578208_992410284_object.yaml +++ b/nebula_tests/data/continental/srr520/1708578208_992410284_object.yaml @@ -2,7 +2,7 @@ header: stamp: sec: 1708578208 nanosec: 992410284 - frame_id: some_sensor_frame + frame_id: some_base_frame internal_time_stamp_usec: 468502487 global_time_stamp_sync_status: 1 signal_status: 1 diff --git a/nebula_tests/data/continental/srr520/1708578209_45566935_near_detection.yaml b/nebula_tests/data/continental/srr520/1708578209_45566935_near_detection.yaml index 10b233b89..556a9532c 100644 --- a/nebula_tests/data/continental/srr520/1708578209_45566935_near_detection.yaml +++ b/nebula_tests/data/continental/srr520/1708578209_45566935_near_detection.yaml @@ -2,7 +2,7 @@ header: stamp: sec: 1708578209 nanosec: 45566935 - frame_id: some_base_frame + frame_id: some_sensor_frame internal_time_stamp_usec: 468555644 global_time_stamp_sync_status: 1 signal_status: 1 diff --git a/nebula_tests/data/continental/srr520/1708578209_76111685_hrr_detection.yaml b/nebula_tests/data/continental/srr520/1708578209_76111685_hrr_detection.yaml index 1becd4f61..cb8e3279c 100644 --- a/nebula_tests/data/continental/srr520/1708578209_76111685_hrr_detection.yaml +++ b/nebula_tests/data/continental/srr520/1708578209_76111685_hrr_detection.yaml @@ -2,7 +2,7 @@ header: stamp: sec: 1708578209 nanosec: 76111685 - frame_id: some_base_frame + frame_id: some_sensor_frame internal_time_stamp_usec: 468586189 global_time_stamp_sync_status: 1 signal_status: 1 From b379c81ede85d04262b7cc8c12f674d95febbce5 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 13 Jun 2024 19:32:32 +0900 Subject: [PATCH 43/47] chore: updated include rules for follow nebula/autoware rules Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental/continental_srr520.hpp | 2 +- .../decoders/continental_ars548_decoder.cpp | 3 ++- .../decoders/continental_srr520_decoder.cpp | 3 +-- .../continental_ars548_hw_interface.cpp | 3 ++- .../continental_srr520_hw_interface.cpp | 4 +++- .../continental_ars548_decoder_wrapper.hpp | 5 ++-- ...ontinental_ars548_hw_interface_wrapper.hpp | 3 ++- .../continental_ars548_ros_wrapper.hpp | 9 ++++---- .../continental_srr520_decoder_wrapper.hpp | 5 ++-- ...ontinental_srr520_hw_interface_wrapper.hpp | 3 ++- .../continental_srr520_ros_wrapper.hpp | 9 ++++---- .../continental_ars548_decoder_wrapper.cpp | 2 +- ...ontinental_ars548_hw_interface_wrapper.cpp | 2 +- .../continental_ars548_ros_wrapper.cpp | 2 +- .../continental_srr520_decoder_wrapper.cpp | 2 +- ...ontinental_srr520_hw_interface_wrapper.cpp | 2 +- .../continental_srr520_ros_wrapper.cpp | 2 +- .../continental_ros_decoder_test_ars548.cpp | 22 ++++++++++-------- .../continental_ros_decoder_test_ars548.hpp | 23 ++++++++++--------- ...ntinental_ros_decoder_test_main_ars548.cpp | 6 +++-- ...ntinental_ros_decoder_test_main_srr520.cpp | 6 +++-- .../continental_ros_decoder_test_srr520.cpp | 22 ++++++++++-------- .../continental_ros_decoder_test_srr520.hpp | 23 ++++++++++--------- 23 files changed, 91 insertions(+), 72 deletions(-) diff --git a/nebula_common/include/nebula_common/continental/continental_srr520.hpp b/nebula_common/include/nebula_common/continental/continental_srr520.hpp index f5f254f4f..fd4071853 100644 --- a/nebula_common/include/nebula_common/continental/continental_srr520.hpp +++ b/nebula_common/include/nebula_common/continental/continental_srr520.hpp @@ -17,7 +17,7 @@ #include #include -#include "boost/endian/buffers.hpp" +#include #include #include diff --git a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp index 8423d4a0b..548867b7f 100644 --- a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp @@ -12,8 +12,9 @@ // 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 -#include #include #include diff --git a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp index d5c2d8c83..d559b5714 100644 --- a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp @@ -14,8 +14,7 @@ #include "nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp" -#include "nebula_common/continental/continental_srr520.hpp" - +#include #include #include diff --git a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp index b115ea637..cd1053731 100644 --- a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp @@ -13,8 +13,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp" + #include -#include namespace nebula { diff --git a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp index d019fdf5d..bf7985bf0 100644 --- a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp @@ -13,12 +13,14 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp" + #include #include -#include #include #include + namespace nebula { namespace drivers diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp index cb6cebd9b..3c1eaa905 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp @@ -14,12 +14,13 @@ #pragma once +#include "nebula_ros/common/parameter_descriptors.hpp" +#include "nebula_ros/common/watchdog_timer.hpp" + #include #include #include #include -#include -#include #include #include diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp index 071b67126..855a3b5ba 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp @@ -14,9 +14,10 @@ #pragma once +#include "nebula_ros/common/parameter_descriptors.hpp" + #include #include -#include #include #include diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp index f136f6a11..ea9b3a822 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp @@ -14,14 +14,15 @@ #pragma once +#include "nebula_ros/common/mt_queue.hpp" +#include "nebula_ros/common/parameter_descriptors.hpp" +#include "nebula_ros/continental/continental_ars548_decoder_wrapper.hpp" +#include "nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp" + #include #include #include #include -#include -#include -#include -#include #include #include diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp index 89b9759fe..2dbfa3447 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp @@ -14,13 +14,14 @@ #pragma once +#include "nebula_ros/common/parameter_descriptors.hpp" +#include "nebula_ros/common/watchdog_timer.hpp" + #include #include #include #include #include -#include -#include #include #include diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp index ff664c968..3da76cab6 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp @@ -14,9 +14,10 @@ #pragma once +#include "nebula_ros/common/parameter_descriptors.hpp" + #include #include -#include #include #include diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp index 7b373d03a..6df11e9b6 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp @@ -14,14 +14,15 @@ #pragma once +#include "nebula_ros/common/mt_queue.hpp" +#include "nebula_ros/common/parameter_descriptors.hpp" +#include "nebula_ros/continental/continental_srr520_decoder_wrapper.hpp" +#include "nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp" + #include #include #include #include -#include -#include -#include -#include #include #include diff --git a/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp index 853837fdd..dede30210 100644 --- a/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "nebula_ros/continental/continental_ars548_decoder_wrapper.hpp" #include diff --git a/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp index 17e5b5ca8..9e6d3086f 100644 --- a/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp" #include #include diff --git a/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp index 505c9ec7d..52f8c1df5 100644 --- a/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "nebula_ros/continental/continental_ars548_ros_wrapper.hpp" namespace nebula { diff --git a/nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp index d5355eb69..e85e6cdca 100644 --- a/nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp +++ b/nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "nebula_ros/continental/continental_srr520_decoder_wrapper.hpp" #include diff --git a/nebula_ros/src/continental/continental_srr520_hw_interface_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_hw_interface_wrapper.cpp index f76c94a06..42745fa58 100644 --- a/nebula_ros/src/continental/continental_srr520_hw_interface_wrapper.cpp +++ b/nebula_ros/src/continental/continental_srr520_hw_interface_wrapper.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp" #include #include diff --git a/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp index 2f3591ff9..8fff84139 100644 --- a/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "nebula_ros/continental/continental_srr520_ros_wrapper.hpp" namespace nebula { diff --git a/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp b/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp index 28ae5a9b4..727b9db62 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp +++ b/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp @@ -14,17 +14,19 @@ #include "continental_ros_decoder_test_ars548.hpp" -#include "gtest/gtest.h" #include "parameter_descriptors.hpp" -#include "rclcpp/serialization.hpp" -#include "rclcpp/serialized_message.hpp" -#include "rcpputils/filesystem_helper.hpp" -#include "rcutils/time.h" -#include "rosbag2_cpp/reader.hpp" -#include "rosbag2_cpp/readers/sequential_reader.hpp" -#include "rosbag2_cpp/writer.hpp" -#include "rosbag2_cpp/writers/sequential_writer.hpp" -#include "rosbag2_storage/storage_options.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include #include #include diff --git a/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp b/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp index 9b88c394d..64e53c04b 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp +++ b/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp @@ -15,17 +15,18 @@ #ifndef NEBULA_ContinentalRosDecoderTestArs548_H #define NEBULA_ContinentalRosDecoderTestArs548_H -#include "diagnostic_updater/diagnostic_updater.hpp" -#include "gtest/gtest.h" -#include "nebula_common/nebula_common.hpp" -#include "nebula_common/nebula_status.hpp" -#include "nebula_common/velodyne/velodyne_common.hpp" -#include "nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp" -#include "rclcpp/rclcpp.hpp" - -#include "continental_msgs/msg/continental_ars548_detection_list.hpp" -#include "continental_msgs/msg/continental_ars548_object_list.hpp" -#include "nebula_msgs/msg/nebula_packets.hpp" +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include #include #include diff --git a/nebula_tests/continental/continental_ros_decoder_test_main_ars548.cpp b/nebula_tests/continental/continental_ros_decoder_test_main_ars548.cpp index 0f6b73ed7..9935e446f 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_main_ars548.cpp +++ b/nebula_tests/continental/continental_ros_decoder_test_main_ars548.cpp @@ -13,8 +13,10 @@ // limitations under the License. #include "continental_ros_decoder_test_ars548.hpp" -#include "gtest/gtest.h" -#include "rclcpp/rclcpp.hpp" + +#include + +#include #include diff --git a/nebula_tests/continental/continental_ros_decoder_test_main_srr520.cpp b/nebula_tests/continental/continental_ros_decoder_test_main_srr520.cpp index 4830b6862..56b072750 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_main_srr520.cpp +++ b/nebula_tests/continental/continental_ros_decoder_test_main_srr520.cpp @@ -13,8 +13,10 @@ // limitations under the License. #include "continental_ros_decoder_test_srr520.hpp" -#include "gtest/gtest.h" -#include "rclcpp/rclcpp.hpp" + +#include + +#include #include diff --git a/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp b/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp index 9aa2af968..7991fada1 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp +++ b/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp @@ -14,17 +14,19 @@ #include "continental_ros_decoder_test_srr520.hpp" -#include "gtest/gtest.h" #include "parameter_descriptors.hpp" -#include "rclcpp/serialization.hpp" -#include "rclcpp/serialized_message.hpp" -#include "rcpputils/filesystem_helper.hpp" -#include "rcutils/time.h" -#include "rosbag2_cpp/reader.hpp" -#include "rosbag2_cpp/readers/sequential_reader.hpp" -#include "rosbag2_cpp/writer.hpp" -#include "rosbag2_cpp/writers/sequential_writer.hpp" -#include "rosbag2_storage/storage_options.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include #include #include diff --git a/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp b/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp index f6cdb1d50..525360e03 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp +++ b/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp @@ -15,17 +15,18 @@ #ifndef NEBULA_ContinentalRosDecoderTestsrr520_H #define NEBULA_ContinentalRosDecoderTestsrr520_H -#include "diagnostic_updater/diagnostic_updater.hpp" -#include "gtest/gtest.h" -#include "nebula_common/nebula_common.hpp" -#include "nebula_common/nebula_status.hpp" -#include "nebula_common/velodyne/velodyne_common.hpp" -#include "nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp" -#include "rclcpp/rclcpp.hpp" - -#include "continental_msgs/msg/continental_srr520_detection_list.hpp" -#include "continental_msgs/msg/continental_srr520_object_list.hpp" -#include "nebula_msgs/msg/nebula_packets.hpp" +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include #include #include From 6819c282bb6ae1638962f665bb9e7507f935d373 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 13 Jun 2024 19:35:29 +0900 Subject: [PATCH 44/47] chore: updated missing include rules violations Signed-off-by: Kenzo Lobos-Tsunekawa --- .../include/nebula_common/continental/continental_ars548.hpp | 4 ++-- .../include/nebula_common/continental/continental_srr520.hpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/nebula_common/include/nebula_common/continental/continental_ars548.hpp b/nebula_common/include/nebula_common/continental/continental_ars548.hpp index 949b025be..06bdb3789 100644 --- a/nebula_common/include/nebula_common/continental/continental_ars548.hpp +++ b/nebula_common/include/nebula_common/continental/continental_ars548.hpp @@ -14,8 +14,8 @@ #pragma once -#include -#include +#include "nebula_common/nebula_common.hpp" +#include "nebula_common/nebula_status.hpp" #include #include diff --git a/nebula_common/include/nebula_common/continental/continental_srr520.hpp b/nebula_common/include/nebula_common/continental/continental_srr520.hpp index fd4071853..ca75e6944 100644 --- a/nebula_common/include/nebula_common/continental/continental_srr520.hpp +++ b/nebula_common/include/nebula_common/continental/continental_srr520.hpp @@ -14,8 +14,8 @@ #pragma once -#include -#include +#include "nebula_common/nebula_common.hpp" +#include "nebula_common/nebula_status.hpp" #include From fdcf7274def77eade93489c3d605ff224008423c Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 13 Jun 2024 19:39:36 +0900 Subject: [PATCH 45/47] chore: missing include violations in the continental decoders Signed-off-by: Kenzo Lobos-Tsunekawa --- .../decoders/continental_ars548_decoder.hpp | 3 ++- .../decoders/continental_srr520_decoder.hpp | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp index af0c508e3..b017e6efd 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp @@ -14,8 +14,9 @@ #pragma once +#include "nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp" + #include -#include #include #include diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp index a3dfd07b9..c70cb4706 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp @@ -14,8 +14,9 @@ #pragma once +#include "nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp" + #include -#include #include #include From 259b4764f48ab8c3aa9a5fb42a724985322c396f Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 13 Jun 2024 19:46:05 +0900 Subject: [PATCH 46/47] chore: missing include violations in the continental hw interfaces Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental_ars548_hw_interface.hpp | 3 ++- .../continental_srr520_hw_interface.hpp | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp index 605fe8fe0..0563dc605 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp @@ -15,9 +15,10 @@ #ifndef NEBULA_CONTINENTAL_ARS548_HW_INTERFACE_H #define NEBULA_CONTINENTAL_ARS548_HW_INTERFACE_H +#include "nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp" + #include #include -#include #include #include diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp index df3f28bb0..ddbbe1619 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp @@ -15,8 +15,9 @@ #ifndef NEBULA_CONTINENTAL_SRR520_HW_INTERFACE_H #define NEBULA_CONTINENTAL_SRR520_HW_INTERFACE_H +#include "nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp" + #include -#include #include #include #include From 2c0717901078355f417366b35c152a9568df8e97 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 13 Jun 2024 20:03:17 +0900 Subject: [PATCH 47/47] chore: added missing include Signed-off-by: Kenzo Lobos-Tsunekawa --- .../decoders/continental_packets_decoder.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp index 8760c11b0..933490542 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp @@ -15,6 +15,7 @@ #ifndef NEBULA_WS_CONTINENTAL_PACKETS_DECODER_HPP #define NEBULA_WS_CONTINENTAL_PACKETS_DECODER_HPP +#include #include #include