From 810849e1b8bf9b39d7090a7b04faf876915550fd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mehmet=20Emin=20BA=C5=9EO=C4=9ELU?= Date: Thu, 7 Dec 2023 07:45:54 +0300 Subject: [PATCH] feat: add robosense support (#77) * Added Helios and Bpearl as supported models * Created Robosense hardware interface * Created hardware interface ROS 2 wrapper for Robosense * Creating Robosense decoder * Created Robosense angle corrector * Created Robosense decoder * Created decoder ROS 2 wrapper * Fix pragma pack and packet * Delete forgotten parenthesis * Fix byte order issue * Calculate timestamp for each point * Consider return type * Use scan phase and get distance unit from packet * Format code and convert header guards to #pragma once * Added time offsets for dual return mode * Save calibration file * Get calibration from sensor * Fix stringstream new line issue * Load calibration from sensor by default * Close info packet UDP driver after receiving for the first time * Fix calibration file format * Created hardware monitor * Receive DIFOP packet at the same time * Refactor hardware monitor * Create calibration file name dynamically * Get return mode from DIFOP packet * Delete comments * Get n_returns from sensor config * Added dual return distance threshold * Created LoadFromStream method * Added Bpearl * Get hardware info of Bpearl * Added time offsets for Bpearl * Remove calibration configuration from Bpearl hw monitor * Fix the format of hardware monitor * Check calibration format while loading it * Wait for first DIFOP packet before continuing * Tidied up the code * Fix Bpearl return mode issue * Created robosense_msgs * Use robosense_msgs on hw interface and decoder * Fix package name * Publish both MSOP and DIFOP packets from hw interface ros wrapper * Subscribe packets on decoder ros wrapper * Subscribe to robosense difop packets on hw monitor * Updated MAX_SCAN_BUFFER_POINTS * Fixed point return type * Fixed return mode diagnostic * Fixed calibration sign issue * Fixed return mode types * Deleted unused code * Added lidar model to the packet message * Deleted hw_interface from decoder ros wrapper * Checking model type in decoder ros wrapper * Initialize driver for different Bpearl models * Added Bpearl v4.0 * Added Bpearl v4 diag * Added Bpearl V4 to hw monitor * Fixed packet structs * Fix Bpearl v3 timestamp * Handle sensor timestamp usage * Deleted unnecessary params * Fix info driver initialization * Fixed diagnostic format * Added Robosense to python launch file * Deleted unnecessary parts * Fix Helios range resolution * Fix diagnostic info for Bpearl v3 * Added generic float function * Fixed bpearl diag * Fixed Helios diag * Fixed getSyncStatus * Fix axes * Change Helios 5515 name to only Helios * Add robosense words to .cspell.json * refactor(nebula_common.hpp): convert if chain to switch-case * refactor(nebula_decoders): add "using namespace" to simplify big endian types * refactor(nebula_decoders): use switch statements over if-else * fix(nebula_decoders): add necessary header * refactor(nebula_decoders): get rid of magic numbers * refactor(nebula_decoders): add using namespace for boost:endian * refactor: get rid of magic numbers * chore: add formulas for unpacking difop packet * refactor(robosense_hw_monitor): remove unnecessary log * fix(robosense_packet.hpp): change nanoseconds to microseconds * refactor(robosense_packet.hpp): throw unknown resolution error * chore(cspell): add robosense current and voltage words * fix(robosense): create corrected channel numbers * fix(robosense): handle ptp synchronization --- .cspell.json | 9 +- .../include/nebula_common/nebula_common.hpp | 84 +++- .../robosense/robosense_common.hpp | 202 ++++++++ nebula_decoders/CMakeLists.txt | 10 + .../decoders/angle_corrector.hpp | 57 +++ .../angle_corrector_calibration_based.hpp | 85 ++++ .../decoders/bpearl_v3.hpp | 462 ++++++++++++++++++ .../decoders/bpearl_v4.hpp | 381 +++++++++++++++ .../decoders/helios.hpp | 457 +++++++++++++++++ .../decoders/robosense_decoder.hpp | 268 ++++++++++ .../decoders/robosense_info_decoder.hpp | 84 ++++ .../decoders/robosense_info_decoder_base.hpp | 42 ++ .../decoders/robosense_packet.hpp | 267 ++++++++++ .../decoders/robosense_scan_decoder.hpp | 42 ++ .../decoders/robosense_sensor.hpp | 124 +++++ .../robosense_driver.hpp | 61 +++ .../robosense_info_driver.hpp | 64 +++ nebula_decoders/package.xml | 1 + .../robosense_driver.cpp | 83 ++++ .../robosense_info_driver.cpp | 67 +++ nebula_hw_interfaces/CMakeLists.txt | 5 + .../robosense_hw_interface.hpp | 126 +++++ nebula_hw_interfaces/package.xml | 1 + .../robosense_hw_interface.cpp | 241 +++++++++ nebula_messages/robosense_msgs/CMakeLists.txt | 21 + .../msg/RobosenseInfoPacket.msg | 2 + .../robosense_msgs/msg/RobosensePacket.msg | 2 + .../robosense_msgs/msg/RobosenseScan.msg | 2 + nebula_messages/robosense_msgs/package.xml | 22 + nebula_ros/CMakeLists.txt | 32 ++ nebula_ros/config/robosense/Bpearl.yaml | 11 + nebula_ros/config/robosense/Helios.yaml | 11 + .../robosense_decoder_ros_wrapper.hpp | 99 ++++ .../robosense_hw_interface_ros_wrapper.hpp | 88 ++++ .../robosense_hw_monitor_ros_wrapper.hpp | 114 +++++ nebula_ros/launch/nebula_launch.py | 12 +- nebula_ros/launch/robosense_launch.all_hw.xml | 55 +++ nebula_ros/package.xml | 1 + .../robosense_decoder_ros_wrapper.cpp | 302 ++++++++++++ .../robosense_hw_interface_ros_wrapper.cpp | 169 +++++++ .../robosense_hw_monitor_ros_wrapper.cpp | 192 ++++++++ 41 files changed, 4352 insertions(+), 6 deletions(-) create mode 100644 nebula_common/include/nebula_common/robosense/robosense_common.hpp create mode 100644 nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector.hpp create mode 100644 nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector_calibration_based.hpp create mode 100644 nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v3.hpp create mode 100644 nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v4.hpp create mode 100644 nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/helios.hpp create mode 100644 nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_decoder.hpp create mode 100644 nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder.hpp create mode 100644 nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder_base.hpp create mode 100644 nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp create mode 100644 nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_scan_decoder.hpp create mode 100644 nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_sensor.hpp create mode 100644 nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_driver.hpp create mode 100644 nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_info_driver.hpp create mode 100644 nebula_decoders/src/nebula_decoders_robosense/robosense_driver.cpp create mode 100644 nebula_decoders/src/nebula_decoders_robosense/robosense_info_driver.cpp create mode 100644 nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp create mode 100644 nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp create mode 100644 nebula_messages/robosense_msgs/CMakeLists.txt create mode 100644 nebula_messages/robosense_msgs/msg/RobosenseInfoPacket.msg create mode 100644 nebula_messages/robosense_msgs/msg/RobosensePacket.msg create mode 100644 nebula_messages/robosense_msgs/msg/RobosenseScan.msg create mode 100644 nebula_messages/robosense_msgs/package.xml create mode 100644 nebula_ros/config/robosense/Bpearl.yaml create mode 100644 nebula_ros/config/robosense/Helios.yaml create mode 100644 nebula_ros/include/nebula_ros/robosense/robosense_decoder_ros_wrapper.hpp create mode 100644 nebula_ros/include/nebula_ros/robosense/robosense_hw_interface_ros_wrapper.hpp create mode 100644 nebula_ros/include/nebula_ros/robosense/robosense_hw_monitor_ros_wrapper.hpp create mode 100644 nebula_ros/launch/robosense_launch.all_hw.xml create mode 100644 nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp create mode 100644 nebula_ros/src/robosense/robosense_hw_interface_ros_wrapper.cpp create mode 100644 nebula_ros/src/robosense/robosense_hw_monitor_ros_wrapper.cpp diff --git a/.cspell.json b/.cspell.json index 0015d5dcf..e2b0768d9 100644 --- a/.cspell.json +++ b/.cspell.json @@ -31,6 +31,13 @@ "Vccint", "XT", "XTM", - "DHAVE" + "DHAVE", + "Bpearl", + "Helios", + "Msop", + "Difop", + "gptp", + "Idat", + "Vdat" ] } diff --git a/nebula_common/include/nebula_common/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp index 3baccec73..c75359aa8 100644 --- a/nebula_common/include/nebula_common/nebula_common.hpp +++ b/nebula_common/include/nebula_common/nebula_common.hpp @@ -323,6 +323,10 @@ enum class SensorModel { VELODYNE_VLP32MR, VELODYNE_HDL32, VELODYNE_VLP16, + ROBOSENSE_HELIOS, + ROBOSENSE_BPEARL, + ROBOSENSE_BPEARL_V3, + ROBOSENSE_BPEARL_V4, }; /// @brief not used? @@ -401,6 +405,18 @@ inline std::ostream & operator<<(std::ostream & os, nebula::drivers::SensorModel case SensorModel::VELODYNE_VLP16: os << "VLP16"; break; + case SensorModel::ROBOSENSE_HELIOS: + os << "HELIOS"; + break; + case SensorModel::ROBOSENSE_BPEARL: + os << "BPEARL"; + break; + case SensorModel::ROBOSENSE_BPEARL_V3: + os << "BPEARL V3.0"; + break; + case SensorModel::ROBOSENSE_BPEARL_V4: + os << "BPEARL V4.0"; + break; case SensorModel::UNKNOWN: os << "Sensor Unknown"; break; @@ -424,6 +440,7 @@ struct SensorConfigurationBase double max_range; bool remove_nans; /// todo: consider changing to only_finite std::vector fields; + bool use_sensor_time{false}; }; /// @brief Convert SensorConfigurationBase to string (Overloading the << operator) @@ -436,7 +453,8 @@ inline std::ostream & operator<<( os << "SensorModel: " << arg.sensor_model << ", ReturnMode: " << arg.return_mode << ", HostIP: " << arg.host_ip << ", SensorIP: " << arg.sensor_ip << ", FrameID: " << arg.frame_id << ", DataPort: " << arg.data_port - << ", Frequency: " << arg.frequency_ms << ", MTU: " << arg.packet_mtu_size; + << ", Frequency: " << arg.frequency_ms << ", MTU: " << arg.packet_mtu_size + << ", Use sensor time: " << arg.use_sensor_time; return os; } @@ -468,9 +486,63 @@ inline SensorModel SensorModelFromString(const std::string & sensor_model) if (sensor_model == "VLP32MR") return SensorModel::VELODYNE_VLP32MR; if (sensor_model == "HDL32") return SensorModel::VELODYNE_HDL32; 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_V3") return SensorModel::ROBOSENSE_BPEARL_V3; + if (sensor_model == "Bpearl_V4") return SensorModel::ROBOSENSE_BPEARL_V4; return SensorModel::UNKNOWN; } +inline std::string SensorModelToString(const SensorModel & sensor_model) +{ + switch (sensor_model) { + // Hesai + case SensorModel::HESAI_PANDAR64: + return "Pandar64"; + case SensorModel::HESAI_PANDAR40P: + return "Pandar40P"; + case SensorModel::HESAI_PANDAR40M: + return "Pandar40M"; + case SensorModel::HESAI_PANDARXT32: + return "PandarXT32"; + case SensorModel::HESAI_PANDARXT32M: + return "PandarXT32M"; + case SensorModel::HESAI_PANDARAT128: + return "PandarAT128"; + case SensorModel::HESAI_PANDARQT64: + return "PandarQT64"; + case SensorModel::HESAI_PANDARQT128: + return "PandarQT128"; + case SensorModel::HESAI_PANDAR128_E4X: + return "Pandar128E4X"; + // Velodyne + case SensorModel::VELODYNE_VLS128: + return "VLS128"; + case SensorModel::VELODYNE_HDL64: + return "HDL64"; + case SensorModel::VELODYNE_VLP32: + return "VLP32"; + case SensorModel::VELODYNE_VLP32MR: + return "VLP32MR"; + case SensorModel::VELODYNE_HDL32: + return "HDL32"; + case SensorModel::VELODYNE_VLP16: + return "VLP16"; + // 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"; + default: + return "UNKNOWN"; + } +} + /// @brief Convert return mode name to ReturnMode enum /// @param return_mode Return mode name (Upper and lower case letters must match) /// @return Corresponding ReturnMode @@ -496,12 +568,18 @@ pcl::PointCloud::Ptr convertPointXYZIRCAEDTToPointXYZIRADT( /// @brief Converts degrees to radians /// @param radians /// @return degrees -static inline float deg2rad(double degrees) { return degrees * M_PI / 180.0; } +static inline float deg2rad(double degrees) +{ + return degrees * M_PI / 180.0; +} /// @brief Converts radians to degrees /// @param radians /// @return degrees -static inline float rad2deg(double radians) { return radians * 180.0 / M_PI; } +static inline float rad2deg(double radians) +{ + return radians * 180.0 / M_PI; +} } // namespace drivers } // namespace nebula diff --git a/nebula_common/include/nebula_common/robosense/robosense_common.hpp b/nebula_common/include/nebula_common/robosense/robosense_common.hpp new file mode 100644 index 000000000..e92302f2d --- /dev/null +++ b/nebula_common/include/nebula_common/robosense/robosense_common.hpp @@ -0,0 +1,202 @@ +#pragma once + +#include "nebula_common/nebula_common.hpp" +#include "nebula_common/nebula_status.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ + +// Flag for detecting Bpearl version +constexpr uint8_t BPEARL_V4_FLAG = 0x04; + +/// @brief struct for Robosense sensor configuration +struct RobosenseSensorConfiguration : SensorConfigurationBase +{ + uint16_t gnss_port{}; // difop + double scan_phase{}; // start/end angle + double dual_return_distance_threshold{}; + uint16_t rotation_speed; + uint16_t cloud_min_angle; + uint16_t cloud_max_angle; +}; + +/// @brief Convert RobosenseSensorConfiguration to string (Overloading the << operator) +/// @param os +/// @param arg +/// @return stream +inline std::ostream & operator<<(std::ostream & os, RobosenseSensorConfiguration const & arg) +{ + os << (SensorConfigurationBase)(arg) << ", GnssPort: " << arg.gnss_port + << ", ScanPhase:" << arg.scan_phase << ", RotationSpeed:" << arg.rotation_speed + << ", FOV(Start):" << arg.cloud_min_angle << ", FOV(End):" << arg.cloud_max_angle; + return os; +} + +/// @brief Convert return mode name to ReturnMode enum (Robosense-specific ReturnModeFromString) +/// @param return_mode Return mode name (Upper and lower case letters must match) +/// @return Corresponding ReturnMode +inline ReturnMode ReturnModeFromStringRobosense(const std::string & return_mode) +{ + if (return_mode == "Dual") return ReturnMode::DUAL; + if (return_mode == "Strongest") return ReturnMode::SINGLE_STRONGEST; + if (return_mode == "Last") return ReturnMode::SINGLE_LAST; + if (return_mode == "First") return ReturnMode::SINGLE_FIRST; + + return ReturnMode::UNKNOWN; +} + +size_t GetChannelSize(const SensorModel & model) +{ + switch (model) { + case SensorModel::ROBOSENSE_BPEARL_V3: + return 32; + case SensorModel::ROBOSENSE_HELIOS: + return 32; + } +} + +struct ChannelCorrection +{ + float azimuth{NAN}; + float elevation{NAN}; + uint16_t channel{}; + + [[nodiscard]] bool has_value() const { return !std::isnan(azimuth) && !std::isnan(elevation); } +}; + +/// @brief struct for Robosense calibration configuration +struct RobosenseCalibrationConfiguration : CalibrationConfigurationBase +{ + std::vector calibration; + + void SetChannelSize(const size_t channel_num) { calibration.resize(channel_num); } + + template + inline nebula::Status LoadFromStream(stream_t & stream) + { + std::string header; + std::getline(stream, header); + + char sep; + int laser_id; + float elevation; + float azimuth; + Status load_status = Status::OK; + for (size_t i = 0; i < calibration.size(); ++i) { + stream >> laser_id >> sep >> elevation >> sep >> azimuth; + + if (laser_id <= 0 || laser_id > calibration.size()) { + std::cout << "Invalid laser id: " << laser_id << std::endl; + load_status = Status::INVALID_CALIBRATION_FILE; + } + if (std::isnan(elevation) || std::isnan(azimuth)) { + std::cout << "Invalid calibration data" << laser_id << "," << elevation << "," << azimuth + << std::endl; + load_status = Status::INVALID_CALIBRATION_FILE; + } + if ( + calibration[laser_id - 1].has_value() && calibration[laser_id - 1].elevation != elevation && + calibration[laser_id - 1].azimuth != azimuth) { + std::cout << "Duplicate calibration data for laser id: " << laser_id << std::endl; + load_status = Status::INVALID_CALIBRATION_FILE; + } + + ChannelCorrection correction{azimuth, elevation}; + calibration[laser_id - 1] = correction; + } + + for (auto & calib : calibration) { + if (!calib.has_value()) { + std::cout << calib.elevation << "," << calib.azimuth << std::endl; + std::cout << "Missing calibration data" << std::endl; + load_status = Status::INVALID_CALIBRATION_FILE; + } + } + + if (load_status != Status::OK) { + for (auto & correction : calibration) { + correction.elevation = NAN; + correction.azimuth = NAN; + } + } + + return load_status; + } + + inline nebula::Status LoadFromFile(const std::string & calibration_file) + { + std::ifstream ifs(calibration_file); + if (!ifs) { + return Status::INVALID_CALIBRATION_FILE; + } + + const auto status = LoadFromStream(ifs); + ifs.close(); + return status; + } + + /// @brief Loading calibration data (not used) + /// @param calibration_content + /// @return Resulting status + inline nebula::Status LoadFromString(const std::string & calibration_content) + { + std::stringstream ss; + ss << calibration_content; + + const auto status = LoadFromStream(ss); + return status; + } + + // inline nebula::Status LoadFromDifop(const std::string & calibration_file) + + /// @brief Saving calibration data (not used) + /// @param calibration_file + /// @return Resulting status + inline nebula::Status SaveFile(const std::string & calibration_file) + { + std::ofstream ofs(calibration_file); + if (!ofs) { + return Status::CANNOT_SAVE_FILE; + } + ofs << "Laser id,Elevation,Azimuth" << std::endl; + + for (size_t i = 0; i < calibration.size(); ++i) { + auto laser_id = i + 1; + float elevation = calibration[i].elevation; + float azimuth = calibration[i].azimuth; + ofs << laser_id << "," << elevation << "," << azimuth << std::endl; + } + + ofs.close(); + return Status::OK; + } + + [[nodiscard]] inline ChannelCorrection GetCorrection(const size_t channel_id) const + { + return calibration[channel_id]; + } + + void CreateCorrectedChannels() + { + for(auto& correction : calibration) { + uint16_t channel = 0; + for(const auto& compare:calibration) { + if(compare.elevation < correction.elevation) ++channel; + } + correction.channel = channel; + } + } +}; + +} // namespace drivers +} // namespace nebula diff --git a/nebula_decoders/CMakeLists.txt b/nebula_decoders/CMakeLists.txt index e9b35101c..6f38b97af 100644 --- a/nebula_decoders/CMakeLists.txt +++ b/nebula_decoders/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(PCL REQUIRED) find_package(pcl_conversions REQUIRED) find_package(yaml-cpp REQUIRED) find_package(nebula_common REQUIRED) +find_package(robosense_msgs REQUIRED) include_directories( include @@ -41,6 +42,15 @@ ament_auto_add_library(nebula_decoders_velodyne SHARED src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp ) +# Robosense +ament_auto_add_library(nebula_decoders_robosense SHARED + src/nebula_decoders_robosense/robosense_driver.cpp + ) + +ament_auto_add_library(nebula_decoders_robosense_info SHARED + src/nebula_decoders_robosense/robosense_info_driver.cpp + ) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector.hpp new file mode 100644 index 000000000..60b345861 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector.hpp @@ -0,0 +1,57 @@ +#pragma once + +#include "nebula_common/robosense/robosense_common.hpp" + +#include + +#include +#include + +namespace nebula +{ +namespace drivers +{ + +struct CorrectedAngleData +{ + float azimuth_rad; + float elevation_rad; + float sin_azimuth; + float cos_azimuth; + float sin_elevation; + float cos_elevation; + uint16_t corrected_channel_id; +}; + +/// @brief Handles angle correction for given azimuth/channel combinations, as well as trigonometry +/// lookup tables +class AngleCorrector +{ +protected: + const std::shared_ptr sensor_calibration_; + +public: + explicit AngleCorrector( + const std::shared_ptr & sensor_calibration) + : sensor_calibration_(sensor_calibration) + { + } + + /// @brief Get the corrected azimuth and elevation for a given block and channel, along with their + /// sin/cos values. + /// @param block_azimuth The block's azimuth (including optional fine azimuth), in the sensor's + /// angle unit + /// @param channel_id The laser channel's id + /// @return The corrected angles (azimuth, elevation) in radians and their sin/cos values + virtual CorrectedAngleData getCorrectedAngleData(uint32_t block_azimuth, uint32_t channel_id) = 0; + + /// @brief Returns true if the current azimuth lies in a different (new) scan compared to the last + /// azimuth + /// @param current_azimuth The current azimuth value in the sensor's angle resolution + /// @param last_azimuth The last azimuth in the sensor's angle resolution + /// @return true if the current azimuth is in a different scan than the last one, false otherwise + virtual bool hasScanned(int current_azimuth, int last_azimuth) = 0; +}; + +} // namespace drivers +} // namespace nebula \ No newline at end of file diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector_calibration_based.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector_calibration_based.hpp new file mode 100644 index 000000000..99bd0c02f --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector_calibration_based.hpp @@ -0,0 +1,85 @@ +#pragma once + +#include "nebula_common/robosense/robosense_common.hpp" +#include "nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector.hpp" + +#include + +namespace nebula +{ +namespace drivers +{ + +template +class AngleCorrectorCalibrationBased : public AngleCorrector +{ +private: + static constexpr size_t MAX_AZIMUTH_LEN = 360 * AngleUnit; + + std::array elevation_angle_rad_{}; + std::array azimuth_offset_rad_{}; + std::array block_azimuth_rad_{}; + + std::array elevation_cos_{}; + std::array elevation_sin_{}; + std::array, MAX_AZIMUTH_LEN> azimuth_cos_{}; + std::array, MAX_AZIMUTH_LEN> azimuth_sin_{}; + +public: + explicit AngleCorrectorCalibrationBased( + const std::shared_ptr & sensor_calibration) + : AngleCorrector(sensor_calibration) + { + if (sensor_calibration == nullptr) { + throw std::runtime_error( + "Cannot instantiate AngleCorrectorCalibrationBased without calibration data"); + } + + for (size_t channel_id = 0; channel_id < ChannelN; ++channel_id) { + const auto correction = sensor_calibration->GetCorrection(channel_id); + float elevation_angle_deg = correction.elevation; + float azimuth_offset_deg = correction.azimuth; + + elevation_angle_rad_[channel_id] = deg2rad(elevation_angle_deg); + azimuth_offset_rad_[channel_id] = deg2rad(azimuth_offset_deg); + + elevation_cos_[channel_id] = cosf(elevation_angle_rad_[channel_id]); + elevation_sin_[channel_id] = sinf(elevation_angle_rad_[channel_id]); + } + + for (size_t block_azimuth = 0; block_azimuth < MAX_AZIMUTH_LEN; block_azimuth++) { + block_azimuth_rad_[block_azimuth] = deg2rad(block_azimuth / static_cast(AngleUnit)); + + for (size_t channel_id = 0; channel_id < ChannelN; ++channel_id) { + float precision_azimuth = + block_azimuth_rad_[block_azimuth] + azimuth_offset_rad_[channel_id]; + + azimuth_cos_[block_azimuth][channel_id] = cosf(precision_azimuth); + azimuth_sin_[block_azimuth][channel_id] = sinf(precision_azimuth); + } + } + } + + CorrectedAngleData getCorrectedAngleData(uint32_t block_azimuth, uint32_t channel_id) override + { + float azimuth_rad = block_azimuth_rad_[block_azimuth] + azimuth_offset_rad_[channel_id]; + float elevation_rad = elevation_angle_rad_[channel_id]; + + return { + azimuth_rad, + elevation_rad, + azimuth_sin_[block_azimuth][channel_id], + azimuth_cos_[block_azimuth][channel_id], + elevation_sin_[channel_id], + elevation_cos_[channel_id], + sensor_calibration_->calibration[channel_id].channel}; + } + + bool hasScanned(int current_azimuth, int last_azimuth) override + { + return current_azimuth < last_azimuth; + } +}; + +} // namespace drivers +} // namespace nebula \ No newline at end of file diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v3.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v3.hpp new file mode 100644 index 000000000..a54c52bd6 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v3.hpp @@ -0,0 +1,462 @@ +#pragma once + +#include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp" +#include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_sensor.hpp" + +#include "boost/endian/buffers.hpp" + +#include +#include +#include + +using namespace boost::endian; + +namespace nebula +{ +namespace drivers +{ +namespace robosense_packet +{ +namespace bpearl_v3 +{ +#pragma pack(push, 1) + +struct Timestamp +{ + big_uint8_buf_t year; + big_uint8_buf_t month; + big_uint8_buf_t day; + big_uint8_buf_t hour; + big_uint8_buf_t minute; + big_uint8_buf_t second; + big_uint16_buf_t millisecond; + big_uint16_buf_t microsecond; + + [[nodiscard]] uint64_t get_time_in_ns() const + { + std::tm tm{}; + tm.tm_year = year.value() + 100; + tm.tm_mon = month.value() - 1; // starts from 0 in C + tm.tm_mday = day.value(); + tm.tm_hour = hour.value(); + tm.tm_min = minute.value(); + tm.tm_sec = second.value(); + const uint64_t time = timegm(&tm) * 1000000000ULL + millisecond.value() * 1000000ULL + + microsecond.value() * 1000ULL; + return time; + } +}; + +struct Header +{ + big_uint64_buf_t header_id; + big_uint32_buf_t checksum; + big_uint32_buf_t packet_count; + big_uint32_buf_t reserved_first; + Timestamp timestamp; + big_uint8_buf_t lidar_model; + uint8_t reserved_second[7]; + big_uint16_buf_t temperature; + big_uint16_buf_t top_board_temperature; +}; + +struct Packet : public PacketBase<12, 32, 2, 100> +{ + typedef Body, Packet::N_BLOCKS> body_t; + Header header; + body_t body; + big_uint48_buf_t tail; +}; + +struct OperatingStatus +{ + big_uint48_buf_t reserved; + big_uint16_buf_t v_dat_0v5; + big_uint16_buf_t v_dat_12v; + big_uint16_buf_t v_dat_5v; + big_uint16_buf_t v_dat_1v25; + big_uint16_buf_t v_dat_0v; + big_uint16_buf_t v_dat_1v; +}; + +struct FaultDiagnosis +{ + uint8_t reserved_first[11]; + big_uint16_buf_t manc_err1; + big_uint16_buf_t manc_err2; + big_uint8_buf_t gps_st; + big_uint8_buf_t temperature1; + big_uint8_buf_t temperature2; + uint8_t reserved_second[2]; + big_uint8_buf_t temperature3; + big_uint8_buf_t temperature4; + big_uint8_buf_t temperature5; + big_uint8_buf_t temperature6; + uint8_t reserved_third[7]; + big_uint8_buf_t r_rpm1; + big_uint8_buf_t r_rpm2; + uint8_t reserved_fourth[7]; +}; + +struct InfoPacket +{ + big_uint64_buf_t header; + big_uint16_buf_t motor_speed; + Ethernet ethernet; + FovSetting fov_setting; + big_uint16_buf_t tcp_msop_port; + big_uint16_buf_t phase_lock; + FirmwareVersion top_firmware_version; + FirmwareVersion bottom_firmware_version; + FirmwareVersion bottom_software_version; + FirmwareVersion motor_firmware_version; + uint8_t reserved_first[230]; + big_uint16_buf_t reverse_zero_angle_offset; + SerialNumber serial_number; + big_uint16_buf_t zero_angle_offset; + big_uint8_buf_t return_mode; + big_uint8_buf_t time_sync_mode; + big_uint8_buf_t sync_status; + Timestamp time; + OperatingStatus operating_status; + uint8_t reserved_second[6]; + big_uint8_buf_t rotation_direction; + big_uint32_buf_t elapsed_time_flag; + FaultDiagnosis fault_diagnosis; + big_uint8_buf_t gprmc[86]; + SensorCalibration sensor_calibration; + uint8_t reserved_fourth[586]; + big_uint16_buf_t tail; +}; + +#pragma pack(pop) +} // namespace bpearl_v3 + +/// @brief Get the distance unit of the given @ref BpearlV3 packet in meters. +/// @return 0.005m (0.5cm) +template <> +inline double get_dis_unit(const bpearl_v3::Packet & /* packet */) +{ + return 0.005; +} + +} // namespace robosense_packet + +class BpearlV3 : public RobosenseSensor< + robosense_packet::bpearl_v3::Packet, robosense_packet::bpearl_v3::InfoPacket> +{ +private: + static constexpr int firing_time_offset_ns_single_[12][32]{ + {0, 256, 512, 768, 1024, 1280, 1536, 1792, 2568, 2824, 3080, 3336, 3592, 3848, 4104, 4360, + 128, 384, 640, 896, 1152, 1408, 1664, 1920, 2696, 2952, 3208, 3464, 3720, 3976, 4232, 4488}, + {5552, 5808, 6064, 6320, 6576, 6832, 7088, 7344, 8120, 8376, 8632, + 8888, 9144, 9400, 9656, 9912, 5680, 5936, 6192, 6448, 6704, 6960, + 7216, 7472, 8248, 8504, 8760, 9016, 9272, 9528, 9784, 10040}, + {11104, 11360, 11616, 11872, 12128, 12384, 12640, 12896, 13672, 13928, 14184, + 14440, 14696, 14952, 15208, 15464, 11232, 11488, 11744, 12000, 12256, 12512, + 12768, 13024, 13800, 14056, 14312, 14568, 14824, 15080, 15336, 15592}, + {16656, 16912, 17168, 17424, 17680, 17936, 18192, 18448, 19224, 19480, 19736, + 19992, 20248, 20504, 20760, 21016, 16784, 17040, 17296, 17552, 17808, 18064, + 18320, 18576, 19352, 19608, 19864, 20120, 20376, 20632, 20888, 21144}, + {22208, 22464, 22720, 22976, 23232, 23488, 23744, 24000, 24776, 25032, 25288, + 25544, 25800, 26056, 26312, 26568, 22336, 22592, 22848, 23104, 23360, 23616, + 23872, 24128, 24904, 25160, 25416, 25672, 25928, 26184, 26440, 26696}, + {27760, 28016, 28272, 28528, 28784, 29040, 29296, 29552, 30328, 30584, 30840, + 31096, 31352, 31608, 31864, 32120, 27888, 28144, 28400, 28656, 28912, 29168, + 29424, 29680, 30456, 30712, 30968, 31224, 31480, 31736, 31992, 32248}, + {33312, 33568, 33824, 34080, 34336, 34592, 34848, 35104, 35880, 36136, 36392, + 36648, 36904, 37160, 37416, 37672, 33440, 33696, 33952, 34208, 34464, 34720, + 34976, 35232, 36008, 36264, 36520, 36776, 37032, 37288, 37544, 37800}, + {38864, 39120, 39376, 39632, 39888, 40144, 40400, 40656, 41432, 41688, 41944, + 42200, 42456, 42712, 42968, 43224, 38992, 39248, 39504, 39760, 40016, 40272, + 40528, 40784, 41560, 41816, 42072, 42328, 42584, 42840, 43096, 43352}, + {44416, 44672, 44928, 45184, 45440, 45696, 45952, 46208, 46984, 47240, 47496, + 47752, 48008, 48264, 48520, 48776, 44544, 44800, 45056, 45312, 45568, 45824, + 46080, 46336, 47112, 47368, 47624, 47880, 48136, 48392, 48648, 48904}, + {49968, 50224, 50480, 50736, 50992, 51248, 51504, 51760, 52536, 52792, 53048, + 53304, 53560, 53816, 54072, 54328, 50096, 50352, 50608, 50864, 51120, 51376, + 51632, 51888, 52664, 52920, 53176, 53432, 53688, 53944, 54200, 54456}, + {55520, 55776, 56032, 56288, 56544, 56800, 57056, 57312, 58088, 58344, 58600, + 58856, 59112, 59368, 59624, 59880, 55648, 55904, 56160, 56416, 56672, 56928, + 57184, 57440, 58216, 58472, 58728, 58984, 59240, 59496, 59752, 60008}, + {61072, 61328, 61584, 61840, 62096, 62352, 62608, 62864, 63640, 63896, 64152, + 64408, 64664, 64920, 65176, 65432, 61200, 61456, 61712, 61968, 62224, 62480, + 62736, 62992, 63768, 64024, 64280, 64536, 64792, 65048, 65304, 65560}}; + + static constexpr int firing_time_offset_ns_dual_[12][32]{ + {0, 256, 512, 768, 1024, 1280, 1536, 1792, 2568, 2824, 3080, 3336, 3592, 3848, 4104, 4360, + 128, 384, 640, 896, 1152, 1408, 1664, 1920, 2696, 2952, 3208, 3464, 3720, 3976, 4232, 4488}, + {0, 256, 512, 768, 1024, 1280, 1536, 1792, 2568, 2824, 3080, 3336, 3592, 3848, 4104, 4360, + 128, 384, 640, 896, 1152, 1408, 1664, 1920, 2696, 2952, 3208, 3464, 3720, 3976, 4232, 4488}, + {5552, 5808, 6064, 6320, 6576, 6832, 7088, 7344, 8120, 8376, 8632, + 8888, 9144, 9400, 9656, 9912, 5680, 5936, 6192, 6448, 6704, 6960, + 7216, 7472, 8248, 8504, 8760, 9016, 9272, 9528, 9784, 10040}, + {5552, 5808, 6064, 6320, 6576, 6832, 7088, 7344, 8120, 8376, 8632, + 8888, 9144, 9400, 9656, 9912, 5680, 5936, 6192, 6448, 6704, 6960, + 7216, 7472, 8248, 8504, 8760, 9016, 9272, 9528, 9784, 10040}, + {11104, 11360, 11616, 11872, 12128, 12384, 12640, 12896, 13672, 13928, 14184, + 14440, 14696, 14952, 15208, 15464, 11232, 11488, 11744, 12000, 12256, 12512, + 12768, 13024, 13800, 14056, 14312, 14568, 14824, 15080, 15336, 15592}, + {11104, 11360, 11616, 11872, 12128, 12384, 12640, 12896, 13672, 13928, 14184, + 14440, 14696, 14952, 15208, 15464, 11232, 11488, 11744, 12000, 12256, 12512, + 12768, 13024, 13800, 14056, 14312, 14568, 14824, 15080, 15336, 15592}, + {16656, 16912, 17168, 17424, 17680, 17936, 18192, 18448, 19224, 19480, 19736, + 19992, 20248, 20504, 20760, 21016, 16784, 17040, 17296, 17552, 17808, 18064, + 18320, 18576, 19352, 19608, 19864, 20120, 20376, 20632, 20888, 21144}, + {16656, 16912, 17168, 17424, 17680, 17936, 18192, 18448, 19224, 19480, 19736, + 19992, 20248, 20504, 20760, 21016, 16784, 17040, 17296, 17552, 17808, 18064, + 18320, 18576, 19352, 19608, 19864, 20120, 20376, 20632, 20888, 21144}, + {22208, 22464, 22720, 22976, 23232, 23488, 23744, 24000, 24776, 25032, 25288, + 25544, 25800, 26056, 26312, 26568, 22336, 22592, 22848, 23104, 23360, 23616, + 23872, 24128, 24904, 25160, 25416, 25672, 25928, 26184, 26440, 26696}, + {22208, 22464, 22720, 22976, 23232, 23488, 23744, 24000, 24776, 25032, 25288, + 25544, 25800, 26056, 26312, 26568, 22336, 22592, 22848, 23104, 23360, 23616, + 23872, 24128, 24904, 25160, 25416, 25672, 25928, 26184, 26440, 26696}, + {27760, 28016, 28272, 28528, 28784, 29040, 29296, 29552, 30328, 30584, 30840, + 31096, 31352, 31608, 31864, 32120, 27888, 28144, 28400, 28656, 28912, 29168, + 29424, 29680, 30456, 30712, 30968, 31224, 31480, 31736, 31992, 32248}, + {27760, 28016, 28272, 28528, 28784, 29040, 29296, 29552, 30328, 30584, 30840, + 31096, 31352, 31608, 31864, 32120, 27888, 28144, 28400, 28656, 28912, 29168, + 29424, 29680, 30456, 30712, 30968, 31224, 31480, 31736, 31992, 32248}}; + + static constexpr uint8_t DUAL_RETURN_FLAG = 0x00; + static constexpr uint8_t STRONGEST_RETURN_FLAG = 0x01; + static constexpr uint8_t LAST_RETURN_FLAG = 0x02; + + static constexpr uint8_t SYNC_MODE_GPS_FLAG = 0x00; + static constexpr uint8_t SYNC_MODE_E2E_FLAG = 0x01; + static constexpr uint8_t SYNC_MODE_P2P_FLAG = 0x02; + static constexpr uint8_t SYNC_MODE_GPTP_FLAG = 0x03; + + static constexpr uint8_t SYNC_STATUS_INVALID_FLAG = 0x00; + static constexpr uint8_t SYNC_STATUS_GPS_SUCCESS_FLAG = 0x01; + static constexpr uint8_t SYNC_STATUS_PTP_SUCCESS_FLAG = 0x02; + +public: + static constexpr float MIN_RANGE = 0.1f; + static constexpr float MAX_RANGE = 30.f; + static constexpr size_t MAX_SCAN_BUFFER_POINTS = 1152000; + + int getPacketRelativePointTimeOffset( + const uint32_t block_id, const uint32_t channel_id, + const std::shared_ptr & sensor_configuration) override + { + if (sensor_configuration->return_mode == ReturnMode::DUAL) + return firing_time_offset_ns_dual_[block_id][channel_id]; + else + return firing_time_offset_ns_single_[block_id][channel_id]; + } + + ReturnMode getReturnMode(const robosense_packet::bpearl_v3::InfoPacket & info_packet) + { + switch (info_packet.return_mode.value()) { + case DUAL_RETURN_FLAG: + return ReturnMode::DUAL; + case STRONGEST_RETURN_FLAG: + return ReturnMode::SINGLE_STRONGEST; + case LAST_RETURN_FLAG: + return ReturnMode::SINGLE_LAST; + default: + return ReturnMode::UNKNOWN; + } + } + + RobosenseCalibrationConfiguration getSensorCalibration( + const robosense_packet::bpearl_v3::InfoPacket & info_packet) + { + return info_packet.sensor_calibration.getCalibration(); + } + + bool getSyncStatus(const robosense_packet::bpearl_v3::InfoPacket & info_packet) + { + switch (info_packet.time_sync_mode.value()) { + case SYNC_MODE_GPS_FLAG: + return true; + case SYNC_MODE_E2E_FLAG: + return true; + case SYNC_MODE_P2P_FLAG: + return true; + case SYNC_MODE_GPTP_FLAG: + return true; + default: + return false; + } + } + + std::map getSensorInfo( + const robosense_packet::bpearl_v3::InfoPacket & info_packet) + { + std::map sensor_info; + sensor_info["motor_speed"] = std::to_string(info_packet.motor_speed.value()); + sensor_info["lidar_ip"] = info_packet.ethernet.lidar_ip.to_string(); + sensor_info["dest_pc_ip"] = info_packet.ethernet.dest_pc_ip.to_string(); + sensor_info["mac_addr"] = info_packet.ethernet.mac_addr.to_string(); + sensor_info["lidar_out_msop_port"] = + std::to_string(info_packet.ethernet.lidar_out_msop_port.value()); + sensor_info["lidar_out_difop_port"] = + std::to_string(info_packet.ethernet.lidar_out_difop_port.value()); + sensor_info["fov_start"] = + robosense_packet::get_float_value(info_packet.fov_setting.fov_start.value()); + sensor_info["fov_end"] = + robosense_packet::get_float_value(info_packet.fov_setting.fov_end.value()); + sensor_info["tcp_msop_port"] = std::to_string(info_packet.tcp_msop_port.value()); + sensor_info["phase_lock"] = std::to_string(info_packet.phase_lock.value()); + sensor_info["top_firmware_version"] = info_packet.top_firmware_version.to_string(); + sensor_info["bottom_firmware_version"] = info_packet.bottom_firmware_version.to_string(); + sensor_info["bottom_software_version"] = info_packet.bottom_software_version.to_string(); + sensor_info["motor_firmware_version"] = info_packet.motor_firmware_version.to_string(); + sensor_info["reverse_zero_angle_offset"] = + std::to_string(info_packet.reverse_zero_angle_offset.value()); + sensor_info["serial_number"] = info_packet.serial_number.to_string(); + sensor_info["zero_angle_offset"] = std::to_string(info_packet.zero_angle_offset.value()); + + switch (info_packet.return_mode.value()) { + case DUAL_RETURN_FLAG: + sensor_info["return_mode"] = "dual"; + break; + case STRONGEST_RETURN_FLAG: + sensor_info["return_mode"] = "strongest"; + break; + case LAST_RETURN_FLAG: + sensor_info["return_mode"] = "last"; + break; + default: + sensor_info["return_mode"] = "n/a"; + break; + } + + switch (info_packet.time_sync_mode.value()) { + case SYNC_MODE_GPS_FLAG: + sensor_info["time_sync_mode"] = "gps"; + break; + case SYNC_MODE_E2E_FLAG: + sensor_info["time_sync_mode"] = "e2e"; + break; + case SYNC_MODE_P2P_FLAG: + sensor_info["time_sync_mode"] = "p2p"; + break; + case SYNC_MODE_GPTP_FLAG: + sensor_info["time_sync_mode"] = "gptp"; + break; + default: + sensor_info["time_sync_mode"] = "n/a"; + break; + } + + switch (info_packet.sync_status.value()) { + case SYNC_STATUS_INVALID_FLAG: + sensor_info["sync_status"] = "time_sync_invalid"; + break; + case SYNC_STATUS_GPS_SUCCESS_FLAG: + sensor_info["sync_status"] = "gps_time_sync_successful"; + break; + case SYNC_STATUS_PTP_SUCCESS_FLAG: + sensor_info["sync_status"] = "ptp_time_sync_successful"; + break; + default: + sensor_info["sync_status"] = "n/a"; + } + + sensor_info["time"] = std::to_string(info_packet.time.get_time_in_ns()); + sensor_info["v_dat_0v5"] = + robosense_packet::get_float_value(info_packet.operating_status.v_dat_0v5.value()); + sensor_info["v_dat_12v"] = + robosense_packet::get_float_value(info_packet.operating_status.v_dat_12v.value()); + sensor_info["v_dat_5v"] = + robosense_packet::get_float_value(info_packet.operating_status.v_dat_5v.value()); + sensor_info["v_dat_1v25"] = + robosense_packet::get_float_value(info_packet.operating_status.v_dat_1v25.value()); + sensor_info["v_dat_0v"] = + robosense_packet::get_float_value(info_packet.operating_status.v_dat_0v.value()); + sensor_info["v_dat_1v"] = + robosense_packet::get_float_value(info_packet.operating_status.v_dat_1v.value()); + sensor_info["rotation_direction"] = std::to_string(info_packet.rotation_direction.value()); + sensor_info["elapsed_time_flag"] = std::to_string(info_packet.elapsed_time_flag.value()); + + sensor_info["manc_err1"] = std::to_string(info_packet.fault_diagnosis.manc_err1.value()); + sensor_info["manc_err2"] = std::to_string(info_packet.fault_diagnosis.manc_err2.value()); + + const std::bitset<8> gps_st_bits{info_packet.fault_diagnosis.gps_st.value()}; + if (gps_st_bits[0]) + sensor_info["pps_lock"] = "valid"; + else + sensor_info["pps_lock"] = "invalid"; + if (gps_st_bits[1]) + sensor_info["gprmc_lock"] = "valid"; + else + sensor_info["gprmc_lock"] = "invalid"; + if (gps_st_bits[2]) + sensor_info["utc_lock"] = "synchronized"; + else + sensor_info["utc_lock"] = "not_synchronized"; + if (gps_st_bits[3]) + sensor_info["gprmc_input_status"] = "received_gprmc"; + else + sensor_info["gprmc_input_status"] = "no_gprmc"; + if (gps_st_bits[4]) + sensor_info["pps_input_status"] = "received_pps"; + else + sensor_info["pps_input_status"] = "no_pps"; + + /* + * From the manual, here is the formula for calculating bottom board temperature: + * Temp =（Value（temp1）* 256 + Value（temp2）& 0Xffff）* 503.975 / 4096.0 - 273.15 + */ + sensor_info["bottom_board_temp"] = std::to_string( + (static_cast(info_packet.fault_diagnosis.temperature1.value()) * 256 + + static_cast(info_packet.fault_diagnosis.temperature2.value())) * + 503.975 / 4096.0 - + 273.15); + + /* + * From the manual, here is the formula for calculating APD temperature: + * When Value ≤ 32768, the temperature value is positive; otherwise negative. + * Temp = ( (value(362) * 256 + value(363) ) & 0x7FF8 ) / 128.0 + */ + double apd_temp = ((info_packet.fault_diagnosis.temperature3.value() * 256 + + info_packet.fault_diagnosis.temperature4.value()) & + 0x7FF8) / + 128.0; + if ( + ((info_packet.fault_diagnosis.temperature3.value() << 8)) + + info_packet.fault_diagnosis.temperature4.value() > + 32768) + apd_temp = -apd_temp; + sensor_info["apd_temp"] = std::to_string(apd_temp); + + /* + * From the manual, here is the formula for calculating top board temperature: + * When Value ≤ 32768, the temperature value is positive; otherwise negative. + * Temp= ( (value(362) * 256 + value(363) ) & 0x7FF8 ) / 128.0 + */ + double top_board_temp = ((info_packet.fault_diagnosis.temperature5.value() * 256 + + info_packet.fault_diagnosis.temperature6.value()) & + 0x7FF8) / + 128.0; + if ( + ((info_packet.fault_diagnosis.temperature5.value() << 8)) + + info_packet.fault_diagnosis.temperature6.value() > + 32768) + top_board_temp = -top_board_temp; + sensor_info["top_board_temp"] = std::to_string(top_board_temp); + + /* + * From the manual, here is the formula for calculating real time rotation speed: + * Speed = (256 * r_rpm1 + r_rpm2) / 6 + */ + sensor_info["real_time_rot_speed"] = std::to_string( + (info_packet.fault_diagnosis.r_rpm1.value() * 256 + + info_packet.fault_diagnosis.r_rpm2.value()) / + 6); + + std::string gprmc_string; + for (auto i : info_packet.gprmc) { + gprmc_string += static_cast(i.value()); + } + sensor_info["gprmc_string"] = gprmc_string; + + return sensor_info; + } +}; +} // namespace drivers +} // namespace nebula \ No newline at end of file diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v4.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v4.hpp new file mode 100644 index 000000000..80b0c35ec --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v4.hpp @@ -0,0 +1,381 @@ +#pragma once + +#include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp" +#include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_sensor.hpp" + +#include "boost/endian/buffers.hpp" + +#include +#include +#include + +using namespace boost::endian; + +namespace nebula +{ +namespace drivers +{ +namespace robosense_packet +{ +namespace bpearl_v4 +{ +#pragma pack(push, 1) + +struct Header +{ + big_uint64_buf_t header_id; + uint8_t reserved_first[4]; + big_uint32_buf_t packet_count; + uint8_t reserved_second[4]; + Timestamp timestamp; + uint8_t reserved_third[1]; + big_uint8_buf_t lidar_type; + big_uint8_buf_t lidar_model; + uint8_t reserved_fourth[9]; +}; + +struct Packet : public PacketBase<12, 32, 2, 100> +{ + typedef Body, Packet::N_BLOCKS> body_t; + Header header; + body_t body; + big_uint48_buf_t tail; +}; + +struct OperatingStatus +{ + big_uint8_buf_t reserved_first; + big_uint16_buf_t machine_current; + big_uint24_buf_t reserved_second; + big_uint16_buf_t machine_voltage; + uint8_t reserved_third[16]; +}; + +struct FaultDiagnosis +{ + big_uint16_buf_t startup_times; + big_uint32_buf_t reserved; + big_uint8_buf_t gps_status; + big_uint16_buf_t machine_temp; + uint8_t reserved_first[11]; + big_uint16_buf_t phase; + big_uint16_buf_t rotation_speed; +}; + +struct InfoPacket +{ + big_uint64_buf_t header; + big_uint16_buf_t motor_speed_setting; + Ethernet ethernet; + FovSetting fov_setting; + big_uint16_buf_t tcp_msop_port; + big_uint16_buf_t phase_lock; + FirmwareVersion mainboard_firmware_version; + FirmwareVersion bottom_firmware_version; + FirmwareVersion app_software_version; + FirmwareVersion motor_firmware_version; + uint8_t reserved_first[228]; + big_uint8_buf_t baud_rate; + uint8_t reserved_second[3]; + SerialNumber serial_number; + uint8_t reserved_third[2]; + big_uint8_buf_t return_mode; + big_uint8_buf_t time_sync_mode; + big_uint8_buf_t time_sync_state; + Timestamp time; + OperatingStatus operating_status; + big_uint8_buf_t rotation_direction; + big_uint32_buf_t running_time; + uint8_t reserved_fourth[9]; + FaultDiagnosis fault_diagnosis; + uint8_t reserved_fifth[7]; + big_uint8_buf_t gprmc[86]; + SensorCalibration sensor_calibration; + uint8_t reserved_sixth[586]; + big_uint16_buf_t tail; +}; + +#pragma pack(pop) +} // namespace bpearl_v4 + +/// @brief Get the distance unit of the given @ref BpearlV3 packet in meters. +/// @return 0.0025m (0.25cm) +template <> +inline double get_dis_unit(const bpearl_v4::Packet & /* packet */) +{ + return 0.0025; +} + +} // namespace robosense_packet + +class BpearlV4 : public RobosenseSensor< + robosense_packet::bpearl_v4::Packet, robosense_packet::bpearl_v4::InfoPacket> +{ +private: + static constexpr int firing_time_offset_ns_single_[12][32] = { + {0, 167, 334, 500, 667, 834, 1001, 1168, 1334, 1501, 1668, + 1835, 2002, 2168, 2335, 2502, 2669, 2836, 3002, 3169, 3336, 3503, + 3670, 3836, 4003, 4170, 4337, 4504, 4670, 4837, 5004, 5171}, + {5555, 5722, 5889, 6055, 6222, 6389, 6556, 6723, 6889, 7056, 7223, + 7390, 7557, 7723, 7890, 8057, 8224, 8391, 8557, 8724, 8891, 9058, + 9225, 9391, 9558, 9725, 9892, 10059, 10225, 10392, 10559, 10726}, + {11110, 11277, 11444, 11610, 11777, 11944, 12111, 12278, 12444, 12611, 12778, + 12945, 13112, 13278, 13445, 13612, 13779, 13946, 14112, 14279, 14446, 14613, + 14780, 14946, 15113, 15280, 15447, 15614, 15780, 15947, 16114, 16281}, + {16665, 16832, 16999, 17165, 17332, 17499, 17666, 17833, 17999, 18166, 18333, + 18500, 18667, 18833, 19000, 19167, 19334, 19501, 19667, 19834, 20001, 20168, + 20335, 20501, 20668, 20835, 21002, 21169, 21335, 21502, 21669, 21836}, + {22220, 22387, 22554, 22720, 22887, 23054, 23221, 23388, 23554, 23721, 23888, + 24055, 24222, 24388, 24555, 24722, 24889, 25056, 25222, 25389, 25556, 25723, + 25890, 26056, 26223, 26390, 26557, 26724, 26890, 27057, 27224, 27391}, + {27775, 27942, 28109, 28275, 28442, 28609, 28776, 28943, 29109, 29276, 29443, + 29610, 29777, 29943, 30110, 30277, 30444, 30611, 30777, 30944, 31111, 31278, + 31445, 31611, 31778, 31945, 32112, 32279, 32445, 32612, 32779, 32946}, + {33330, 33497, 33664, 33830, 33997, 34164, 34331, 34498, 34664, 34831, 34998, + 35165, 35332, 35498, 35665, 35832, 35999, 36166, 36332, 36499, 36666, 36833, + 37000, 37166, 37333, 37500, 37667, 37834, 38000, 38167, 38334, 38501}, + {38885, 39052, 39219, 39385, 39552, 39719, 39886, 40053, 40219, 40386, 40553, + 40720, 40887, 41053, 41220, 41387, 41554, 41721, 41887, 42054, 42221, 42388, + 42555, 42721, 42888, 43055, 43222, 43389, 43555, 43722, 43889, 44056}, + {44440, 44607, 44774, 44940, 45107, 45274, 45441, 45608, 45774, 45941, 46108, + 46275, 46442, 46608, 46775, 46942, 47109, 47276, 47442, 47609, 47776, 47943, + 48110, 48276, 48443, 48610, 48777, 48944, 49110, 49277, 49444, 49611}, + {49995, 50162, 50329, 50495, 50662, 50829, 50996, 51163, 51329, 51496, 51663, + 51830, 51997, 52163, 52330, 52497, 52664, 52831, 52997, 53164, 53331, 53498, + 53665, 53831, 53998, 54165, 54332, 54499, 54665, 54832, 54999, 55166}, + {55550, 55717, 55884, 56050, 56217, 56384, 56551, 56718, 56884, 57051, 57218, + 57385, 57552, 57718, 57885, 58052, 58219, 58386, 58552, 58719, 58886, 59053, + 59220, 59386, 59553, 59720, 59887, 60054, 60220, 60387, 60554, 60721}, + {61105, 61272, 61439, 61605, 61772, 61939, 62106, 62273, 62439, 62606, 62773, + 62940, 63107, 63273, 63440, 63607, 63774, 63941, 64107, 64274, 64441, 64608, + 64775, 64941, 65108, 65275, 65442, 65609, 65775, 65942, 66109, 66276}}; + + static constexpr int firing_time_offset_ns_dual_[12][32]{ + {0, 167, 334, 500, 667, 834, 1001, 1168, 1334, 1501, 1668, + 1835, 2002, 2168, 2335, 2502, 2669, 2836, 3002, 3169, 3336, 3503, + 3670, 3836, 4003, 4170, 4337, 4504, 4670, 4837, 5004, 5171}, + {0, 167, 334, 500, 667, 834, 1001, 1168, 1334, 1501, 1668, + 1835, 2002, 2168, 2335, 2502, 2669, 2836, 3002, 3169, 3336, 3503, + 3670, 3836, 4003, 4170, 4337, 4504, 4670, 4837, 5004, 5171}, + {5555, 5722, 5889, 6055, 6222, 6389, 6556, 6723, 6889, 7056, 7223, + 7390, 7557, 7723, 7890, 8057, 8224, 8391, 8557, 8724, 8891, 9058, + 9225, 9391, 9558, 9725, 9892, 10059, 10225, 10392, 10559, 10726}, + {5555, 5722, 5889, 6055, 6222, 6389, 6556, 6723, 6889, 7056, 7223, + 7390, 7557, 7723, 7890, 8057, 8224, 8391, 8557, 8724, 8891, 9058, + 9225, 9391, 9558, 9725, 9892, 10059, 10225, 10392, 10559, 10726}, + {11110, 11277, 11444, 11610, 11777, 11944, 12111, 12278, 12444, 12611, 12778, + 12945, 13112, 13278, 13445, 13612, 13779, 13946, 14112, 14279, 14446, 14613, + 14780, 14946, 15113, 15280, 15447, 15614, 15780, 15947, 16114, 16281}, + {11110, 11277, 11444, 11610, 11777, 11944, 12111, 12278, 12444, 12611, 12778, + 12945, 13112, 13278, 13445, 13612, 13779, 13946, 14112, 14279, 14446, 14613, + 14780, 14946, 15113, 15280, 15447, 15614, 15780, 15947, 16114, 16281}, + {16665, 16832, 16999, 17165, 17332, 17499, 17666, 17833, 17999, 18166, 18333, + 18500, 18667, 18833, 19000, 19167, 19334, 19501, 19667, 19834, 20001, 20168, + 20335, 20501, 20668, 20835, 21002, 21169, 21335, 21502, 21669, 21836}, + {16665, 16832, 16999, 17165, 17332, 17499, 17666, 17833, 17999, 18166, 18333, + 18500, 18667, 18833, 19000, 19167, 19334, 19501, 19667, 19834, 20001, 20168, + 20335, 20501, 20668, 20835, 21002, 21169, 21335, 21502, 21669, 21836}, + {22220, 22387, 22554, 22720, 22887, 23054, 23221, 23388, 23554, 23721, 23888, + 24055, 24222, 24388, 24555, 24722, 24889, 25056, 25222, 25389, 25556, 25723, + 25890, 26056, 26223, 26390, 26557, 26724, 26890, 27057, 27224, 27391}, + {22220, 22387, 22554, 22720, 22887, 23054, 23221, 23388, 23554, 23721, 23888, + 24055, 24222, 24388, 24555, 24722, 24889, 25056, 25222, 25389, 25556, 25723, + 25890, 26056, 26223, 26390, 26557, 26724, 26890, 27057, 27224, 27391}, + {27775, 27942, 28109, 28275, 28442, 28609, 28776, 28943, 29109, 29276, 29443, + 29610, 29777, 29943, 30110, 30277, 30444, 30611, 30777, 30944, 31111, 31278, + 31445, 31611, 31778, 31945, 32112, 32279, 32445, 32612, 32779, 32946}, + {27775, 27942, 28109, 28275, 28442, 28609, 28776, 28943, 29109, 29276, 29443, + 29610, 29777, 29943, 30110, 30277, 30444, 30611, 30777, 30944, 31111, 31278, + 31445, 31611, 31778, 31945, 32112, 32279, 32445, 32612, 32779, 32946}}; + + static constexpr uint8_t DUAL_RETURN_FLAG = 0x00; + static constexpr uint8_t STRONGEST_RETURN_FLAG = 0x04; + static constexpr uint8_t LAST_RETURN_FLAG = 0x05; + static constexpr uint8_t FIRST_RETURN_FLAG = 0x06; + + static constexpr uint8_t SYNC_MODE_GPS_FLAG = 0x00; + static constexpr uint8_t SYNC_MODE_E2E_FLAG = 0x01; + static constexpr uint8_t SYNC_MODE_P2P_FLAG = 0x02; + static constexpr uint8_t SYNC_MODE_GPTP_FLAG = 0x03; + + static constexpr uint8_t SYNC_STATUS_INVALID_FLAG = 0x00; + static constexpr uint8_t SYNC_STATUS_GPS_SUCCESS_FLAG = 0x01; + static constexpr uint8_t SYNC_STATUS_PTP_SUCCESS_FLAG = 0x02; + +public: + static constexpr float MIN_RANGE = 0.1f; + static constexpr float MAX_RANGE = 30.f; + static constexpr size_t MAX_SCAN_BUFFER_POINTS = 1152000; + + int getPacketRelativePointTimeOffset( + const uint32_t block_id, const uint32_t channel_id, + const std::shared_ptr & sensor_configuration) override + { + if (sensor_configuration->return_mode == ReturnMode::DUAL) + return firing_time_offset_ns_dual_[block_id][channel_id]; + else + return firing_time_offset_ns_single_[block_id][channel_id]; + } + + ReturnMode getReturnMode(const robosense_packet::bpearl_v4::InfoPacket & info_packet) + { + switch (info_packet.return_mode.value()) { + case DUAL_RETURN_FLAG: + return ReturnMode::DUAL; + case STRONGEST_RETURN_FLAG: + return ReturnMode::SINGLE_STRONGEST; + case LAST_RETURN_FLAG: + return ReturnMode::SINGLE_LAST; + case FIRST_RETURN_FLAG: + return ReturnMode::SINGLE_FIRST; + default: + return ReturnMode::UNKNOWN; + } + } + + RobosenseCalibrationConfiguration getSensorCalibration( + const robosense_packet::bpearl_v4::InfoPacket & info_packet) + { + return info_packet.sensor_calibration.getCalibration(); + } + + bool getSyncStatus(const robosense_packet::bpearl_v4::InfoPacket & info_packet) + { + switch (info_packet.time_sync_mode.value()) { + case SYNC_MODE_GPS_FLAG: + return true; + case SYNC_MODE_E2E_FLAG: + return true; + case SYNC_MODE_P2P_FLAG: + return true; + case SYNC_MODE_GPTP_FLAG: + return true; + default: + return false; + } + } + + std::map getSensorInfo( + const robosense_packet::bpearl_v4::InfoPacket & info_packet) + { + std::map sensor_info; + sensor_info["motor_speed"] = std::to_string(info_packet.motor_speed_setting.value()); + sensor_info["lidar_ip"] = info_packet.ethernet.lidar_ip.to_string(); + sensor_info["dest_pc_ip"] = info_packet.ethernet.dest_pc_ip.to_string(); + sensor_info["mac_addr"] = info_packet.ethernet.mac_addr.to_string(); + sensor_info["lidar_out_msop_port"] = + std::to_string(info_packet.ethernet.lidar_out_msop_port.value()); + sensor_info["lidar_out_difop_port"] = + std::to_string(info_packet.ethernet.lidar_out_difop_port.value()); + sensor_info["fov_start"] = + robosense_packet::get_float_value(info_packet.fov_setting.fov_start.value()); + sensor_info["fov_end"] = + robosense_packet::get_float_value(info_packet.fov_setting.fov_end.value()); + sensor_info["tcp_msop_port"] = std::to_string(info_packet.tcp_msop_port.value()); + sensor_info["phase_lock"] = std::to_string(info_packet.phase_lock.value()); + sensor_info["mainboard_firmware_version"] = info_packet.mainboard_firmware_version.to_string(); + sensor_info["bottom_firmware_version"] = info_packet.bottom_firmware_version.to_string(); + sensor_info["app_software_version"] = info_packet.app_software_version.to_string(); + sensor_info["motor_firmware_version"] = info_packet.motor_firmware_version.to_string(); + sensor_info["baud_rate"] = std::to_string(info_packet.baud_rate.value()); + sensor_info["serial_number"] = info_packet.serial_number.to_string(); + + switch (info_packet.return_mode.value()) { + case DUAL_RETURN_FLAG: + sensor_info["return_mode"] = "dual"; + break; + case STRONGEST_RETURN_FLAG: + sensor_info["return_mode"] = "strongest"; + break; + case LAST_RETURN_FLAG: + sensor_info["return_mode"] = "last"; + break; + case FIRST_RETURN_FLAG: + sensor_info["return_mode"] = "first"; + break; + default: + sensor_info["return_mode"] = "n/a"; + break; + } + + switch (info_packet.time_sync_mode.value()) { + case SYNC_MODE_GPS_FLAG: + sensor_info["time_sync_mode"] = "gps"; + break; + case SYNC_MODE_E2E_FLAG: + sensor_info["time_sync_mode"] = "e2e"; + break; + case SYNC_MODE_P2P_FLAG: + sensor_info["time_sync_mode"] = "p2p"; + break; + case SYNC_MODE_GPTP_FLAG: + sensor_info["time_sync_mode"] = "gptp"; + break; + default: + sensor_info["time_sync_mode"] = "n/a"; + break; + } + + switch (info_packet.time_sync_state.value()) { + case SYNC_STATUS_INVALID_FLAG: + sensor_info["time_sync_state"] = "time_sync_invalid"; + break; + case SYNC_STATUS_GPS_SUCCESS_FLAG: + sensor_info["time_sync_state"] = "gps_time_sync_successful"; + break; + case SYNC_STATUS_PTP_SUCCESS_FLAG: + sensor_info["time_sync_state"] = "ptp_time_sync_successful"; + break; + default: + sensor_info["time_sync_state"] = "n/a"; + } + + sensor_info["time"] = std::to_string(info_packet.time.get_time_in_ns()); + sensor_info["machine_current"] = + robosense_packet::get_float_value(info_packet.operating_status.machine_current.value()); + sensor_info["machine_voltage"] = + robosense_packet::get_float_value(info_packet.operating_status.machine_voltage.value()); + sensor_info["rotation_direction"] = std::to_string(info_packet.rotation_direction.value()); + sensor_info["running_time"] = std::to_string(info_packet.running_time.value()); + sensor_info["startup_times"] = + std::to_string(info_packet.fault_diagnosis.startup_times.value()); + + const std::bitset<8> gps_st_bits{info_packet.fault_diagnosis.gps_status.value()}; + if (gps_st_bits[0]) + sensor_info["pps_lock"] = "valid"; + else + sensor_info["pps_lock"] = "invalid"; + if (gps_st_bits[1]) + sensor_info["gprmc_lock"] = "valid"; + else + sensor_info["gprmc_lock"] = "invalid"; + if (gps_st_bits[2]) + sensor_info["utc_lock"] = "synchronized"; + else + sensor_info["utc_lock"] = "not_synchronized"; + if (gps_st_bits[3]) + sensor_info["gprmc_input_status"] = "received_gprmc"; + else + sensor_info["gprmc_input_status"] = "no_gprmc"; + if (gps_st_bits[4]) + sensor_info["pps_input_status"] = "received_pps"; + else + sensor_info["pps_input_status"] = "no_pps"; + + sensor_info["machine_temp"] = + robosense_packet::get_float_value(info_packet.fault_diagnosis.machine_temp.value()); + sensor_info["phase"] = std::to_string(info_packet.fault_diagnosis.phase.value()); + sensor_info["rotation_speed"] = + std::to_string(info_packet.fault_diagnosis.rotation_speed.value()); + + std::string gprmc_string; + for (auto i : info_packet.gprmc) { + gprmc_string += static_cast(i.value()); + } + sensor_info["gprmc_string"] = gprmc_string; + + return sensor_info; + } +}; +} // namespace drivers +} // namespace nebula \ No newline at end of file diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/helios.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/helios.hpp new file mode 100644 index 000000000..8ade0f893 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/helios.hpp @@ -0,0 +1,457 @@ +#pragma once + +#include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp" +#include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_sensor.hpp" + +#include "boost/endian/buffers.hpp" + +#include +#include +#include + +using namespace boost::endian; + +namespace nebula +{ +namespace drivers +{ +namespace robosense_packet +{ +namespace helios +{ +#pragma pack(push, 1) + +struct Header +{ + big_uint32_buf_t header_id; + big_uint16_buf_t protocol_version; + big_uint16_buf_t reserved_first; + big_uint32_buf_t top_packet_count; + big_uint32_buf_t bottom_packet_count; + big_uint8_buf_t reserved_second; + big_uint8_buf_t range_resolution; + big_uint16_buf_t angle_interval_count; + Timestamp timestamp; + big_uint8_buf_t reserved_third; + big_uint8_buf_t lidar_type; + big_uint8_buf_t lidar_model; + big_uint8_buf_t reserved_fourth[9]; +}; + +struct Packet : public PacketBase<12, 32, 2, 100> +{ + typedef Body, Packet::N_BLOCKS> body_t; + Header header; + body_t body; + big_uint48_buf_t tail; +}; + +struct OperatingStatus +{ + big_uint16_buf_t i_dat; + big_uint16_buf_t v_dat; + big_uint16_buf_t v_dat_12v; + big_uint16_buf_t v_dat_5v; + big_uint16_buf_t v_dat_2v5; + big_uint16_buf_t v_dat_apd; +}; + +struct FaultDiagnosis +{ + big_uint16_buf_t temperature1; + big_uint16_buf_t temperature2; + big_uint16_buf_t temperature3; + big_uint16_buf_t temperature4; + big_uint16_buf_t temperature5; + big_uint16_buf_t r_rpm; + big_uint8_buf_t lane_up; + big_uint16_buf_t lane_up_cnt; + big_uint16_buf_t top_status; + big_uint8_buf_t gps_status; +}; + +struct SensorHwVersion +{ + big_uint8_buf_t first_octet; + big_uint8_buf_t second_octet; + big_uint8_buf_t third_octet; + + [[nodiscard]] std::string to_string() const + { + std::stringstream ss; + ss << std::hex << std::setfill('0') << std::setw(2) << static_cast(first_octet.value()) + << std::setw(2) << static_cast(second_octet.value()) << std::setw(2) + << static_cast(third_octet.value()); + return ss.str(); + } +}; + +struct WebPageVersion +{ + big_uint8_buf_t first_octet; + big_uint8_buf_t second_octet; + big_uint8_buf_t third_octet; + big_uint8_buf_t fourth_octet; + + [[nodiscard]] std::string to_string() const + { + std::stringstream ss; + ss << std::hex << std::setfill('0') << std::setw(2) << static_cast(first_octet.value()) + << std::setw(2) << static_cast(second_octet.value()) << std::setw(2) + << static_cast(third_octet.value()) << std::setw(2) + << static_cast(fourth_octet.value()); + return ss.str(); + } +}; + +struct InfoPacket +{ + big_uint64_buf_t header; + big_uint16_buf_t motor_speed; + Ethernet ethernet; + FovSetting fov_setting; + big_uint16_buf_t reserved_first; + big_uint16_buf_t phase_lock; + FirmwareVersion top_firmware_version; + FirmwareVersion bottom_firmware_version; + FirmwareVersion bottom_software_version; + FirmwareVersion motor_firmware_version; + SensorHwVersion sensor_hw_version; + WebPageVersion web_page_version; + big_uint32_buf_t top_backup_crc; + big_uint32_buf_t bottom_backup_crc; + big_uint32_buf_t software_backup_crc; + big_uint32_buf_t webpage_backup_crc; + IpAddress ethernet_gateway; + IpAddress subnet_mask; + uint8_t reserved_second[201]; + SerialNumber serial_number; + big_uint16_buf_t zero_angle_offset; + big_uint8_buf_t return_mode; + big_uint8_buf_t time_sync_mode; + big_uint8_buf_t sync_status; + Timestamp time; + OperatingStatus operating_status; + uint8_t reserved_third[17]; + FaultDiagnosis fault_diagnosis; + big_uint8_buf_t code_wheel_status; + big_uint8_buf_t pps_trigger_mode; + uint8_t reserved_fourth[20]; + big_uint8_buf_t gprmc[86]; + SensorCalibration sensor_calibration; + uint8_t reserved_fifth[586]; + big_uint16_buf_t tail; +}; + +#pragma pack(pop) +} // namespace helios +} // namespace robosense_packet + +class Helios +: public RobosenseSensor +{ +private: + static constexpr int firing_time_offset_ns_single_[12][32] = { + {0, 1570, 3150, 4720, 6300, 7870, 9450, 11360, 13260, 15170, 17080, + 18990, 20560, 22140, 23710, 25290, 26530, 29010, 27770, 30250, 31490, 32730, + 33980, 35220, 36460, 37700, 38940, 40180, 41420, 42670, 43910, 45150}, + {55560, 57130, 58700, 60280, 61850, 63430, 65000, 66910, 68820, 70730, 72640, + 74540, 76120, 77690, 79270, 80840, 82080, 84570, 83320, 85810, 87050, 89530, + 88290, 90770, 92010, 93260, 94500, 95740, 96980, 98220, 99460, 100700}, + {111110, 112690, 114260, 115840, 117410, 118980, 120560, 122470, 124380, 126280, 128190, + 130100, 131670, 133250, 134820, 136400, 137640, 140120, 138880, 141360, 142600, 145090, + 143850, 146330, 147570, 148810, 150050, 151290, 152540, 153780, 155020, 156260}, + {166670, 168240, 169820, 171390, 172970, 174540, 176110, 178020, 179930, 181840, 183750, + 185650, 187230, 188800, 190380, 191950, 193190, 195680, 194440, 196920, 198160, 200640, + 199400, 201880, 203130, 204370, 205610, 206850, 208090, 209330, 210570, 211810}, + {222220, 223800, 225370, 226950, 228520, 230100, 231670, 233580, 235490, 237390, 239300, + 241210, 242780, 244360, 245930, 247510, 248750, 251230, 249990, 252470, 253720, 256200, + 254960, 257440, 258680, 259920, 261160, 262400, 263650, 264890, 266130, 267370}, + {277780, 279350, 280930, 282500, 284080, 285650, 287230, 289130, 291040, 292950, 294860, + 296770, 298340, 299920, 301490, 303060, 304310, 306790, 305550, 308030, 309270, 311750, + 310510, 313000, 314240, 315480, 316720, 317960, 319200, 320440, 321680, 322930}, + {333330, 334910, 336480, 338060, 339630, 341210, 342780, 344690, 346600, 348510, 350410, + 352320, 353900, 355470, 357050, 358620, 359860, 362340, 361100, 363590, 364830, 367310, + 366070, 368550, 369790, 371030, 372270, 373520, 374760, 376000, 377240, 378480}, + {388890, 390460, 392040, 393610, 395190, 396760, 398340, 400240, 402150, 404060, 405970, + 407880, 409450, 411030, 412600, 414180, 415420, 417900, 416660, 419140, 420380, 422860, + 421620, 424110, 425350, 426590, 427830, 429070, 430310, 431550, 432800, 434040}, + {444440, 446020, 447590, 449170, 450740, 452320, 453890, 455800, 457710, 459620, 461520, + 463430, 465010, 466580, 468160, 469730, 470970, 473460, 472210, 474700, 475940, 478420, + 477180, 479660, 480900, 482140, 483390, 484630, 485870, 487110, 488350, 489590}, + {500000, 501570, 503150, 504720, 506300, 507870, 509450, 511360, 513260, 515170, 517080, + 518990, 520560, 522140, 523710, 525290, 526530, 529010, 527770, 530250, 531490, 533980, + 532730, 535220, 536460, 537700, 538940, 540180, 541420, 542670, 543910, 545150}, + {555560, 557130, 558700, 560280, 561850, 563430, 565000, 566910, 568820, 570730, 572640, + 574540, 576120, 577690, 579270, 580840, 582080, 584570, 583320, 585810, 587050, 589530, + 588290, 590770, 592010, 593260, 594500, 595740, 596980, 598220, 599460, 600700}, + {611110, 612690, 614260, 615840, 617410, 618980, 620560, 622470, 624380, 626280, 628190, + 630100, 631670, 633250, 634820, 636400, 637640, 640120, 638880, 641360, 642600, 645090, + 643850, 646330, 647570, 648810, 650050, 651290, 652540, 653780, 655020, 656260}}; + + static constexpr int firing_time_offset_ns_dual_[12][32]{ + {0, 1570, 3150, 4720, 6300, 7870, 9450, 11360, 13260, 15170, 17080, + 18990, 20560, 22140, 23710, 25290, 26530, 27770, 29010, 30250, 31490, 32730, + 33980, 35220, 36460, 37700, 38940, 40180, 41420, 42670, 43910, 45150}, + {0, 1570, 3150, 4720, 6300, 7870, 9450, 11360, 13260, 15170, 17080, + 18990, 20560, 22140, 23710, 25290, 26530, 27770, 29010, 30250, 31490, 32730, + 33980, 35220, 36460, 37700, 38940, 40180, 41420, 42670, 43910, 45150}, + {55560, 57130, 58700, 60280, 61850, 63430, 65000, 66910, 68820, 70730, 72640, + 74540, 76120, 77690, 79270, 80840, 82080, 83320, 84570, 85810, 87050, 88290, + 89530, 90770, 92010, 93260, 94500, 95740, 96980, 98220, 99460, 100700}, + {55560, 57130, 58700, 60280, 61850, 63430, 65000, 66910, 68820, 70730, 72640, + 74540, 76120, 77690, 79270, 80840, 82080, 83320, 84570, 85810, 87050, 88290, + 89530, 90770, 92010, 93260, 94500, 95740, 96980, 98220, 99460, 100700}, + {111110, 112690, 114260, 115840, 117410, 118980, 120560, 122470, 124380, 126280, 128190, + 130100, 131670, 133250, 134820, 136400, 137640, 138880, 140120, 141360, 142600, 143850, + 145090, 146330, 147570, 148810, 150050, 151290, 152540, 153780, 155020, 156260}, + {111110, 112690, 114260, 115840, 117410, 118980, 120560, 122470, 124380, 126280, 128190, + 130100, 131670, 133250, 134820, 136400, 137640, 138880, 140120, 141360, 142600, 143850, + 145090, 146330, 147570, 148810, 150050, 151290, 152540, 153780, 155020, 156260}, + {166670, 168240, 169820, 171390, 172970, 174540, 176110, 178020, 179930, 181840, 183750, + 185650, 187230, 188800, 190380, 191950, 193190, 194440, 195680, 196920, 198160, 199400, + 200640, 201880, 203130, 204370, 205610, 206850, 208090, 209330, 210570, 211810}, + {166670, 168240, 169820, 171390, 172970, 174540, 176110, 178020, 179930, 181840, 183750, + 185650, 187230, 188800, 190380, 191950, 193190, 194440, 195680, 196920, 198160, 199400, + 200640, 201880, 203130, 204370, 205610, 206850, 208090, 209330, 210570, 211810}, + {222220, 223800, 225370, 226950, 228520, 230100, 231670, 233580, 235490, 237390, 239300, + 241210, 242780, 244360, 245930, 247510, 248750, 249990, 251230, 252470, 253720, 254960, + 256200, 257440, 258680, 259920, 261160, 262400, 263650, 264890, 266130, 267370}, + {222220, 223800, 225370, 226950, 228520, 230100, 231670, 233580, 235490, 237390, 239300, + 241210, 242780, 244360, 245930, 247510, 248750, 249990, 251230, 252470, 253720, 254960, + 256200, 257440, 258680, 259920, 261160, 262400, 263650, 264890, 266130, 267370}, + {277780, 279350, 280930, 282500, 284080, 285650, 287230, 289130, 291040, 292950, 294860, + 296770, 298340, 299920, 301490, 303060, 304310, 305550, 306790, 308030, 309270, 310510, + 311750, 313000, 314240, 315480, 316720, 317960, 319200, 320440, 321680, 322930}, + {277780, 279350, 280930, 282500, 284080, 285650, 287230, 289130, 291040, 292950, 294860, + 296770, 298340, 299920, 301490, 303060, 304310, 305550, 306790, 308030, 309270, 310510, + 311750, 313000, 314240, 315480, 316720, 317960, 319200, 320440, 321680, 322930}}; + + static constexpr uint8_t DUAL_RETURN_FLAG = 0x00; + static constexpr uint8_t STRONGEST_RETURN_FLAG = 0x04; + static constexpr uint8_t LAST_RETURN_FLAG = 0x05; + static constexpr uint8_t FIRST_RETURN_FLAG = 0x06; + + static constexpr uint8_t SYNC_MODE_GPS_FLAG = 0x00; + static constexpr uint8_t SYNC_MODE_E2E_FLAG = 0x01; + static constexpr uint8_t SYNC_MODE_P2P_FLAG = 0x02; + static constexpr uint8_t SYNC_MODE_GPTP_FLAG = 0x03; + + static constexpr uint8_t SYNC_STATUS_INVALID_FLAG = 0x00; + static constexpr uint8_t SYNC_STATUS_GPS_SUCCESS_FLAG = 0x01; + static constexpr uint8_t SYNC_STATUS_PTP_SUCCESS_FLAG = 0x02; + +public: + static constexpr float MIN_RANGE = 0.2f; + static constexpr float MAX_RANGE = 150.f; + static constexpr size_t MAX_SCAN_BUFFER_POINTS = 1152000; + + int getPacketRelativePointTimeOffset( + const uint32_t block_id, const uint32_t channel_id, + const std::shared_ptr & sensor_configuration) override + { + if (sensor_configuration->return_mode == ReturnMode::DUAL) + return firing_time_offset_ns_dual_[block_id][channel_id]; + else + return firing_time_offset_ns_single_[block_id][channel_id]; + } + + ReturnMode getReturnMode(const robosense_packet::helios::InfoPacket & info_packet) + { + switch (info_packet.return_mode.value()) { + case DUAL_RETURN_FLAG: + return ReturnMode::DUAL; + case STRONGEST_RETURN_FLAG: + return ReturnMode::SINGLE_STRONGEST; + case LAST_RETURN_FLAG: + return ReturnMode::SINGLE_LAST; + case FIRST_RETURN_FLAG: + return ReturnMode::SINGLE_FIRST; + default: + return ReturnMode::UNKNOWN; + } + } + + RobosenseCalibrationConfiguration getSensorCalibration( + const robosense_packet::helios::InfoPacket & info_packet) + { + return info_packet.sensor_calibration.getCalibration(); + } + + bool getSyncStatus(const robosense_packet::helios::InfoPacket & info_packet) + { + switch (info_packet.time_sync_mode.value()) { + case SYNC_MODE_GPS_FLAG: + return true; + case SYNC_MODE_E2E_FLAG: + return true; + case SYNC_MODE_P2P_FLAG: + return true; + case SYNC_MODE_GPTP_FLAG: + return true; + default: + return false; + } + } + + std::map getSensorInfo( + const robosense_packet::helios::InfoPacket & info_packet) + { + std::map sensor_info; + sensor_info["motor_speed"] = std::to_string(info_packet.motor_speed.value()); + sensor_info["lidar_ip"] = info_packet.ethernet.lidar_ip.to_string(); + sensor_info["dest_pc_ip"] = info_packet.ethernet.dest_pc_ip.to_string(); + sensor_info["mac_addr"] = info_packet.ethernet.mac_addr.to_string(); + sensor_info["lidar_out_msop_port"] = + std::to_string(info_packet.ethernet.lidar_out_msop_port.value()); + sensor_info["pc_dest_msop_port"] = + std::to_string(info_packet.ethernet.pc_dest_msop_port.value()); + sensor_info["lidar_out_difop_port"] = + std::to_string(info_packet.ethernet.lidar_out_difop_port.value()); + sensor_info["pc_dest_difop_port"] = + std::to_string(info_packet.ethernet.pc_dest_difop_port.value()); + sensor_info["fov_start"] = + robosense_packet::get_float_value(info_packet.fov_setting.fov_start.value()); + sensor_info["fov_end"] = + robosense_packet::get_float_value(info_packet.fov_setting.fov_end.value()); + sensor_info["phase_lock"] = std::to_string(info_packet.phase_lock.value()); + sensor_info["top_firmware_version"] = info_packet.top_firmware_version.to_string(); + sensor_info["bottom_firmware_version"] = info_packet.bottom_firmware_version.to_string(); + sensor_info["bottom_software_version"] = info_packet.bottom_software_version.to_string(); + sensor_info["motor_firmware_version"] = info_packet.motor_firmware_version.to_string(); + sensor_info["sensor_hw_version"] = info_packet.sensor_hw_version.to_string(); + sensor_info["web_page_version"] = info_packet.web_page_version.to_string(); + sensor_info["top_backup_crc"] = std::to_string(info_packet.top_backup_crc.value()); + sensor_info["bottom_backup_crc"] = std::to_string(info_packet.bottom_backup_crc.value()); + sensor_info["software_backup_crc"] = std::to_string(info_packet.software_backup_crc.value()); + sensor_info["webpage_backup_crc"] = std::to_string(info_packet.webpage_backup_crc.value()); + sensor_info["ethernet_gateway"] = info_packet.ethernet_gateway.to_string(); + sensor_info["subnet_mask"] = info_packet.subnet_mask.to_string(); + sensor_info["serial_number"] = info_packet.serial_number.to_string(); + sensor_info["zero_angle_offset"] = std::to_string(info_packet.zero_angle_offset.value()); + + switch (info_packet.return_mode.value()) { + case DUAL_RETURN_FLAG: + sensor_info["return_mode"] = "dual"; + break; + case STRONGEST_RETURN_FLAG: + sensor_info["return_mode"] = "strongest"; + break; + case LAST_RETURN_FLAG: + sensor_info["return_mode"] = "last"; + break; + case FIRST_RETURN_FLAG: + sensor_info["return_mode"] = "first"; + break; + default: + sensor_info["return_mode"] = "n/a"; + break; + } + + switch (info_packet.time_sync_mode.value()) { + case SYNC_MODE_GPS_FLAG: + sensor_info["time_sync_mode"] = "gps"; + break; + case SYNC_MODE_E2E_FLAG: + sensor_info["time_sync_mode"] = "e2e"; + break; + case SYNC_MODE_P2P_FLAG: + sensor_info["time_sync_mode"] = "p2p"; + break; + case SYNC_MODE_GPTP_FLAG: + sensor_info["time_sync_mode"] = "gptp"; + break; + default: + sensor_info["time_sync_mode"] = "n/a"; + break; + } + + switch (info_packet.sync_status.value()) { + case SYNC_STATUS_INVALID_FLAG: + sensor_info["sync_status"] = "time_sync_invalid"; + break; + case SYNC_STATUS_GPS_SUCCESS_FLAG: + sensor_info["sync_status"] = "gps_time_sync_successful"; + break; + case SYNC_STATUS_PTP_SUCCESS_FLAG: + sensor_info["sync_status"] = "ptp_time_sync_successful"; + break; + default: + sensor_info["sync_status"] = "n/a"; + } + + /* + * From the manual, here are the formulas for calculating following values: + * + * Idat = Value_temp / 4096 * 5 [A] + * Vdat = value / 4096 [V] + * Vdat_12V_reg = value / 4096 * 24.5 [V] + * Vdat_5V_reg = value / 4096 x 11 [V] + * Vdat_2V5_reg = value / 4096 x 10 [V] + * Vdat_APD = 516.65 * (value) / 4096 - 465.8 [V](negative value) + * Temperature 2&3&4 = 200 *（value）/ 4096 - 50 + * Temperature 1&5 = 503.975 * (value) / 4096 - 273.15 + */ + sensor_info["time"] = std::to_string(info_packet.time.get_time_in_ns()); + sensor_info["i_dat"] = std::to_string(info_packet.operating_status.i_dat.value() / 4096.0 * 5); + sensor_info["v_dat"] = std::to_string(info_packet.operating_status.v_dat.value() / 4096.0); + sensor_info["v_dat_12v"] = + std::to_string(info_packet.operating_status.v_dat_12v.value() / 4096.0 * 24.5); + sensor_info["v_dat_5v"] = + std::to_string(info_packet.operating_status.v_dat_5v.value() / 4096.0 * 11); + sensor_info["v_dat_2v5"] = + std::to_string(info_packet.operating_status.v_dat_2v5.value() / 4096.0 * 10); + sensor_info["v_dat_apd"] = + std::to_string(info_packet.operating_status.v_dat_apd.value() * 516.65 / 4096 - 465.8); + sensor_info["top_board_temp"] = + std::to_string(info_packet.fault_diagnosis.temperature1.value() * 503.975 / 4096.0 - 273.15); + sensor_info["temperature2"] = + std::to_string(info_packet.fault_diagnosis.temperature2.value() * 200 / 4096.0 - 50); + sensor_info["temperature3"] = + std::to_string(info_packet.fault_diagnosis.temperature3.value() * 200 / 4096.0 - 50); + sensor_info["temperature4"] = + std::to_string(info_packet.fault_diagnosis.temperature4.value() * 200 / 4096.0 - 50); + sensor_info["bottom_board_temp"] = + std::to_string(info_packet.fault_diagnosis.temperature5.value() * 503.975 / 4096.0 - 273.15); + sensor_info["real_time_rot_speed"] = std::to_string(info_packet.fault_diagnosis.r_rpm.value()); + sensor_info["lane_up"] = std::to_string(info_packet.fault_diagnosis.lane_up.value()); + sensor_info["lane_up_cnt"] = std::to_string(info_packet.fault_diagnosis.lane_up_cnt.value()); + sensor_info["top_status"] = std::to_string(info_packet.fault_diagnosis.top_status.value()); + + const std::bitset<8> gps_st_bits{info_packet.fault_diagnosis.gps_status.value()}; + if (gps_st_bits[0]) + sensor_info["pps_lock"] = "valid"; + else + sensor_info["pps_lock"] = "invalid"; + if (gps_st_bits[1]) + sensor_info["gprmc_lock"] = "valid"; + else + sensor_info["gprmc_lock"] = "invalid"; + if (gps_st_bits[2]) + sensor_info["utc_lock"] = "synchronized"; + else + sensor_info["utc_lock"] = "not_synchronized"; + if (gps_st_bits[3]) + sensor_info["gprmc_input_status"] = "received_gprmc"; + else + sensor_info["gprmc_input_status"] = "no_gprmc"; + if (gps_st_bits[4]) + sensor_info["pps_input_status"] = "received_pps"; + else + sensor_info["pps_input_status"] = "no_pps"; + + sensor_info["code_wheel_status"] = std::to_string(info_packet.code_wheel_status.value()); + sensor_info["pps_trigger_mode"] = std::to_string(info_packet.pps_trigger_mode.value()); + + std::string gprmc_string; + for (auto i : info_packet.gprmc) { + gprmc_string += static_cast(i.value()); + } + sensor_info["gprmc_string"] = gprmc_string; + + return sensor_info; + } +}; + +} // namespace drivers +} // namespace nebula \ No newline at end of file diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_decoder.hpp new file mode 100644 index 000000000..79630bc23 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_decoder.hpp @@ -0,0 +1,268 @@ +#pragma once + +#include "nebula_common/robosense/robosense_common.hpp" +#include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp" +#include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_scan_decoder.hpp" + +#include + +#include "robosense_msgs/msg/robosense_packet.hpp" +#include "robosense_msgs/msg/robosense_scan.hpp" + +namespace nebula +{ +namespace drivers +{ +template +class RobosenseDecoder : public RobosenseScanDecoder +{ +protected: + /// @brief Configuration for this decoder + const std::shared_ptr sensor_configuration_; + + /// @brief The sensor definition, used for return mode and time offset handling + SensorT sensor_{}; + + /// @brief Decodes azimuth/elevation angles given calibration/correction data + typename SensorT::angle_corrector_t angle_corrector_; + + /// @brief The point cloud new points get added to + NebulaPointCloudPtr decode_pc_; + /// @brief The point cloud that is returned when a scan is complete + NebulaPointCloudPtr output_pc_; + + /// @brief The last decoded packet + typename SensorT::packet_t packet_; + /// @brief The last azimuth processed + int last_phase_; + /// @brief The timestamp of the last completed scan in nanoseconds + uint64_t output_scan_timestamp_ns_; + /// @brief The timestamp of the scan currently in progress + uint64_t decode_scan_timestamp_ns_; + /// @brief Whether a full scan has been processed + bool has_scanned_; + + rclcpp::Logger logger_; + + /// @brief Validates and parses MsopPacket. Currently only checks size, not checksums etc. + /// @param msop_packet The incoming MsopPacket + /// @return Whether the packet was parsed successfully + bool parsePacket(const robosense_msgs::msg::RobosensePacket & msop_packet) + { + if (msop_packet.data.size() < sizeof(typename SensorT::packet_t)) { + RCLCPP_ERROR_STREAM( + logger_, "Packet size mismatch:" << msop_packet.data.size() << " | Expected at least:" + << sizeof(typename SensorT::packet_t)); + return false; + } + if (std::memcpy(&packet_, msop_packet.data.data(), sizeof(typename SensorT::packet_t))) { + return true; + } + + RCLCPP_ERROR(logger_, "Packet memcopy failed"); + return false; + } + + /// @brief Converts a group of returns (i.e. 1 for single return, 2 for dual return, etc.) to + /// points and appends them to the point cloud + /// @param start_block_id The first block in the group of returns + /// @param n_blocks The number of returns in the group + void convertReturns(size_t start_block_id, size_t n_blocks) + { + uint64_t packet_timestamp_ns = robosense_packet::get_timestamp_ns(packet_); + uint32_t raw_azimuth = packet_.body.blocks[start_block_id].get_azimuth(); + + std::vector return_units; + + for (size_t channel_id = 0; channel_id < SensorT::packet_t::N_CHANNELS; ++channel_id) { + // Find the units corresponding to the same return group as the current one. + // These are used to find duplicates in multi-return mode. + return_units.clear(); + for (size_t block_offset = 0; block_offset < n_blocks; ++block_offset) { + return_units.push_back( + &packet_.body.blocks[block_offset + start_block_id].units[channel_id]); + } + + for (size_t block_offset = 0; block_offset < n_blocks; ++block_offset) { + auto & unit = *return_units[block_offset]; + + if (unit.distance.value() == 0) { + continue; + } + + auto distance = getDistance(unit); + + if (distance < SensorT::MIN_RANGE || distance > SensorT::MAX_RANGE) { + continue; + } + + auto return_type = + sensor_.getReturnType(sensor_configuration_->return_mode, block_offset, return_units); + + // Keep only last of multiple identical points + if (return_type == ReturnType::IDENTICAL && block_offset != n_blocks - 1) { + continue; + } + + // Keep only last (if any) of multiple points that are too close + if (block_offset != n_blocks - 1) { + bool is_below_multi_return_threshold = false; + + for (size_t return_idx = 0; return_idx < n_blocks; ++return_idx) { + if (return_idx == block_offset) { + continue; + } + + if ( + fabsf(getDistance(*return_units[return_idx]) - distance) < + sensor_configuration_->dual_return_distance_threshold) { + is_below_multi_return_threshold = true; + break; + } + } + + if (is_below_multi_return_threshold) { + continue; + } + } + + NebulaPoint point; + point.distance = distance; + point.intensity = unit.reflectivity.value(); + point.time_stamp = + getPointTimeRelative(packet_timestamp_ns, block_offset + start_block_id, channel_id); + + point.return_type = static_cast(return_type); + + auto corrected_angle_data = angle_corrector_.getCorrectedAngleData(raw_azimuth, channel_id); + point.channel = corrected_angle_data.corrected_channel_id; + + // The raw_azimuth and channel are only used as indices, sin/cos functions use the precise + // corrected angles + float xyDistance = distance * corrected_angle_data.cos_elevation; + point.x = xyDistance * corrected_angle_data.cos_azimuth; + point.y = -xyDistance * corrected_angle_data.sin_azimuth; + point.z = distance * corrected_angle_data.sin_elevation; + + // The driver wrapper converts to degrees, expects radians + point.azimuth = corrected_angle_data.azimuth_rad; + point.elevation = corrected_angle_data.elevation_rad; + + decode_pc_->emplace_back(point); + } + } + } + + /// @brief Checks whether the last processed block was the last block of a scan + /// @param current_phase The azimuth of the last processed block + /// @return Whether the scan has completed + bool checkScanCompleted(int current_phase) + { + return angle_corrector_.hasScanned(current_phase, last_phase_); + } + + /// @brief Get the distance of the given unit in meters + /// @param unit The unit to get the distance from + /// @return The distance in meters + float getDistance(const typename SensorT::packet_t::body_t::block_t::unit_t & unit) + { + return unit.distance.value() * robosense_packet::get_dis_unit(packet_); + } + + /// @brief Get timestamp of point in nanoseconds, relative to scan timestamp. Includes firing time + /// offset correction for channel and block + /// @param packet_timestamp_ns The timestamp of the current MsopPacket in nanoseconds + /// @param block_id The block index of the point + /// @param channel_id The channel index of the point + uint32_t getPointTimeRelative(uint64_t packet_timestamp_ns, size_t block_id, size_t channel_id) + { + auto point_to_packet_offset_ns = + sensor_.getPacketRelativePointTimeOffset(block_id, channel_id, sensor_configuration_); + auto packet_to_scan_offset_ns = + static_cast(packet_timestamp_ns - decode_scan_timestamp_ns_); + return packet_to_scan_offset_ns + point_to_packet_offset_ns; + } + +public: + /// @brief Constructor + /// @param sensor_configuration SensorConfiguration for this decoder + /// @param calibration_configuration Calibration for this decoder + /// calibration_configuration is set) + explicit RobosenseDecoder( + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & calibration_configuration) + : sensor_configuration_(sensor_configuration), + angle_corrector_(calibration_configuration), + logger_(rclcpp::get_logger("RobosenseDecoder")) + { + logger_.set_level(rclcpp::Logger::Level::Debug); + RCLCPP_INFO_STREAM(logger_, sensor_configuration_); + + decode_pc_.reset(new NebulaPointCloud); + output_pc_.reset(new NebulaPointCloud); + + decode_pc_->reserve(SensorT::MAX_SCAN_BUFFER_POINTS); + output_pc_->reserve(SensorT::MAX_SCAN_BUFFER_POINTS); + } + + int unpack(const robosense_msgs::msg::RobosensePacket & msop_packet) override + { + if (!parsePacket(msop_packet)) { + return -1; + } + + if (decode_scan_timestamp_ns_ == 0) { + decode_scan_timestamp_ns_ = robosense_packet::get_timestamp_ns(packet_); + } + + if (has_scanned_) { + has_scanned_ = false; + } + + // For the dual return mode, the packet contains two blocks with the same azimuth, one for each + // return. For the single return mode, the packet contains only one block per azimuth. + // So, if the return mode is dual, we process two blocks per iteration, otherwise one. + const size_t n_returns = robosense_packet::get_n_returns(sensor_configuration_->return_mode); + int current_azimuth; + + for (size_t block_id = 0; block_id < SensorT::packet_t::N_BLOCKS; block_id += n_returns) { + current_azimuth = + (360 * SensorT::packet_t::DEGREE_SUBDIVISIONS + + packet_.body.blocks[block_id].get_azimuth() - + static_cast( + sensor_configuration_->scan_phase * SensorT::packet_t::DEGREE_SUBDIVISIONS)) % + (360 * SensorT::packet_t::DEGREE_SUBDIVISIONS); + + bool scan_completed = checkScanCompleted(current_azimuth); + if (scan_completed) { + std::swap(decode_pc_, output_pc_); + decode_pc_->clear(); + has_scanned_ = true; + output_scan_timestamp_ns_ = decode_scan_timestamp_ns_; + + // A new scan starts within the current packet, so the new scan's timestamp must be + // calculated as the packet timestamp plus the lowest time offset of any point in the + // remainder of the packet + decode_scan_timestamp_ns_ = + robosense_packet::get_timestamp_ns(packet_) + + sensor_.getEarliestPointTimeOffsetForBlock(block_id, sensor_configuration_); + } + + convertReturns(block_id, n_returns); + last_phase_ = current_azimuth; + } + + return last_phase_; + } + + bool hasScanned() override { return has_scanned_; } + + std::tuple getPointcloud() override + { + double scan_timestamp_s = static_cast(output_scan_timestamp_ns_) * 1e-9; + return std::make_pair(output_pc_, scan_timestamp_s); + } +}; + +} // namespace drivers +} // namespace nebula \ No newline at end of file diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder.hpp new file mode 100644 index 000000000..86b9bc4aa --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder.hpp @@ -0,0 +1,84 @@ +#pragma once + +#include "nebula_common/robosense/robosense_common.hpp" +#include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder_base.hpp" +#include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp" + +#include + +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ + +template +class RobosenseInfoDecoder : public RobosenseInfoDecoderBase +{ +protected: + /// @brief The sensor definition, used for return mode and time offset handling + SensorT sensor_{}; + + /// @brief The last decoded packet + typename SensorT::info_t packet_{}; + + rclcpp::Logger logger_; + +public: + /// @brief Validates and parses DIFOP packet. Currently only checks size, not checksums etc. + /// @param raw_packet The incoming DIFOP packet + /// @return Whether the packet was parsed successfully + bool parsePacket(const std::vector & raw_packet) override + { + const auto packet_size = raw_packet.size(); + if (packet_size < sizeof(typename SensorT::info_t)) { + RCLCPP_ERROR_STREAM( + logger_, "Packet size mismatch:" << packet_size << " | Expected at least:" + << sizeof(typename SensorT::info_t)); + return false; + } + try { + if (std::memcpy(&packet_, raw_packet.data(), sizeof(typename SensorT::info_t)) == &packet_) { + return true; + } + } catch (const std::exception & e) { + RCLCPP_ERROR_STREAM(logger_, "Packet memcopy failed: " << e.what()); + } + + return false; + } + + /// @brief Constructor + RobosenseInfoDecoder() : logger_(rclcpp::get_logger("RobosenseInfoDecoder")) + { + logger_.set_level(rclcpp::Logger::Level::Debug); + } + + /// @brief Get the sensor telemetry + /// @return The sensor telemetry + std::map getSensorInfo() override + { + return sensor_.getSensorInfo(packet_); + } + + /// @brief Get the laser return mode + /// @return The laser return mode + ReturnMode getReturnMode() override { return sensor_.getReturnMode(packet_); } + + /// @brief Get sensor calibration + /// @return The sensor calibration + RobosenseCalibrationConfiguration getSensorCalibration() override + { + return sensor_.getSensorCalibration(packet_); + } + + /// @brief Get the status of time synchronization + /// @return True if the sensor's clock is synchronized + bool getSyncStatus() override { return sensor_.getSyncStatus(packet_); } +}; + +} // namespace drivers +} // namespace nebula \ No newline at end of file diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder_base.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder_base.hpp new file mode 100644 index 000000000..d97dc4e1c --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder_base.hpp @@ -0,0 +1,42 @@ +#pragma once + +namespace nebula +{ +namespace drivers +{ + +class RobosenseInfoDecoderBase +{ +public: + RobosenseInfoDecoderBase(RobosenseInfoDecoderBase && c) = delete; + RobosenseInfoDecoderBase & operator=(RobosenseInfoDecoderBase && c) = delete; + RobosenseInfoDecoderBase(const RobosenseInfoDecoderBase & c) = delete; + RobosenseInfoDecoderBase & operator=(const RobosenseInfoDecoderBase & c) = delete; + + virtual ~RobosenseInfoDecoderBase() = default; + RobosenseInfoDecoderBase() = default; + + /// @brief Parses DIFOP and add its telemetry + /// @param raw_packet The incoming DIFOP packet + /// @return Whether the packet was parsed successfully + virtual bool parsePacket(const std::vector & raw_packet) = 0; + + /// @brief Get the sensor telemetry + /// @return The sensor telemetry + virtual std::map getSensorInfo() = 0; + + /// @brief Get the laser return mode + /// @return The laser return mode + virtual ReturnMode getReturnMode() = 0; + + /// @brief Get sensor calibration + /// @return The sensor calibration + virtual RobosenseCalibrationConfiguration getSensorCalibration() = 0; + + /// @brief Get the status of time synchronization + /// @return True if the sensor's clock is synchronized + virtual bool getSyncStatus() = 0; +}; + +} // namespace drivers +} // namespace nebula \ No newline at end of file diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp new file mode 100644 index 000000000..607c44aac --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp @@ -0,0 +1,267 @@ +#pragma once + +#include "boost/endian/buffers.hpp" + +#include +#include +#include + +using namespace boost::endian; + +namespace nebula +{ +namespace drivers +{ +namespace robosense_packet +{ + +#pragma pack(push, 1) + +struct Timestamp +{ + big_uint48_buf_t seconds; + big_uint32_buf_t microseconds; + + uint64_t get_time_in_ns() const + { + constexpr uint64_t NS_IN_SECOND = 1000000000ULL; + constexpr uint64_t NS_IN_MICROSECOND = 1000ULL; + + uint64_t total_nanoseconds = seconds.value() * NS_IN_SECOND; + total_nanoseconds += microseconds.value() * NS_IN_MICROSECOND; + + return total_nanoseconds; + } +}; + + +struct Unit +{ + big_uint16_buf_t distance; + big_uint8_buf_t reflectivity; +}; + +template +struct Block +{ + big_uint16_buf_t flag; + big_uint16_buf_t azimuth; + UnitT units[UnitN]; + typedef UnitT unit_t; + + [[nodiscard]] uint16_t get_azimuth() const { return azimuth.value(); } +}; + +template +struct Body +{ + typedef BlockT block_t; + BlockT blocks[BlockN]; +}; + +/// @brief Base struct for all Robosense packets. This struct is not allowed to have any non-static +/// members, otherwise memory layout is not guaranteed for the derived structs. +/// @tparam nBlocks The number of blocks in the packet +/// @tparam nChannels The number of channels per block +/// @tparam maxReturns The maximum number of returns, e.g. 2 for dual return +/// @tparam degreeSubdivisions The resolution of the azimuth angle in the packet, e.g. 100 if packet +/// azimuth is given in 1/100th of a degree +template +struct PacketBase +{ + static constexpr size_t N_BLOCKS = nBlocks; + static constexpr size_t N_CHANNELS = nChannels; + static constexpr size_t MAX_RETURNS = maxReturns; + static constexpr size_t DEGREE_SUBDIVISIONS = degreeSubdivisions; +}; + +struct IpAddress +{ + big_uint8_buf_t first_octet; + big_uint8_buf_t second_octet; + big_uint8_buf_t third_octet; + big_uint8_buf_t fourth_octet; + + [[nodiscard]] std::string to_string() const + { + return std::to_string(first_octet.value()) + "." + std::to_string(second_octet.value()) + "." + + std::to_string(third_octet.value()) + "." + std::to_string(fourth_octet.value()); + } +}; + +struct MacAddress +{ + big_uint8_buf_t first_octet; + big_uint8_buf_t second_octet; + big_uint8_buf_t third_octet; + big_uint8_buf_t fourth_octet; + big_uint8_buf_t fifth_octet; + big_uint8_buf_t sixth_octet; + + [[nodiscard]] std::string to_string() const + { + std::stringstream ss; + ss << std::hex << std::setfill('0') << std::setw(2) << static_cast(first_octet.value()) + << ":" << std::setw(2) << static_cast(second_octet.value()) << ":" << std::setw(2) + << static_cast(third_octet.value()) << ":" << std::setw(2) + << static_cast(fourth_octet.value()) << ":" << std::setw(2) + << static_cast(fifth_octet.value()) << ":" << std::setw(2) + << static_cast(sixth_octet.value()); + return ss.str(); + } +}; + +struct Ethernet +{ + IpAddress lidar_ip; + IpAddress dest_pc_ip; + MacAddress mac_addr; + big_uint16_buf_t lidar_out_msop_port; + big_uint16_buf_t pc_dest_msop_port; + big_uint16_buf_t lidar_out_difop_port; + big_uint16_buf_t pc_dest_difop_port; +}; + +struct FovSetting +{ + big_uint16_buf_t fov_start; + big_uint16_buf_t fov_end; +}; + +constexpr uint8_t ANGLE_SIGN_FLAG = 0x00; + +struct ChannelAngleCorrection +{ + big_uint8_buf_t sign; + big_uint16_buf_t angle; + + [[nodiscard]] float getAngle() const + { + return sign.value() == ANGLE_SIGN_FLAG + ? static_cast(angle.value()) / 100.0f + : static_cast(angle.value()) / -100.0f; + } +}; + +struct CorrectedVerticalAngle +{ + ChannelAngleCorrection angles[32]; +}; + +struct CorrectedHorizontalAngle +{ + ChannelAngleCorrection angles[32]; +}; + +struct SensorCalibration +{ + CorrectedVerticalAngle corrected_vertical_angle; + CorrectedHorizontalAngle corrected_horizontal_angle; + + RobosenseCalibrationConfiguration getCalibration() const + { + RobosenseCalibrationConfiguration calibration; + for (size_t i = 0; i < 32; ++i) { + ChannelCorrection channel_correction; + channel_correction.azimuth = corrected_horizontal_angle.angles[i].getAngle(); + channel_correction.elevation = corrected_vertical_angle.angles[i].getAngle(); + calibration.calibration.push_back(channel_correction); + } + return calibration; + } +}; + +struct FirmwareVersion +{ + big_uint8_buf_t first_octet; + big_uint8_buf_t second_octet; + big_uint8_buf_t third_octet; + big_uint8_buf_t fourth_octet; + big_uint8_buf_t fifth_octet; + + [[nodiscard]] std::string to_string() const + { + std::stringstream ss; + ss << std::hex << std::setfill('0') << std::setw(2) << static_cast(first_octet.value()) + << std::setw(2) << static_cast(second_octet.value()) << std::setw(2) + << static_cast(third_octet.value()) << std::setw(2) + << static_cast(fourth_octet.value()) << std::setw(2) + << static_cast(fifth_octet.value()); + return ss.str(); + } +}; + +struct SerialNumber +{ + big_uint8_buf_t first_octet; + big_uint8_buf_t second_octet; + big_uint8_buf_t third_octet; + big_uint8_buf_t fourth_octet; + big_uint8_buf_t fifth_octet; + big_uint8_buf_t sixth_octet; + + [[nodiscard]] std::string to_string() const + { + std::stringstream ss; + ss << std::hex << std::setfill('0') << std::setw(2) << static_cast(first_octet.value()) + << std::setw(2) << static_cast(second_octet.value()) << std::setw(2) + << static_cast(third_octet.value()) << std::setw(2) + << static_cast(fourth_octet.value()) << std::setw(2) + << static_cast(fifth_octet.value()) << std::setw(2) + << static_cast(sixth_octet.value()); + return ss.str(); + } +}; + +#pragma pack(pop) + +/// @brief Get the number of returns for a given return mode +/// @param return_mode The return mode +/// @return The number of returns +size_t get_n_returns(ReturnMode return_mode) +{ + if (return_mode == ReturnMode::DUAL) { + return 2; + } + return 1; +} + +/// @brief Get timestamp from packet in nanoseconds +/// @tparam PacketT The packet type +/// @param packet The packet to get the timestamp from +/// @return The timestamp in nanoseconds +template +uint64_t get_timestamp_ns(const PacketT & packet) +{ + return packet.header.timestamp.get_time_in_ns(); +} + +/// @brief Get the distance unit of the given packet type in meters. Distance values in the packet, +/// multiplied by this value, yield the distance in meters. +/// @tparam PacketT The packet type +/// @param packet The packet to get the distance unit from +/// @return The distance unit in meters +template +double get_dis_unit(const PacketT & packet) +{ + // Packets define distance unit in millimeters, convert to meters here + const uint8_t range_resolution = packet.header.range_resolution.value(); + if (range_resolution == 0) { + return 0.0050; + } else if (range_resolution == 1) { + return 0.0025; + } + throw std::runtime_error("Unknown range resolution"); +} + +/// @brief Convert raw angle value from packet to std::string +/// @param raw_angle The raw angle value from the packet +/// @return The angle as std::string +std::string get_float_value(const uint16_t & raw_angle) +{ + return std::to_string(static_cast(raw_angle) / 100.0f); +} + +} // namespace robosense_packet +} // namespace drivers +} // namespace nebula \ No newline at end of file diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_scan_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_scan_decoder.hpp new file mode 100644 index 000000000..c5beaa342 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_scan_decoder.hpp @@ -0,0 +1,42 @@ +#pragma once + +#include "nebula_common/point_types.hpp" +#include "nebula_common/robosense/robosense_common.hpp" + +#include "robosense_msgs/msg/robosense_packet.hpp" +#include "robosense_msgs/msg/robosense_scan.hpp" + +#include + +namespace nebula +{ +namespace drivers +{ +/// @brief Base class for Robosense LiDAR decoder +class RobosenseScanDecoder +{ +public: + RobosenseScanDecoder(RobosenseScanDecoder && c) = delete; + RobosenseScanDecoder & operator=(RobosenseScanDecoder && c) = delete; + RobosenseScanDecoder(const RobosenseScanDecoder & c) = delete; + RobosenseScanDecoder & operator=(const RobosenseScanDecoder & c) = delete; + + virtual ~RobosenseScanDecoder() = default; + RobosenseScanDecoder() = default; + + /// @brief Parses RobosensePacket and add its points to the point cloud + /// @param msop_packet The incoming MsopPacket + /// @return The last azimuth processed + virtual int unpack(const robosense_msgs::msg::RobosensePacket & msop_packet) = 0; + + /// @brief Indicates whether one full scan is ready + /// @return Whether a scan is ready + virtual bool hasScanned() = 0; + + /// @brief Returns the point cloud and timestamp of the last scan + /// @return A tuple of point cloud and timestamp in nanoseconds + virtual std::tuple getPointcloud() = 0; +}; + +} // namespace drivers +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_sensor.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_sensor.hpp new file mode 100644 index 000000000..4e8b3c25a --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_sensor.hpp @@ -0,0 +1,124 @@ +#pragma once + +#include "nebula_common/nebula_common.hpp" +#include "nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector_calibration_based.hpp" +#include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp" + +#include +#include + +namespace nebula +{ +namespace drivers +{ + +/// @brief Base class for all sensor definitions +/// @tparam PacketT The packet type of the sensor +template +class RobosenseSensor +{ +private: +public: + typedef PacketT packet_t; + typedef InfoPacketT info_t; + typedef class AngleCorrectorCalibrationBased + angle_corrector_t; + + RobosenseSensor() = default; + virtual ~RobosenseSensor() = default; + + /// @brief Computes the exact relative time between the timestamp of the given packet and the one + /// of the point identified by the given block and channel, in nanoseconds + /// @param block_id The point's block id + /// @param channel_id The point's channel id + /// @param sensor_configuration The sensor configuration + /// @return The relative time offset in nanoseconds + virtual int getPacketRelativePointTimeOffset( + uint32_t block_id, uint32_t channel_id, + const std::shared_ptr & sensor_configuration) = 0; + + /// @brief For a given start block index, find the earliest (lowest) relative time offset of any + /// point in the packet in or after the start block + /// @param start_block_id The index of the block in and after which to consider points + /// @param sensor_configuration The sensor configuration + /// @return The lowest point time offset (relative to the packet timestamp) of any point in or + /// after the start block, in nanoseconds + int getEarliestPointTimeOffsetForBlock( + uint32_t start_block_id, + const std::shared_ptr & sensor_configuration) + { + const auto n_returns = robosense_packet::get_n_returns(sensor_configuration->return_mode); + int min_offset_ns = std::numeric_limits::max(); + + for (uint32_t block_id = start_block_id; block_id < start_block_id + n_returns; ++block_id) { + for (uint32_t channel_id = 0; channel_id < PacketT::N_CHANNELS; ++channel_id) { + min_offset_ns = std::min( + min_offset_ns, + getPacketRelativePointTimeOffset(block_id, channel_id, sensor_configuration)); + } + } + + return min_offset_ns; + } + + /// @brief Whether the unit given by return_idx is a duplicate of any other unit in return_units + /// @param return_idx The unit's index in the return_units vector + /// @param return_units The vector of all the units corresponding to the same return group (i.e. + /// length 2 for dual-return with both units having the same channel but coming from different + /// blocks) + /// @return true if the unit is identical to any other one in return_units, false otherwise + static bool is_duplicate( + uint32_t return_idx, + const std::vector & return_units) + { + for (unsigned int i = 0; i < return_units.size(); ++i) { + if (i == return_idx) { + continue; + } + + if ( + return_units[return_idx]->distance.value() == return_units[i]->distance.value() && + return_units[return_idx]->reflectivity.value() == return_units[i]->reflectivity.value()) { + return true; + } + } + + return false; + } + + /// @brief Get the return type of the point given by return_idx + /// + /// @param return_mode The sensor's currently active return mode + /// @param return_idx The block index of the point within the group of blocks that make up the + /// return group (e.g. either 0 or 1 for dual return) + /// @param return_units The units corresponding to all the returns in the group. These are usually + /// from the same column across adjascent blocks. + /// @return The return type of the point + virtual ReturnType getReturnType( + ReturnMode return_mode, unsigned int return_idx, + const std::vector & return_units) + { + if (is_duplicate(return_idx, return_units)) { + return ReturnType::IDENTICAL; + } + + switch (return_mode) { + case ReturnMode::SINGLE_FIRST: + return ReturnType::FIRST; + case ReturnMode::SINGLE_LAST: + return ReturnType::LAST; + case ReturnMode::SINGLE_STRONGEST: + return ReturnType::STRONGEST; + case ReturnMode::DUAL: + if (return_idx == 0) { + return ReturnType::STRONGEST; + } else { + return ReturnType::LAST; + } + default: + return ReturnType::UNKNOWN; + } + } +}; +} // namespace drivers +} // namespace nebula \ No newline at end of file diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_driver.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_driver.hpp new file mode 100644 index 000000000..fe9968162 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_driver.hpp @@ -0,0 +1,61 @@ +#pragma once + +#include "nebula_common/nebula_common.hpp" +#include "nebula_common/nebula_status.hpp" +#include "nebula_common/point_types.hpp" +#include "nebula_common/robosense/robosense_common.hpp" +#include "nebula_decoders/nebula_decoders_common/nebula_driver_base.hpp" +#include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_decoder.hpp" + +#include "robosense_msgs/msg/robosense_packet.hpp" +#include "robosense_msgs/msg/robosense_scan.hpp" + +#include + +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +/// @brief Robosense driver +class RobosenseDriver : NebulaDriverBase +{ +private: + /// @brief Current driver status + Status driver_status_; + + /// @brief Decoder according to the model + std::shared_ptr scan_decoder_; + +public: + RobosenseDriver() = delete; + + /// @brief Constructor + /// @param sensor_configuration SensorConfiguration for this driver + /// @param calibration_configuration CalibrationConfiguration for this driver + explicit RobosenseDriver( + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & calibration_configuration); + + /// @brief Get current status of this driver + /// @return Current status + Status GetStatus(); + + /// @brief Setting CalibrationConfiguration (not used) + /// @param calibration_configuration + /// @return Resulting status + Status SetCalibrationConfiguration( + const CalibrationConfigurationBase & calibration_configuration) override; + + /// @brief Convert RobosenseScan message to point cloud + /// @param robosense_scan Message + /// @return tuple of Point cloud and timestamp + std::tuple ConvertScanToPointcloud( + const std::shared_ptr & robosense_scan); +}; + +} // namespace drivers +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_info_driver.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_info_driver.hpp new file mode 100644 index 000000000..bf44a3ce5 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_info_driver.hpp @@ -0,0 +1,64 @@ +#pragma once + +#include "nebula_common/nebula_common.hpp" +#include "nebula_common/nebula_status.hpp" +#include "nebula_common/point_types.hpp" +#include "nebula_common/robosense/robosense_common.hpp" +#include "nebula_decoders/nebula_decoders_common/nebula_driver_base.hpp" +#include "nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v3.hpp" +#include "nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v4.hpp" +#include "nebula_decoders/nebula_decoders_robosense/decoders/helios.hpp" +#include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_decoder.hpp" +#include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder.hpp" +#include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder_base.hpp" + +#include "pandar_msgs/msg/pandar_packet.hpp" +#include "pandar_msgs/msg/pandar_scan.hpp" + +#include + +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +/// @brief Robosense driver +class RobosenseInfoDriver +{ +private: + /// @brief Current driver status + Status driver_status_; + + /// @brief Decoder according to the model + std::shared_ptr info_decoder_; + +public: + RobosenseInfoDriver() = delete; + + /// @brief Constructor + /// @param sensor_configuration SensorConfiguration for this driver + explicit RobosenseInfoDriver( + const std::shared_ptr & sensor_configuration); + + /// @brief Get current status of this driver + /// @return Current status + Status GetStatus(); + + Status DecodeInfoPacket(const std::vector & packet); + + std::map GetSensorInfo(); + + ReturnMode GetReturnMode(); + + RobosenseCalibrationConfiguration GetSensorCalibration(); + + /// @brief Get the status of time synchronization + /// @return True if the sensor's clock is synchronized + bool GetSyncStatus(); +}; + +} // namespace drivers +} // namespace nebula diff --git a/nebula_decoders/package.xml b/nebula_decoders/package.xml index f25fc698a..6956bc3d1 100644 --- a/nebula_decoders/package.xml +++ b/nebula_decoders/package.xml @@ -20,6 +20,7 @@ sensor_msgs velodyne_msgs yaml-cpp + robosense_msgs ament_cmake_gtest ament_lint_auto diff --git a/nebula_decoders/src/nebula_decoders_robosense/robosense_driver.cpp b/nebula_decoders/src/nebula_decoders_robosense/robosense_driver.cpp new file mode 100644 index 000000000..c6cb23378 --- /dev/null +++ b/nebula_decoders/src/nebula_decoders_robosense/robosense_driver.cpp @@ -0,0 +1,83 @@ +#include "nebula_decoders/nebula_decoders_robosense/robosense_driver.hpp" + +#include "nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v3.hpp" +#include "nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v4.hpp" +#include "nebula_decoders/nebula_decoders_robosense/decoders/helios.hpp" + +namespace nebula +{ +namespace drivers +{ + +RobosenseDriver::RobosenseDriver( + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & calibration_configuration) +{ + // initialize proper parser from cloud config's model and echo mode + driver_status_ = nebula::Status::OK; + switch (sensor_configuration->sensor_model) { + case SensorModel::UNKNOWN: + driver_status_ = nebula::Status::INVALID_SENSOR_MODEL; + break; + case SensorModel::ROBOSENSE_BPEARL_V3: + scan_decoder_.reset( + new RobosenseDecoder(sensor_configuration, calibration_configuration)); + break; + case SensorModel::ROBOSENSE_BPEARL_V4: + scan_decoder_.reset( + new RobosenseDecoder(sensor_configuration, calibration_configuration)); + break; + case SensorModel::ROBOSENSE_HELIOS: + scan_decoder_.reset( + new RobosenseDecoder(sensor_configuration, calibration_configuration)); + break; + default: + driver_status_ = nebula::Status::NOT_INITIALIZED; + throw std::runtime_error("Driver not Implemented for selected sensor."); + } +} + +Status RobosenseDriver::GetStatus() +{ + return driver_status_; +} + +Status RobosenseDriver::SetCalibrationConfiguration( + const CalibrationConfigurationBase & calibration_configuration) +{ + throw std::runtime_error( + "SetCalibrationConfiguration. Not yet implemented (" + + calibration_configuration.calibration_file + ")"); +} + +std::tuple RobosenseDriver::ConvertScanToPointcloud( + const std::shared_ptr & robosense_scan) +{ + std::tuple pointcloud; + auto logger = rclcpp::get_logger("RobosenseDriver"); + + if (driver_status_ != nebula::Status::OK) { + RCLCPP_ERROR(logger, "Driver not OK."); + return pointcloud; + } + + int cnt = 0, last_azimuth = 0; + for (auto & packet : robosense_scan->packets) { + last_azimuth = scan_decoder_->unpack(packet); + if (scan_decoder_->hasScanned()) { + pointcloud = scan_decoder_->getPointcloud(); + cnt++; + } + } + + if (cnt == 0) { + RCLCPP_ERROR_STREAM( + logger, "Scanned " << robosense_scan->packets.size() << " packets, but no " + << "pointclouds were generated. Last azimuth: " << last_azimuth); + } + + return pointcloud; +} + +} // namespace drivers +} // namespace nebula diff --git a/nebula_decoders/src/nebula_decoders_robosense/robosense_info_driver.cpp b/nebula_decoders/src/nebula_decoders_robosense/robosense_info_driver.cpp new file mode 100644 index 000000000..4d415afd2 --- /dev/null +++ b/nebula_decoders/src/nebula_decoders_robosense/robosense_info_driver.cpp @@ -0,0 +1,67 @@ +#include "nebula_decoders/nebula_decoders_robosense/robosense_info_driver.hpp" + +namespace nebula +{ +namespace drivers +{ + +RobosenseInfoDriver::RobosenseInfoDriver( + const std::shared_ptr & sensor_configuration) +{ + // initialize proper parser from cloud config's model and echo mode + driver_status_ = nebula::Status::OK; + switch (sensor_configuration->sensor_model) { + case SensorModel::UNKNOWN: + driver_status_ = nebula::Status::INVALID_SENSOR_MODEL; + break; + case SensorModel::ROBOSENSE_BPEARL_V3: + info_decoder_.reset(new RobosenseInfoDecoder()); + break; + case SensorModel::ROBOSENSE_BPEARL_V4: + info_decoder_.reset(new RobosenseInfoDecoder()); + break; + case SensorModel::ROBOSENSE_HELIOS: + info_decoder_.reset(new RobosenseInfoDecoder()); + break; + + default: + driver_status_ = nebula::Status::NOT_INITIALIZED; + throw std::runtime_error("Driver not Implemented for selected sensor."); + break; + } +} + +Status RobosenseInfoDriver::GetStatus() +{ + return driver_status_; +} + +Status RobosenseInfoDriver::DecodeInfoPacket(const std::vector & packet) +{ + const auto parsed = info_decoder_->parsePacket(packet); + if (parsed) return nebula::Status::OK; + return nebula::Status::ERROR_1; +} + +std::map RobosenseInfoDriver::GetSensorInfo() +{ + return info_decoder_->getSensorInfo(); +} + +ReturnMode RobosenseInfoDriver::GetReturnMode() +{ + return info_decoder_->getReturnMode(); +} + +RobosenseCalibrationConfiguration RobosenseInfoDriver::GetSensorCalibration() +{ + return info_decoder_->getSensorCalibration(); +} + +bool RobosenseInfoDriver::GetSyncStatus() +{ + return info_decoder_->getSyncStatus(); +} + +} // namespace drivers +} // namespace nebula diff --git a/nebula_hw_interfaces/CMakeLists.txt b/nebula_hw_interfaces/CMakeLists.txt index 23d8a52bf..b0e15b4d5 100644 --- a/nebula_hw_interfaces/CMakeLists.txt +++ b/nebula_hw_interfaces/CMakeLists.txt @@ -5,6 +5,7 @@ find_package(ament_cmake_auto REQUIRED) find_package(PCL REQUIRED) find_package(PCL REQUIRED COMPONENTS common) find_package(pcl_conversions REQUIRED) +find_package(robosense_msgs REQUIRED) ament_auto_find_build_dependencies() @@ -33,6 +34,10 @@ ament_auto_add_library(nebula_hw_interfaces_velodyne SHARED src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp ) +ament_auto_add_library(nebula_hw_interfaces_robosense SHARED + src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp + ) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp new file mode 100644 index 000000000..cd45984af --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp @@ -0,0 +1,126 @@ +#pragma once + +// 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 "boost_udp_driver/udp_driver.hpp" +#include "nebula_common/robosense/robosense_common.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp" + +#include + +#include "robosense_msgs/msg/robosense_info_packet.hpp" +#include "robosense_msgs/msg/robosense_packet.hpp" +#include "robosense_msgs/msg/robosense_scan.hpp" + +namespace nebula +{ +namespace drivers +{ +constexpr uint16_t MTU_SIZE = 1248; +constexpr uint16_t HELIOS_PACKET_SIZE = 1248; +constexpr uint16_t HELIOS_INFO_PACKET_SIZE = 1248; +constexpr uint16_t BPEARL_PACKET_SIZE = 1248; +constexpr uint16_t BPEARL_INFO_PACKET_SIZE = 1248; + +/// @brief Hardware interface of Robosense driver +class RobosenseHwInterface : NebulaHwInterfaceBase +{ +private: + std::unique_ptr<::drivers::common::IoContext> cloud_io_context_; + std::unique_ptr<::drivers::common::IoContext> info_io_context_; + std::unique_ptr<::drivers::udp_driver::UdpDriver> cloud_udp_driver_; + std::unique_ptr<::drivers::udp_driver::UdpDriver> info_udp_driver_; + std::shared_ptr sensor_configuration_; + std::unique_ptr scan_cloud_ptr_; + size_t azimuth_index_{44}; // For Helios and Bpearl 42 byte header + 2 byte flag + int prev_phase_{}; + std::atomic is_info_received{false}; // To check if DIFOP is received + std::optional> info_buffer_; // To hold DIFOP data + std::optional sensor_model_; // To hold sensor model + std::function + is_valid_packet_; /*Lambda Function Array to verify proper packet size for data*/ + std::function + is_valid_info_packet_; /*Lambda Function Array to verify proper packet size for info*/ + std::function buffer)> + scan_reception_callback_; /**This function pointer is called when the scan is complete*/ + std::function buffer)> + info_reception_callback_; /**This function pointer is called when DIFOP packet is received*/ + 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_DEBUG_STREAM + /// @param debug Target string + void PrintDebug(std::string debug); + +public: + /// @brief Constructor + RobosenseHwInterface(); + + /// @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 ReceiveCloudPacketCallback(const std::vector & buffer) final; + + /// @brief Callback function to receive the Info Packet data from the UDP Driver + /// @param buffer Buffer containing the data received from the UDP socket + void ReceiveInfoPacketCallback(const std::vector & buffer); + + /// @brief Starting the interface that handles UDP streams for MSOP packets + /// @return Resulting status + Status CloudInterfaceStart() final; + + /// @brief Starting the interface that handles UDP streams for DIFOP packets + /// @return Resulting status + Status InfoInterfaceStart(); + + /// @brief Function for stopping the interface that handles UDP streams + /// @return Resulting status + Status CloudInterfaceStop() final; + + /// @brief Setting sensor configuration + /// @param sensor_configuration SensorConfiguration for this interface + /// @return Resulting status + Status SetSensorConfiguration( + std::shared_ptr sensor_configuration) final; + + /// @brief Printing sensor configuration + /// @param sensor_configuration SensorConfiguration for this interface + /// @return Resulting status + Status GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) final; + + /// @brief Printing calibration configuration + /// @param calibration_configuration CalibrationConfiguration for the checking + /// @return Resulting status + Status GetCalibrationConfiguration( + CalibrationConfigurationBase & calibration_configuration) override; + + /// @brief Registering callback for RobosenseScan + /// @param scan_callback Callback function + /// @return Resulting status + Status RegisterScanCallback( + std::function)> scan_callback); + + /// @brief Registering callback for RobosensePacket + /// @param scan_callback Callback function + /// @return Resulting status + Status RegisterInfoCallback( + std::function)> info_callback); + + /// @brief Setting rclcpp::Logger + /// @param node Logger + void SetLogger(std::shared_ptr logger); +}; + +} // namespace drivers +} // namespace nebula diff --git a/nebula_hw_interfaces/package.xml b/nebula_hw_interfaces/package.xml index aa169d320..25bdf7f8e 100644 --- a/nebula_hw_interfaces/package.xml +++ b/nebula_hw_interfaces/package.xml @@ -19,6 +19,7 @@ boost_tcp_driver boost_udp_driver velodyne_msgs + robosense_msgs ament_cmake_gtest ament_lint_auto diff --git a/nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp new file mode 100644 index 000000000..8332933e0 --- /dev/null +++ b/nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp @@ -0,0 +1,241 @@ +#include "nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp" +namespace nebula +{ +namespace drivers +{ +RobosenseHwInterface::RobosenseHwInterface() +: cloud_io_context_{new ::drivers::common::IoContext(1)}, + info_io_context_{new ::drivers::common::IoContext(1)}, + cloud_udp_driver_{new ::drivers::udp_driver::UdpDriver(*cloud_io_context_)}, + info_udp_driver_{new ::drivers::udp_driver::UdpDriver(*info_io_context_)}, + scan_cloud_ptr_{std::make_unique()} +{ +} + +void RobosenseHwInterface::ReceiveCloudPacketCallback(const std::vector & buffer) +{ + int scan_phase = static_cast(sensor_configuration_->scan_phase * 100.0); + if (!is_valid_packet_(buffer.size())) { + PrintDebug("Invalid Packet: " + std::to_string(buffer.size())); + return; + } + // Copy data + uint32_t buffer_size = buffer.size(); + std::array packet_data{}; + std::copy_n(std::make_move_iterator(buffer.begin()), buffer_size, packet_data.begin()); + robosense_msgs::msg::RobosensePacket msop_packet; + msop_packet.data = packet_data; + + // Add timestamp (Sensor timestamp will be handled by decoder) + const auto now = std::chrono::system_clock::now(); + const auto timestamp_ns = + std::chrono::duration_cast(now.time_since_epoch()).count(); + + constexpr int nanosec_per_sec = 1000000000; + msop_packet.stamp.sec = static_cast(timestamp_ns / nanosec_per_sec); + msop_packet.stamp.nanosec = static_cast(timestamp_ns % nanosec_per_sec); + + if ( + !sensor_model_.has_value() && + sensor_configuration_->sensor_model == SensorModel::ROBOSENSE_BPEARL) { + if (buffer[32] == BPEARL_V4_FLAG) { + sensor_model_.emplace(drivers::SensorModel::ROBOSENSE_BPEARL_V4); + PrintInfo("Bpearl V4 detected."); + } else { + sensor_model_.emplace(drivers::SensorModel::ROBOSENSE_BPEARL_V3); + PrintInfo("Bpearl V3 detected."); + } + } + + scan_cloud_ptr_->packets.emplace_back(msop_packet); + + int current_phase{}; + bool comp_flg = false; + + const auto & data = scan_cloud_ptr_->packets.back().data; + current_phase = (data[azimuth_index_ + 1] & 0xff) + ((data[azimuth_index_] & 0xff) << 8); + + current_phase = (static_cast(current_phase) + 36000 - scan_phase) % 36000; + + if (current_phase >= prev_phase_ || scan_cloud_ptr_->packets.size() < 2) { + prev_phase_ = current_phase; + } else { + comp_flg = true; + } + + if (comp_flg) { // Scan complete + if (scan_reception_callback_) { + scan_cloud_ptr_->header.stamp = scan_cloud_ptr_->packets.front().stamp; + // Callback + scan_reception_callback_(std::move(scan_cloud_ptr_)); + scan_cloud_ptr_ = std::make_unique(); + } + } +} + +void RobosenseHwInterface::ReceiveInfoPacketCallback(const std::vector & buffer) +{ + if (!is_valid_info_packet_(buffer.size())) { + PrintDebug("Invalid Packet: " + std::to_string(buffer.size())); + return; + } + + info_buffer_.emplace(buffer); ////// + is_info_received = true; //////// + + if (info_reception_callback_) { + std::unique_ptr difop_packet = + std::make_unique(); + std::copy_n( + std::make_move_iterator(buffer.begin()), buffer.size(), difop_packet->packet.data.begin()); + + if (sensor_model_.has_value()) { + difop_packet->lidar_model = SensorModelToString(sensor_model_.value()); + } else { + difop_packet->lidar_model = SensorModelToString(sensor_configuration_->sensor_model); + } + + info_reception_callback_(std::move(difop_packet)); + } +} + +Status RobosenseHwInterface::CloudInterfaceStart() +{ + try { + std::cout << "Starting UDP server for data packets on: " << *sensor_configuration_ << std::endl; + cloud_udp_driver_->init_receiver( + sensor_configuration_->host_ip, sensor_configuration_->data_port); + cloud_udp_driver_->receiver()->open(); + cloud_udp_driver_->receiver()->bind(); + + cloud_udp_driver_->receiver()->asyncReceive( + std::bind(&RobosenseHwInterface::ReceiveCloudPacketCallback, this, std::placeholders::_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; + } + return Status::OK; +} + +Status RobosenseHwInterface::InfoInterfaceStart() +{ + try { + std::cout << "Starting UDP server for info packets on: " << *sensor_configuration_ << std::endl; + PrintInfo( + "Starting UDP server for info packets on: " + sensor_configuration_->sensor_ip + ":" + + std::to_string(sensor_configuration_->gnss_port)); + info_udp_driver_->init_receiver( + sensor_configuration_->host_ip, sensor_configuration_->gnss_port); + info_udp_driver_->receiver()->open(); + info_udp_driver_->receiver()->bind(); + + info_udp_driver_->receiver()->asyncReceive( + std::bind(&RobosenseHwInterface::ReceiveInfoPacketCallback, this, std::placeholders::_1)); + + } catch (const std::exception & ex) { + Status status = Status::UDP_CONNECTION_ERROR; + std::cerr << status << sensor_configuration_->sensor_ip << "," + << sensor_configuration_->gnss_port << std::endl; + return status; + } + + std::this_thread::sleep_for(std::chrono::seconds(1)); + return Status::OK; +} + +Status RobosenseHwInterface::CloudInterfaceStop() +{ + return Status::ERROR_1; +} + +Status RobosenseHwInterface::SetSensorConfiguration( + std::shared_ptr sensor_configuration) +{ + Status status = Status::OK; + + try { + sensor_configuration_ = + std::static_pointer_cast(sensor_configuration); + + if ( + sensor_configuration_->sensor_model == SensorModel::ROBOSENSE_BPEARL || + sensor_configuration_->sensor_model == SensorModel::ROBOSENSE_BPEARL_V3 || + sensor_configuration_->sensor_model == SensorModel::ROBOSENSE_BPEARL_V4) { + azimuth_index_ = 44; + is_valid_packet_ = [](size_t packet_size) { return (packet_size == BPEARL_PACKET_SIZE); }; + is_valid_info_packet_ = [](size_t packet_size) { + return (packet_size == BPEARL_INFO_PACKET_SIZE); + }; + } else if (sensor_configuration->sensor_model == SensorModel::ROBOSENSE_HELIOS) { + azimuth_index_ = 44; + is_valid_packet_ = [](size_t packet_size) { return (packet_size == HELIOS_PACKET_SIZE); }; + is_valid_info_packet_ = [](size_t packet_size) { + return (packet_size == HELIOS_INFO_PACKET_SIZE); + }; + } else { + status = Status::INVALID_SENSOR_MODEL; + } + } catch (const std::exception & ex) { + status = Status::SENSOR_CONFIG_ERROR; + std::cerr << status << std::endl; + return status; + } + return status; +} + +Status RobosenseHwInterface::GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) +{ + std::stringstream ss; + ss << sensor_configuration; + PrintDebug(ss.str()); + return Status::ERROR_1; +} + +Status RobosenseHwInterface::GetCalibrationConfiguration( + CalibrationConfigurationBase & calibration_configuration) +{ + PrintDebug(calibration_configuration.calibration_file); + return Status::ERROR_1; +} + +Status RobosenseHwInterface::RegisterScanCallback( + std::function)> scan_callback) +{ + scan_reception_callback_ = std::move(scan_callback); + return Status::OK; +} + +Status RobosenseHwInterface::RegisterInfoCallback( + std::function)> info_callback) +{ + info_reception_callback_ = std::move(info_callback); + return Status::OK; +} + +void RobosenseHwInterface::PrintDebug(std::string debug) +{ + if (parent_node_logger_) { + RCLCPP_DEBUG_STREAM((*parent_node_logger_), debug); + } else { + std::cout << debug << std::endl; + } +} + +void RobosenseHwInterface::PrintInfo(std::string info) +{ + if (parent_node_logger_) { + RCLCPP_INFO_STREAM((*parent_node_logger_), info); + } else { + std::cout << info << std::endl; + } +} + +void RobosenseHwInterface::SetLogger(std::shared_ptr logger) +{ + parent_node_logger_ = logger; +} + +} // namespace drivers +} // namespace nebula \ No newline at end of file diff --git a/nebula_messages/robosense_msgs/CMakeLists.txt b/nebula_messages/robosense_msgs/CMakeLists.txt new file mode 100644 index 000000000..9043a8bc0 --- /dev/null +++ b/nebula_messages/robosense_msgs/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.8) +project(robosense_msgs) + +if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif () + +# find dependencies +find_package(ament_cmake_auto REQUIRED) +find_package(std_msgs REQUIRED) +ament_auto_find_build_dependencies() + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/RobosensePacket.msg" + "msg/RobosenseInfoPacket.msg" + "msg/RobosenseScan.msg" + DEPENDENCIES + std_msgs + ) + +ament_package() diff --git a/nebula_messages/robosense_msgs/msg/RobosenseInfoPacket.msg b/nebula_messages/robosense_msgs/msg/RobosenseInfoPacket.msg new file mode 100644 index 000000000..acf5751bd --- /dev/null +++ b/nebula_messages/robosense_msgs/msg/RobosenseInfoPacket.msg @@ -0,0 +1,2 @@ +string lidar_model +RobosensePacket packet \ No newline at end of file diff --git a/nebula_messages/robosense_msgs/msg/RobosensePacket.msg b/nebula_messages/robosense_msgs/msg/RobosensePacket.msg new file mode 100644 index 000000000..713682931 --- /dev/null +++ b/nebula_messages/robosense_msgs/msg/RobosensePacket.msg @@ -0,0 +1,2 @@ +builtin_interfaces/Time stamp +uint8[1248] data \ No newline at end of file diff --git a/nebula_messages/robosense_msgs/msg/RobosenseScan.msg b/nebula_messages/robosense_msgs/msg/RobosenseScan.msg new file mode 100644 index 000000000..0c2b5518b --- /dev/null +++ b/nebula_messages/robosense_msgs/msg/RobosenseScan.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +RobosensePacket[] packets \ No newline at end of file diff --git a/nebula_messages/robosense_msgs/package.xml b/nebula_messages/robosense_msgs/package.xml new file mode 100644 index 000000000..29077edb2 --- /dev/null +++ b/nebula_messages/robosense_msgs/package.xml @@ -0,0 +1,22 @@ + + + + robosense_msgs + 0.0.0 + Robosense message types for Nebula + Mehmet Emin BASOGLU + TODO: License declaration + + ament_cmake_auto + rosidl_default_generators + + builtin_interfaces + std_msgs + + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/nebula_ros/CMakeLists.txt b/nebula_ros/CMakeLists.txt index ea5b0d93d..954c81d04 100644 --- a/nebula_ros/CMakeLists.txt +++ b/nebula_ros/CMakeLists.txt @@ -22,6 +22,7 @@ find_package(nebula_common REQUIRED) find_package(nebula_decoders REQUIRED) find_package(nebula_hw_interfaces REQUIRED) find_package(yaml-cpp REQUIRED) +find_package(robosense_msgs REQUIRED) include_directories( include @@ -91,6 +92,37 @@ rclcpp_components_register_node(velodyne_driver_ros_wrapper EXECUTABLE velodyne_driver_ros_wrapper_node ) +## Robosense +# Hw Interface +ament_auto_add_library(robosense_hw_ros_wrapper SHARED + src/robosense/robosense_hw_interface_ros_wrapper.cpp + ) + +rclcpp_components_register_node(robosense_hw_ros_wrapper + PLUGIN "RobosenseHwInterfaceRosWrapper" + EXECUTABLE robosense_hw_interface_ros_wrapper_node + ) + +# DriverDecoder +ament_auto_add_library(robosense_driver_ros_wrapper SHARED + src/robosense/robosense_decoder_ros_wrapper.cpp + ) + +rclcpp_components_register_node(robosense_driver_ros_wrapper + PLUGIN "RobosenseDriverRosWrapper" + EXECUTABLE robosense_driver_ros_wrapper_node + ) + +# Monitor +ament_auto_add_library(robosense_hw_monitor_ros_wrapper SHARED + src/robosense/robosense_hw_monitor_ros_wrapper.cpp + ) + +rclcpp_components_register_node(robosense_hw_monitor_ros_wrapper + PLUGIN "RobosenseHwMonitorRosWrapper" + EXECUTABLE robosense_hw_monitor_ros_wrapper_node + ) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() diff --git a/nebula_ros/config/robosense/Bpearl.yaml b/nebula_ros/config/robosense/Bpearl.yaml new file mode 100644 index 000000000..9ccb42e56 --- /dev/null +++ b/nebula_ros/config/robosense/Bpearl.yaml @@ -0,0 +1,11 @@ +/**: + ros__parameters: + sensor_model: "Bpearl" # See readme for supported models + sensor_ip: "192.168.1.200" # Lidar Sensor IP + host_ip: "192.168.1.102" # Broadcast IP from Sensor + frame_id: "robosense" + data_port: 6699 # LiDAR Data Port (MSOP) + gnss_port: 7788 # LiDAR GNSS Port (DIFOP) + scan_phase: 0.0 # Angle where scans begin (degrees, [0.,360.] + diag_span: 1000 # milliseconds + dual_return_distance_threshold: 0.1 diff --git a/nebula_ros/config/robosense/Helios.yaml b/nebula_ros/config/robosense/Helios.yaml new file mode 100644 index 000000000..24350822d --- /dev/null +++ b/nebula_ros/config/robosense/Helios.yaml @@ -0,0 +1,11 @@ +/**: + ros__parameters: + sensor_model: "Helios" # See readme for supported models + sensor_ip: "192.168.1.200" # Lidar Sensor IP + host_ip: "192.168.1.102" # Broadcast IP from Sensor + frame_id: "robosense" + data_port: 6699 # LiDAR Data Port (MSOP) + gnss_port: 7788 # LiDAR GNSS Port (DIFOP) + scan_phase: 0.0 # Angle where scans begin (degrees, [0.,360.] + diag_span: 1000 # milliseconds + dual_return_distance_threshold: 0.1 diff --git a/nebula_ros/include/nebula_ros/robosense/robosense_decoder_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/robosense/robosense_decoder_ros_wrapper.hpp new file mode 100644 index 000000000..0f0b02acc --- /dev/null +++ b/nebula_ros/include/nebula_ros/robosense/robosense_decoder_ros_wrapper.hpp @@ -0,0 +1,99 @@ +#pragma once + +#include "nebula_common/nebula_common.hpp" +#include "nebula_common/nebula_status.hpp" +#include "nebula_common/robosense/robosense_common.hpp" +#include "nebula_decoders/nebula_decoders_robosense/robosense_driver.hpp" +#include "nebula_decoders/nebula_decoders_robosense/robosense_info_driver.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp" +#include "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp" + +#include +#include +#include +#include + +#include "robosense_msgs/msg/robosense_info_packet.hpp" +#include "robosense_msgs/msg/robosense_packet.hpp" +#include "robosense_msgs/msg/robosense_scan.hpp" + +#include + +namespace nebula +{ +namespace ros +{ +/// @brief Ros wrapper of robosense driver +class RobosenseDriverRosWrapper final : public rclcpp::Node, NebulaDriverRosWrapperBase +{ + std::shared_ptr driver_ptr_; + std::shared_ptr info_driver_ptr_; + Status wrapper_status_; + bool is_received_info{false}; + rclcpp::Subscription::SharedPtr robosense_scan_sub_; + rclcpp::Subscription::SharedPtr robosense_info_sub_; + rclcpp::Publisher::SharedPtr nebula_points_pub_; + rclcpp::Publisher::SharedPtr aw_points_ex_pub_; + rclcpp::Publisher::SharedPtr aw_points_base_pub_; + + std::shared_ptr calibration_cfg_ptr_; + std::shared_ptr sensor_cfg_ptr_; + + /// @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, + std::shared_ptr calibration_configuration) override; + + /// @brief Initializing info driver + /// @param sensor_configuration SensorConfiguration for this driver + /// @return Resulting status + Status InitializeInfoDriver( + std::shared_ptr sensor_configuration); + + /// @brief Get configurations from ros parameters + /// @param sensor_configuration Output of SensorConfiguration + /// @param calibration_configuration Output of CalibrationConfiguration + /// @return Resulting status + Status GetParameters( + drivers::RobosenseSensorConfiguration & sensor_configuration, + drivers::RobosenseCalibrationConfiguration & calibration_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)); + } + + /*** + * Publishes a sensor_msgs::msg::PointCloud2 to the specified publisher + * @param pointcloud unique pointer containing the point cloud to publish + * @param publisher + */ + void PublishCloud( + std::unique_ptr pointcloud, + const rclcpp::Publisher::SharedPtr & publisher); + +public: + explicit RobosenseDriverRosWrapper(const rclcpp::NodeOptions & options); + + /// @brief Callback for RobosenseScan subscriber + /// @param scan_msg Received RobosenseScan + void ReceiveScanMsgCallback(const robosense_msgs::msg::RobosenseScan::SharedPtr scan_msg); + + /// @brief Callback for DIFOP packet subscriber + /// @param scan_msg Received RobosensePacket + void ReceiveInfoMsgCallback(const robosense_msgs::msg::RobosenseInfoPacket::SharedPtr info_msg); + + /// @brief Get current status of this driver + /// @return Current status + Status GetStatus(); +}; + +} // namespace ros +} // namespace nebula \ No newline at end of file diff --git a/nebula_ros/include/nebula_ros/robosense/robosense_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/robosense/robosense_hw_interface_ros_wrapper.hpp new file mode 100644 index 000000000..2e1155568 --- /dev/null +++ b/nebula_ros/include/nebula_ros/robosense/robosense_hw_interface_ros_wrapper.hpp @@ -0,0 +1,88 @@ +#pragma once + +#include "nebula_common/nebula_common.hpp" +#include "nebula_common/robosense/robosense_common.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp" +#include "nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp" + +#include +#include + +#include "robosense_msgs/msg/robosense_info_packet.hpp" +#include "robosense_msgs/msg/robosense_packet.hpp" +#include "robosense_msgs/msg/robosense_scan.hpp" + +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 Robosense driver +class RobosenseHwInterfaceRosWrapper final : public rclcpp::Node, NebulaHwInterfaceWrapperBase +{ +public: + explicit RobosenseHwInterfaceRosWrapper(const rclcpp::NodeOptions & options); + + /// @brief Start point cloud streaming (Call CloudInterfaceStart 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::RobosenseSensorConfiguration & sensor_configuration); + +private: + drivers::RobosenseHwInterface hw_interface_; + drivers::RobosenseSensorConfiguration sensor_configuration_; + Status interface_status_; + + /// @brief Received Robosense Scan message publisher + rclcpp::Publisher::SharedPtr robosense_scan_pub_; + + /// @brief Received Robosense Difop message publisher + rclcpp::Publisher::SharedPtr robosense_difop_pub_; + + /// @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 RobosenseScan + /// @param scan_buffer Received RobosenseScan + void ReceiveScanDataCallback(std::unique_ptr scan_buffer); + + /// @brief Callback for receiving RobosensePacket + /// @param difop_buffer Received DIFOP packet + void ReceiveInfoDataCallback(std::unique_ptr difop_buffer); +}; + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/robosense/robosense_hw_monitor_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/robosense/robosense_hw_monitor_ros_wrapper.hpp new file mode 100644 index 000000000..2ecdc0b4f --- /dev/null +++ b/nebula_ros/include/nebula_ros/robosense/robosense_hw_monitor_ros_wrapper.hpp @@ -0,0 +1,114 @@ +#pragma once + +#include "boost_tcp_driver/tcp_driver.hpp" +#include "nebula_common/nebula_common.hpp" +#include "nebula_common/robosense/robosense_common.hpp" +#include "nebula_decoders/nebula_decoders_robosense/robosense_info_driver.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp" +#include "nebula_ros/common/nebula_hw_monitor_ros_wrapper_base.hpp" + +#include +#include +#include +#include + +#include "robosense_msgs/msg/robosense_info_packet.hpp" + +#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 monitor ros wrapper of robosense driver +class RobosenseHwMonitorRosWrapper final : public rclcpp::Node, NebulaHwMonitorWrapperBase +{ + drivers::RobosenseHwInterface hw_interface_; + Status interface_status_; + std::unique_ptr info_driver_; + std::vector info_packet_buffer_; + + drivers::RobosenseSensorConfiguration sensor_configuration_; + std::shared_ptr calibration_configuration_; + + rclcpp::Subscription::SharedPtr robosense_info_sub_; + + /// @brief Initializing hardware monitor ros wrapper + /// @param sensor_configuration SensorConfiguration for this driver + /// @return Resulting status + Status InitializeHwMonitor( + const drivers::SensorConfigurationBase & sensor_configuration) override; + +public: + explicit RobosenseHwMonitorRosWrapper(const rclcpp::NodeOptions & options); + /// @brief Not used + /// @return Current status + Status MonitorStart() override; + /// @brief Not used + /// @return Status::OK + Status MonitorStop() override; + /// @brief Not used + /// @return Status::OK + Status Shutdown() override; + /// @brief Get configurations from ros parameters + /// @param sensor_configuration Output of SensorConfiguration + /// @return Resulting status + Status GetParameters(drivers::RobosenseSensorConfiguration & sensor_configuration); + +private: + diagnostic_updater::Updater diagnostics_updater_; + /// @brief Initializing diagnostics + void InitializeRobosenseDiagnostics(); + /// @brief Callback of the timer for getting the current lidar status + void OnRobosenseStatusTimer(); + + /// @brief Check status information from RobosenseLidarStatus for diagnostic_updater + /// @param diagnostics DiagnosticStatusWrapper + void RobosenseCheckStatus(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + + /// @brief Callback for receiving DIFOP packet + /// @param info_msg Received DIFOP packet + void ReceiveInfoMsgCallback(const robosense_msgs::msg::RobosenseInfoPacket::SharedPtr info_msg); + + /// @brief rclcpp parameter callback + /// @param parameters Received parameters + /// @return SetParametersResult + rcl_interfaces::msg::SetParametersResult paramCallback( + const std::vector & parameters); + + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + + rclcpp::TimerBase::SharedPtr diagnostics_status_timer_; + std::map current_sensor_info_; + + std::unique_ptr current_info_time_; + uint16_t diag_span_{1000}; + std::optional hardware_id_; +}; + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/launch/nebula_launch.py b/nebula_ros/launch/nebula_launch.py index f9c30f47d..f84c0004b 100644 --- a/nebula_ros/launch/nebula_launch.py +++ b/nebula_ros/launch/nebula_launch.py @@ -20,6 +20,8 @@ def get_lidar_make(sensor_name): return "Hesai", ".csv" elif sensor_name[:3].lower() in ["hdl", "vlp", "vls"]: return "Velodyne", ".yaml" + elif sensor_name.lower() in ["helios", "bpearl"]: + return "Robosense", None return "unrecognized_sensor_model" @@ -35,11 +37,17 @@ def launch_setup(context, *args, **kwargs): if sensor_params_fp == "": warnings.warn("No config file provided, using sensor model default", RuntimeWarning) sensor_params_fp = os.path.join(nebula_ros_share_dir, "config", sensor_make.lower(), sensor_model + ".yaml") - sensor_calib_fp = os.path.join(nebula_decoders_share_dir, "calibration", sensor_make.lower(), sensor_model + sensor_extension) + if not os.path.exists(sensor_params_fp): sensor_params_fp = os.path.join(nebula_ros_share_dir, "config", "BaseParams.yaml") assert os.path.exists(sensor_params_fp), "Sensor params yaml file under config/ was not found: {}".format(sensor_params_fp) - assert os.path.exists(sensor_calib_fp), "Sensor calib file under calibration/ was not found: {}".format(sensor_calib_fp) + + if sensor_extension is not None: # Velodyne and Hesai + sensor_calib_fp = os.path.join(nebula_decoders_share_dir, "calibration", sensor_make.lower(), sensor_model + sensor_extension) + assert os.path.exists(sensor_calib_fp), "Sensor calib file under calibration/ was not found: {}".format(sensor_calib_fp) + else: # Robosense + sensor_calib_fp = "" + with open(sensor_params_fp, "r") as f: sensor_params = yaml.safe_load(f)["/**"]["ros__parameters"] nodes = [] diff --git a/nebula_ros/launch/robosense_launch.all_hw.xml b/nebula_ros/launch/robosense_launch.all_hw.xml new file mode 100644 index 000000000..8a3f73037 --- /dev/null +++ b/nebula_ros/launch/robosense_launch.all_hw.xml @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/nebula_ros/package.xml b/nebula_ros/package.xml index 92b082e8a..c9c86d896 100644 --- a/nebula_ros/package.xml +++ b/nebula_ros/package.xml @@ -23,6 +23,7 @@ rclcpp_components yaml-cpp velodyne_msgs + robosense_msgs ament_cmake_gtest ament_lint_auto diff --git a/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp b/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp new file mode 100644 index 000000000..d779e3bc6 --- /dev/null +++ b/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp @@ -0,0 +1,302 @@ +#include "nebula_ros/robosense/robosense_decoder_ros_wrapper.hpp" + +namespace nebula +{ +namespace ros +{ +RobosenseDriverRosWrapper::RobosenseDriverRosWrapper(const rclcpp::NodeOptions & options) +: rclcpp::Node("robosense_driver_ros_wrapper", options) +{ + RCLCPP_WARN_STREAM(this->get_logger(), "RobosenseDriverRosWrapper"); + drivers::RobosenseCalibrationConfiguration calibration_configuration; + drivers::RobosenseSensorConfiguration sensor_configuration; + + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + + wrapper_status_ = GetParameters(sensor_configuration, calibration_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..."); + + calibration_cfg_ptr_ = + std::make_shared(calibration_configuration); + + sensor_cfg_ptr_ = std::make_shared(sensor_configuration); + + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); + + robosense_scan_sub_ = create_subscription( + "robosense_packets", rclcpp::SensorDataQoS(), + std::bind(&RobosenseDriverRosWrapper::ReceiveScanMsgCallback, this, std::placeholders::_1)); + robosense_info_sub_ = create_subscription( + "robosense_difop_packets", rclcpp::SensorDataQoS(), + std::bind(&RobosenseDriverRosWrapper::ReceiveInfoMsgCallback, this, std::placeholders::_1)); + nebula_points_pub_ = this->create_publisher( + "robosense_points", rclcpp::SensorDataQoS()); + aw_points_base_pub_ = + this->create_publisher("aw_points", rclcpp::SensorDataQoS()); + aw_points_ex_pub_ = + this->create_publisher("aw_points_ex", rclcpp::SensorDataQoS()); + + RCLCPP_WARN_STREAM(this->get_logger(), "Initialized decoder ros wrapper."); +} + +void RobosenseDriverRosWrapper::ReceiveScanMsgCallback( + const robosense_msgs::msg::RobosenseScan::SharedPtr scan_msg) +{ + if (!driver_ptr_) { + if (sensor_cfg_ptr_->sensor_model == drivers::SensorModel::ROBOSENSE_BPEARL) { + if (scan_msg->packets.back().data[32] == drivers::BPEARL_V4_FLAG) { + sensor_cfg_ptr_->sensor_model = drivers::SensorModel::ROBOSENSE_BPEARL_V4; + RCLCPP_INFO_STREAM(this->get_logger(), "Bpearl V4 detected."); + } else { + sensor_cfg_ptr_->sensor_model = drivers::SensorModel::ROBOSENSE_BPEARL_V3; + RCLCPP_INFO_STREAM(this->get_logger(), "Bpearl V3 detected."); + } + } + if (!info_driver_ptr_) { + wrapper_status_ = InitializeInfoDriver( + std::const_pointer_cast(sensor_cfg_ptr_)); + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); + } + } + + if (!is_received_info) { + RCLCPP_WARN_STREAM(this->get_logger(), "Waiting for info packet."); + return; + } + + auto t_start = std::chrono::high_resolution_clock::now(); + + std::tuple pointcloud_ts = + driver_ptr_->ConvertScanToPointcloud(scan_msg); + nebula::drivers::NebulaPointCloudPtr pointcloud = std::get<0>(pointcloud_ts); + + if (pointcloud == nullptr) { + RCLCPP_WARN_STREAM(get_logger(), "Empty cloud parsed."); + return; + }; + if ( + nebula_points_pub_->get_subscription_count() > 0 || + nebula_points_pub_->get_intra_process_subscription_count() > 0) { + auto ros_pc_msg_ptr = std::make_unique(); + pcl::toROSMsg(*pointcloud, *ros_pc_msg_ptr); + ros_pc_msg_ptr->header.stamp = + rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); + PublishCloud(std::move(ros_pc_msg_ptr), nebula_points_pub_); + } + if ( + aw_points_base_pub_->get_subscription_count() > 0 || + aw_points_base_pub_->get_intra_process_subscription_count() > 0) { + const auto autoware_cloud_xyzi = + nebula::drivers::convertPointXYZIRCAEDTToPointXYZIR(pointcloud); + auto ros_pc_msg_ptr = std::make_unique(); + pcl::toROSMsg(*autoware_cloud_xyzi, *ros_pc_msg_ptr); + ros_pc_msg_ptr->header.stamp = + rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); + PublishCloud(std::move(ros_pc_msg_ptr), aw_points_base_pub_); + } + if ( + aw_points_ex_pub_->get_subscription_count() > 0 || + aw_points_ex_pub_->get_intra_process_subscription_count() > 0) { + const auto autoware_ex_cloud = nebula::drivers::convertPointXYZIRCAEDTToPointXYZIRADT( + pointcloud, std::get<1>(pointcloud_ts)); + auto ros_pc_msg_ptr = std::make_unique(); + pcl::toROSMsg(*autoware_ex_cloud, *ros_pc_msg_ptr); + ros_pc_msg_ptr->header.stamp = + rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); + PublishCloud(std::move(ros_pc_msg_ptr), aw_points_ex_pub_); + } + + auto runtime = std::chrono::high_resolution_clock::now() - t_start; + RCLCPP_DEBUG( + get_logger(), "PROFILING {'d_total': %lu, 'n_out': %lu}", runtime.count(), pointcloud->size()); +} + +void RobosenseDriverRosWrapper::ReceiveInfoMsgCallback( + const robosense_msgs::msg::RobosenseInfoPacket::SharedPtr info_msg) +{ + if (!sensor_cfg_ptr_) { + RCLCPP_WARN_STREAM(this->get_logger(), "Sensor configuration has not been initialized yet."); + return; + } + + if (!info_driver_ptr_) { + RCLCPP_WARN_STREAM(this->get_logger(), "Info driver has not been initialized yet."); + return; + } + + if (info_msg->packet.data.size() == 0) { + RCLCPP_WARN_STREAM(this->get_logger(), "Empty info packet received."); + return; + } + + std::vector info_data(info_msg->packet.data.begin(), info_msg->packet.data.end()); + const auto decode_status = info_driver_ptr_->DecodeInfoPacket(info_data); + if (decode_status != Status::OK) { + RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to decode DIFOP packet."); + return; + } + + sensor_cfg_ptr_->return_mode = info_driver_ptr_->GetReturnMode(); + sensor_cfg_ptr_->use_sensor_time = info_driver_ptr_->GetSyncStatus(); + *calibration_cfg_ptr_ = info_driver_ptr_->GetSensorCalibration(); + calibration_cfg_ptr_->CreateCorrectedChannels(); + + RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << *sensor_cfg_ptr_); + + wrapper_status_ = InitializeDriver(sensor_cfg_ptr_, calibration_cfg_ptr_); + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); + + is_received_info = true; + + // Unsubscribe from info topic + robosense_info_sub_.reset(); +} + +void RobosenseDriverRosWrapper::PublishCloud( + std::unique_ptr pointcloud, + const rclcpp::Publisher::SharedPtr & publisher) +{ + if (!sensor_cfg_ptr_->use_sensor_time) { + pointcloud->header.stamp = this->now(); + } + if (pointcloud->header.stamp.sec < 0) { + RCLCPP_WARN_STREAM(this->get_logger(), "Timestamp error, verify clock source."); + return; + } + pointcloud->header.frame_id = sensor_cfg_ptr_->frame_id; + publisher->publish(std::move(pointcloud)); +} + +Status RobosenseDriverRosWrapper::InitializeDriver( + std::shared_ptr sensor_configuration, + std::shared_ptr calibration_configuration) +{ + RCLCPP_INFO_STREAM(this->get_logger(), "Initializing driver..."); + driver_ptr_ = std::make_shared( + std::static_pointer_cast(sensor_configuration), + std::static_pointer_cast( + calibration_configuration)); + + return driver_ptr_->GetStatus(); +} + +Status RobosenseDriverRosWrapper::InitializeInfoDriver( + std::shared_ptr sensor_configuration) +{ + RCLCPP_INFO_STREAM(this->get_logger(), "Initializing info driver..."); + info_driver_ptr_ = std::make_shared( + std::static_pointer_cast(sensor_configuration)); + + return info_driver_ptr_->GetStatus(); +} + +Status RobosenseDriverRosWrapper::GetStatus() +{ + return wrapper_status_; +} + +Status RobosenseDriverRosWrapper::GetParameters( + drivers::RobosenseSensorConfiguration & sensor_configuration, + drivers::RobosenseCalibrationConfiguration & calibration_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", "255.255.255.255", 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", "192.168.1.201", 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", 2368, 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("gnss_port", 7788, descriptor); + sensor_configuration.gnss_port = this->get_parameter("gnss_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()); + calibration_configuration.SetChannelSize( + nebula::drivers::GetChannelSize(sensor_configuration.sensor_model)); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = "Dual return distance threshold [0.01, 0.5]"; + rcl_interfaces::msg::FloatingPointRange range; + range.set__from_value(0.01).set__to_value(0.5).set__step(0.01); + descriptor.floating_point_range = {range}; + this->declare_parameter("dual_return_distance_threshold", 0.1, descriptor); + sensor_configuration.dual_return_distance_threshold = + this->get_parameter("dual_return_distance_threshold").as_double(); + } + { + 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", "robosense", descriptor); + sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; + rcl_interfaces::msg::FloatingPointRange range; + range.set__from_value(0).set__to_value(360).set__step(0.01); + descriptor.floating_point_range = {range}; + this->declare_parameter("scan_phase", 0., descriptor); + sensor_configuration.scan_phase = this->get_parameter("scan_phase").as_double(); + } + + if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + if (sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { + return Status::SENSOR_CONFIG_ERROR; + } + + RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); + return Status::OK; +} + +RCLCPP_COMPONENTS_REGISTER_NODE(RobosenseDriverRosWrapper) +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/robosense/robosense_hw_interface_ros_wrapper.cpp b/nebula_ros/src/robosense/robosense_hw_interface_ros_wrapper.cpp new file mode 100644 index 000000000..edac60133 --- /dev/null +++ b/nebula_ros/src/robosense/robosense_hw_interface_ros_wrapper.cpp @@ -0,0 +1,169 @@ +#include "nebula_ros/robosense/robosense_hw_interface_ros_wrapper.hpp" + +namespace nebula +{ +namespace ros +{ +ros::RobosenseHwInterfaceRosWrapper::RobosenseHwInterfaceRosWrapper( + const rclcpp::NodeOptions & options) +: rclcpp::Node("robosense_hw_interface_ros_wrapper", options) +{ + interface_status_ = GetParameters(sensor_configuration_); + + 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( + &RobosenseHwInterfaceRosWrapper::ReceiveScanDataCallback, this, std::placeholders::_1)); + + hw_interface_.RegisterInfoCallback(std::bind( + &RobosenseHwInterfaceRosWrapper::ReceiveInfoDataCallback, this, std::placeholders::_1)); + + robosense_scan_pub_ = this->create_publisher( + "robosense_packets", rclcpp::SensorDataQoS()); + + robosense_difop_pub_ = this->create_publisher( + "robosense_difop_packets", rclcpp::SensorDataQoS()); + + StreamStart(); +} + +Status RobosenseHwInterfaceRosWrapper::StreamStart() +{ + if (Status::OK == interface_status_) { + RCLCPP_INFO_STREAM(get_logger(), "Starting interface."); + hw_interface_.CloudInterfaceStart(); + hw_interface_.InfoInterfaceStart(); + } + return interface_status_; +} +Status RobosenseHwInterfaceRosWrapper::StreamStop() +{ + return Status::OK; +} +Status RobosenseHwInterfaceRosWrapper::Shutdown() +{ + return Status::OK; +} + +Status RobosenseHwInterfaceRosWrapper::InitializeHwInterface( + const drivers::SensorConfigurationBase & sensor_configuration) +{ + std::stringstream ss; + ss << sensor_configuration; + RCLCPP_DEBUG_STREAM(this->get_logger(), ss.str()); + return Status::OK; +} + +Status RobosenseHwInterfaceRosWrapper::GetParameters( + drivers::RobosenseSensorConfiguration & 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", ""); + 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", "255.255.255.255", 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", "192.168.1.200", 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 = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("frame_id", "robosense", descriptor); + sensor_configuration.frame_id = this->get_parameter("frame_id").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", 6699, 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("gnss_port", 7788, descriptor); + sensor_configuration.gnss_port = this->get_parameter("gnss_port").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; + rcl_interfaces::msg::FloatingPointRange range; + range.set__from_value(0).set__to_value(360).set__step(0.01); + descriptor.floating_point_range = {range}; + this->declare_parameter("scan_phase", 0., descriptor); + sensor_configuration.scan_phase = this->get_parameter("scan_phase").as_double(); + } + + if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + if (sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { + return Status::SENSOR_CONFIG_ERROR; + } + + RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); + return Status::OK; +} + +void RobosenseHwInterfaceRosWrapper::ReceiveScanDataCallback( + std::unique_ptr scan_buffer) +{ + // Publish + scan_buffer->header.frame_id = sensor_configuration_.frame_id; + scan_buffer->header.stamp = scan_buffer->packets.front().stamp; + robosense_scan_pub_->publish(*scan_buffer); +} + +void RobosenseHwInterfaceRosWrapper::ReceiveInfoDataCallback( + std::unique_ptr difop_buffer) +{ + // Publish + robosense_difop_pub_->publish(*difop_buffer); +} + +RCLCPP_COMPONENTS_REGISTER_NODE(RobosenseHwInterfaceRosWrapper) + +} // namespace ros +} // namespace nebula \ No newline at end of file diff --git a/nebula_ros/src/robosense/robosense_hw_monitor_ros_wrapper.cpp b/nebula_ros/src/robosense/robosense_hw_monitor_ros_wrapper.cpp new file mode 100644 index 000000000..9818d10e5 --- /dev/null +++ b/nebula_ros/src/robosense/robosense_hw_monitor_ros_wrapper.cpp @@ -0,0 +1,192 @@ +#include "nebula_ros/robosense/robosense_hw_monitor_ros_wrapper.hpp" + +#include + +namespace nebula +{ +namespace ros +{ +RobosenseHwMonitorRosWrapper::RobosenseHwMonitorRosWrapper(const rclcpp::NodeOptions & options) +: rclcpp::Node("robosense_hw_monitor_ros_wrapper", options), diagnostics_updater_(this) +{ + interface_status_ = GetParameters(sensor_configuration_); + if (Status::OK != interface_status_) { + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); + return; + } + + robosense_info_sub_ = create_subscription( + "robosense_difop_packets", rclcpp::SensorDataQoS(), + std::bind(&RobosenseHwMonitorRosWrapper::ReceiveInfoMsgCallback, this, std::placeholders::_1)); + + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&RobosenseHwMonitorRosWrapper::paramCallback, this, std::placeholders::_1)); +} + +Status RobosenseHwMonitorRosWrapper::MonitorStart() +{ + return interface_status_; +} + +Status RobosenseHwMonitorRosWrapper::MonitorStop() +{ + return Status::OK; +} +Status RobosenseHwMonitorRosWrapper::Shutdown() +{ + return Status::OK; +} +// +Status RobosenseHwMonitorRosWrapper::InitializeHwMonitor( + const drivers::SensorConfigurationBase & sensor_configuration) +{ + std::stringstream ss; + ss << sensor_configuration; + RCLCPP_DEBUG_STREAM(this->get_logger(), ss.str()); + return Status::OK; +} + +Status RobosenseHwMonitorRosWrapper::GetParameters( + drivers::RobosenseSensorConfiguration & 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", ""); + sensor_configuration.sensor_model = + nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); + } + + if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = "milliseconds"; + this->declare_parameter("diag_span", 1000, descriptor); + diag_span_ = this->get_parameter("diag_span").as_int(); + } + + return Status::OK; +} + +void RobosenseHwMonitorRosWrapper::InitializeRobosenseDiagnostics() +{ + RCLCPP_INFO_STREAM(this->get_logger(), "InitializeRobosenseDiagnostics"); + diagnostics_updater_.setHardwareID(*hardware_id_); + RCLCPP_INFO_STREAM(this->get_logger(), "hardware_id: " + *hardware_id_); + + diagnostics_updater_.add( + "robosense_status", this, &RobosenseHwMonitorRosWrapper::RobosenseCheckStatus); + + auto on_timer_status = [this] { OnRobosenseStatusTimer(); }; + diagnostics_status_timer_ = + this->create_wall_timer(std::chrono::milliseconds(diag_span_), std::move(on_timer_status)); + + RCLCPP_DEBUG_STREAM(get_logger(), "add_timer"); +} + +void RobosenseHwMonitorRosWrapper::OnRobosenseStatusTimer() +{ + RCLCPP_DEBUG_STREAM(this->get_logger(), "OnRobosenseStatusTimer" << std::endl); + info_driver_->DecodeInfoPacket(info_packet_buffer_); + current_sensor_info_ = info_driver_->GetSensorInfo(); + current_info_time_ = std::make_unique(this->get_clock()->now()); +} + +void RobosenseHwMonitorRosWrapper::RobosenseCheckStatus( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + if (!hardware_id_.has_value()) { + return; + } + + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + + for (const auto & info : current_sensor_info_) { + diagnostics.add(info.first, info.second); + } + + diagnostics.summary(level, "OK"); +} + +rcl_interfaces::msg::SetParametersResult RobosenseHwMonitorRosWrapper::paramCallback( + const std::vector & parameters) +{ + RCLCPP_DEBUG_STREAM(get_logger(), "add_on_set_parameters_callback"); + RCLCPP_DEBUG_STREAM(get_logger(), parameters); + RCLCPP_DEBUG_STREAM(get_logger(), sensor_configuration_); + RCLCPP_INFO_STREAM(this->get_logger(), parameters); + + drivers::RobosenseSensorConfiguration new_param{sensor_configuration_}; + RCLCPP_INFO_STREAM(this->get_logger(), new_param); + uint16_t new_diag_span = 0; + if (get_param(parameters, "diag_span", new_diag_span)) { + 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"); + } + + 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 RobosenseHwMonitorRosWrapper::ReceiveInfoMsgCallback( + const robosense_msgs::msg::RobosenseInfoPacket::SharedPtr info_msg) +{ + if (!info_driver_) { + auto sensor_cfg_ptr = + std::make_shared(sensor_configuration_); + + if (sensor_cfg_ptr->sensor_model == drivers::SensorModel::ROBOSENSE_BPEARL) { + if ( + drivers::SensorModelFromString(info_msg->lidar_model) == + drivers::SensorModel::ROBOSENSE_BPEARL_V3) { + sensor_cfg_ptr->sensor_model = drivers::SensorModel::ROBOSENSE_BPEARL_V3; + } else if ( + drivers::SensorModelFromString(info_msg->lidar_model) == + drivers::SensorModel::ROBOSENSE_BPEARL_V4) { + sensor_cfg_ptr->sensor_model = drivers::SensorModel::ROBOSENSE_BPEARL_V4; + } else { + RCLCPP_ERROR_STREAM(this->get_logger(), "No version for Bpearl."); + return; + } + } + + info_driver_ = std::make_unique(sensor_cfg_ptr); + } + + info_packet_buffer_ = + std::vector(info_msg->packet.data.begin(), info_msg->packet.data.end()); + + if (!hardware_id_.has_value()) { + info_driver_->DecodeInfoPacket(info_packet_buffer_); + current_sensor_info_ = info_driver_->GetSensorInfo(); + current_info_time_ = std::make_unique(this->get_clock()->now()); + + RCLCPP_INFO_STREAM(this->get_logger(), "Model:" << sensor_configuration_.sensor_model); + RCLCPP_INFO_STREAM(this->get_logger(), "Serial:" << current_sensor_info_["serial_number"]); + + hardware_id_.emplace( + nebula::drivers::SensorModelToString(sensor_configuration_.sensor_model) + ": " + + current_sensor_info_["serial_number"]); + InitializeRobosenseDiagnostics(); + } +} + +RCLCPP_COMPONENTS_REGISTER_NODE(RobosenseHwMonitorRosWrapper) +} // namespace ros +} // namespace nebula