From 7fae72d086480a7fe9bd1d4dfe1f28e36549970f Mon Sep 17 00:00:00 2001 From: Maximilian Schmeller Date: Thu, 21 Mar 2024 16:58:45 +0900 Subject: [PATCH 01/57] fix(hesai_decoder): print config instead of config address --- .../nebula_decoders_hesai/decoders/hesai_decoder.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp index 7bdfd069e..b81b45d44 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp @@ -213,7 +213,7 @@ class HesaiDecoder : public HesaiScanDecoder logger_(rclcpp::get_logger("HesaiDecoder")) { logger_.set_level(rclcpp::Logger::Level::Debug); - RCLCPP_INFO_STREAM(logger_, sensor_configuration_); + RCLCPP_INFO_STREAM(logger_, *sensor_configuration_); decode_pc_.reset(new NebulaPointCloud); output_pc_.reset(new NebulaPointCloud); From 694f28d9ade295061dc410793c427f3e46bea5c0 Mon Sep 17 00:00:00 2001 From: Maximilian Schmeller Date: Thu, 21 Mar 2024 17:01:34 +0900 Subject: [PATCH 02/57] refactor(nebula_ros): combine Hesai wrapper nodes into one This is step one of the single node refactoring of Nebula. In this step, only the three Hesai wrapper nodes were combined into one, and no optimizations have been done that are made possible by this refactoring. The next step is to do those optimizations (e.g. get rid of unnecessary pointcloud copying). --- nebula_common/CMakeLists.txt | 2 + nebula_ros/CMakeLists.txt | 31 +- .../common/nebula_ros_wrapper_base.hpp | 68 + .../hesai/hesai_decoder_ros_wrapper.hpp | 106 -- .../hesai/hesai_hw_interface_ros_wrapper.hpp | 100 -- ..._ros_wrapper.hpp => hesai_ros_wrapper.hpp} | 194 ++- nebula_ros/launch/hesai_launch_all_hw.xml | 68 +- .../launch/hesai_launch_component.launch.xml | 115 -- .../src/hesai/hesai_decoder_ros_wrapper.cpp | 488 ------- .../hesai/hesai_hw_interface_ros_wrapper.cpp | 510 ------- .../hesai/hesai_hw_monitor_ros_wrapper.cpp | 620 --------- nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 1234 +++++++++++++++++ 12 files changed, 1475 insertions(+), 2061 deletions(-) create mode 100644 nebula_ros/include/nebula_ros/common/nebula_ros_wrapper_base.hpp delete mode 100644 nebula_ros/include/nebula_ros/hesai/hesai_decoder_ros_wrapper.hpp delete mode 100644 nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp rename nebula_ros/include/nebula_ros/hesai/{hesai_hw_monitor_ros_wrapper.hpp => hesai_ros_wrapper.hpp} (53%) delete mode 100644 nebula_ros/launch/hesai_launch_component.launch.xml delete mode 100644 nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp delete mode 100644 nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp delete mode 100644 nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp create mode 100644 nebula_ros/src/hesai/hesai_ros_wrapper.cpp diff --git a/nebula_common/CMakeLists.txt b/nebula_common/CMakeLists.txt index 4c146736c..154f8e453 100644 --- a/nebula_common/CMakeLists.txt +++ b/nebula_common/CMakeLists.txt @@ -40,6 +40,8 @@ ament_auto_add_library(nebula_common SHARED src/velodyne/velodyne_calibration_decoder.cpp ) +target_link_libraries(nebula_common yaml-cpp) + ament_auto_package() # Set ROS_DISTRO macros diff --git a/nebula_ros/CMakeLists.txt b/nebula_ros/CMakeLists.txt index dd81e1ac0..18ce06aad 100644 --- a/nebula_ros/CMakeLists.txt +++ b/nebula_ros/CMakeLists.txt @@ -33,34 +33,13 @@ include_directories( ) link_libraries(${YAML_CPP_LIBRARIES} ${PCL_LIBRARIES}) ## Hesai -# Hw Interface -ament_auto_add_library(hesai_hw_ros_wrapper SHARED - src/hesai/hesai_hw_interface_ros_wrapper.cpp - ) - -rclcpp_components_register_node(hesai_hw_ros_wrapper - PLUGIN "HesaiHwInterfaceRosWrapper" - EXECUTABLE hesai_hw_interface_ros_wrapper_node - ) - -# Monitor -ament_auto_add_library(hesai_hw_monitor_ros_wrapper SHARED - src/hesai/hesai_hw_monitor_ros_wrapper.cpp - ) - -rclcpp_components_register_node(hesai_hw_monitor_ros_wrapper - PLUGIN "HesaiHwMonitorRosWrapper" - EXECUTABLE hesai_hw_monitor_ros_wrapper_node - ) - -# DriverDecoder -ament_auto_add_library(hesai_driver_ros_wrapper SHARED - src/hesai/hesai_decoder_ros_wrapper.cpp +ament_auto_add_library(hesai_ros_wrapper SHARED + src/hesai/hesai_ros_wrapper.cpp ) -rclcpp_components_register_node(hesai_driver_ros_wrapper - PLUGIN "HesaiDriverRosWrapper" - EXECUTABLE hesai_driver_ros_wrapper_node +rclcpp_components_register_node(hesai_ros_wrapper + PLUGIN "HesaiRosWrapper" + EXECUTABLE hesai_ros_wrapper_node ) ## Velodyne diff --git a/nebula_ros/include/nebula_ros/common/nebula_ros_wrapper_base.hpp b/nebula_ros/include/nebula_ros/common/nebula_ros_wrapper_base.hpp new file mode 100644 index 000000000..5aaeb634b --- /dev/null +++ b/nebula_ros/include/nebula_ros/common/nebula_ros_wrapper_base.hpp @@ -0,0 +1,68 @@ +#pragma once + +#include "nebula_common/nebula_common.hpp" +#include "nebula_common/nebula_status.hpp" + +#include +#include +#include + +#include + +#include +#include + +namespace nebula +{ +namespace ros +{ +/// @brief Base class for ros wrapper of each sensor driver +class NebulaRosWrapperBase +{ +public: + NebulaRosWrapperBase() = default; + + NebulaRosWrapperBase(NebulaRosWrapperBase && c) = delete; + NebulaRosWrapperBase & operator=(NebulaRosWrapperBase && c) = delete; + NebulaRosWrapperBase(const NebulaRosWrapperBase & c) = delete; + NebulaRosWrapperBase & operator=(const NebulaRosWrapperBase & c) = delete; + + /// @brief Start point cloud streaming (Call CloudInterfaceStart of HwInterface) + /// @return Resulting status + virtual Status StreamStart() = 0; + + /// @brief Stop point cloud streaming (not used) + /// @return Resulting status + virtual Status StreamStop() = 0; + + /// @brief Shutdown (not used) + /// @return Resulting status + virtual Status Shutdown() = 0; + +private: + /// @brief Virtual function for initializing hardware interface ros wrapper + /// @param sensor_configuration SensorConfiguration for this driver + /// @return Resulting status + virtual Status InitializeHwInterface( + const drivers::SensorConfigurationBase & sensor_configuration) = 0; + + /// @brief Virtual function for initializing hardware monitor ros wrapper + /// @param sensor_configuration SensorConfiguration for this driver + /// @return Resulting status + virtual Status InitializeHwMonitor( + const drivers::SensorConfigurationBase & sensor_configuration) = 0; + + /// @brief Virtual function for initializing ros wrapper + /// @param sensor_configuration SensorConfiguration for this driver + /// @param calibration_configuration CalibrationConfiguration for this driver + /// @return Resulting status + virtual Status InitializeCloudDriver( + std::shared_ptr sensor_configuration, + std::shared_ptr calibration_configuration) = 0; + + /// @brief Point cloud publisher + rclcpp::Publisher::SharedPtr cloud_pub_; +}; + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_decoder_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_decoder_ros_wrapper.hpp deleted file mode 100644 index 52518fcd0..000000000 --- a/nebula_ros/include/nebula_ros/hesai/hesai_decoder_ros_wrapper.hpp +++ /dev/null @@ -1,106 +0,0 @@ -#ifndef NEBULA_HesaiDriverRosWrapper_H -#define NEBULA_HesaiDriverRosWrapper_H - -#include "nebula_common/hesai/hesai_common.hpp" -#include "nebula_common/nebula_common.hpp" -#include "nebula_common/nebula_status.hpp" -#include "nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp" -#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" -#include "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp" - -#include -#include -#include -#include - -#include - -#include "pandar_msgs/msg/pandar_packet.hpp" -#include "pandar_msgs/msg/pandar_scan.hpp" - -namespace nebula -{ -namespace ros -{ -/// @brief Ros wrapper of hesai driver -class HesaiDriverRosWrapper final : public rclcpp::Node, NebulaDriverRosWrapperBase -{ - std::shared_ptr driver_ptr_; - Status wrapper_status_; - rclcpp::Subscription::SharedPtr pandar_scan_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_; - std::shared_ptr correction_cfg_ptr_; - - drivers::HesaiHwInterface hw_interface_; - - /// @brief Initializing ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @return Resulting status - Status InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) override; - - /// @brief Initializing ros wrapper for AT - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @param correction_configuration CorrectionConfiguration for this driver - /// @return Resulting status - Status InitializeDriver( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration, - const std::shared_ptr & correction_configuration); - - /// @brief Get configurations from ros parameters - /// @param sensor_configuration Output of SensorConfiguration - /// @param calibration_configuration Output of CalibrationConfiguration - /// @param correction_configuration Output of CorrectionConfiguration (for AT) - /// @return Resulting status - Status GetParameters( - drivers::HesaiSensorConfiguration & sensor_configuration, - drivers::HesaiCalibrationConfiguration & calibration_configuration, - drivers::HesaiCorrection & correction_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 HesaiDriverRosWrapper(const rclcpp::NodeOptions & options); - - /// @brief Callback for PandarScan subscriber - /// @param scan_msg Received PandarScan - void ReceiveScanMsgCallback(const pandar_msgs::msg::PandarScan::SharedPtr scan_msg); - - /// @brief Get current status of this driver - /// @return Current status - Status GetStatus(); - -private: - /// @brief File path of Correction data (Only required only for AT) - std::string correction_file_path; -}; - -} // namespace ros -} // namespace nebula - -#endif // NEBULA_HesaiDriverRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp deleted file mode 100644 index 1802f87b2..000000000 --- a/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp +++ /dev/null @@ -1,100 +0,0 @@ -#ifndef NEBULA_HesaiHwInterfaceRosWrapper_H -#define NEBULA_HesaiHwInterfaceRosWrapper_H - -#include "nebula_common/hesai/hesai_common.hpp" -#include "nebula_common/nebula_common.hpp" -#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" -#include "nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp" -#include "boost_tcp_driver/tcp_driver.hpp" - -#include -#include -#include - -#include "pandar_msgs/msg/pandar_packet.hpp" -#include "pandar_msgs/msg/pandar_scan.hpp" - -#include - -#include -#include - -namespace nebula -{ -namespace ros -{ -/// @brief Get parameter from rclcpp::Parameter -/// @tparam T -/// @param p Parameter from rclcpp parameter callback -/// @param name Target parameter name -/// @param value Corresponding value -/// @return Whether the target name existed -template -bool get_param(const std::vector & p, const std::string & name, T & value) -{ - auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { - return parameter.get_name() == name; - }); - if (it != p.cend()) { - value = it->template get_value(); - return true; - } - return false; -} - -/// @brief Hardware interface ros wrapper of hesai driver -class HesaiHwInterfaceRosWrapper final : public rclcpp::Node, NebulaHwInterfaceWrapperBase -{ - drivers::HesaiHwInterface hw_interface_; - Status interface_status_; - - drivers::HesaiSensorConfiguration sensor_configuration_; - - /// @brief Received Hesai message publisher - rclcpp::Publisher::SharedPtr pandar_scan_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 PandarScan - /// @param scan_buffer Received PandarScan - void ReceiveScanDataCallback(std::unique_ptr scan_buffer); - -public: - explicit HesaiHwInterfaceRosWrapper(const rclcpp::NodeOptions & options); - ~HesaiHwInterfaceRosWrapper() noexcept override; - /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) - /// @return Resulting status - Status StreamStart() override; - /// @brief Stop point cloud streaming (not used) - /// @return Resulting status - Status StreamStop() override; - /// @brief Shutdown (not used) - /// @return Resulting status - Status Shutdown() override; - /// @brief Get configurations from ros parameters - /// @param sensor_configuration Output of SensorConfiguration - /// @return Resulting status - Status GetParameters(drivers::HesaiSensorConfiguration & sensor_configuration); - -private: - uint16_t delay_hw_ms_; - bool retry_hw_; - std::mutex mtx_config_; - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - /// @brief rclcpp parameter callback - /// @param parameters Received parameters - /// @return SetParametersResult - rcl_interfaces::msg::SetParametersResult paramCallback( - const std::vector & parameters); - /// @brief Updating rclcpp parameter - /// @return SetParametersResult - std::vector updateParameters(); -}; - -} // namespace ros -} // namespace nebula - -#endif // NEBULA_HesaiHwInterfaceRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp similarity index 53% rename from nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp rename to nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp index cf8739279..283f63c68 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp @@ -1,24 +1,29 @@ -#ifndef NEBULA_HesaiHwMonitorRosWrapper_H -#define NEBULA_HesaiHwMonitorRosWrapper_H +#pragma once +#include "boost_tcp_driver/tcp_driver.hpp" #include "nebula_common/hesai/hesai_common.hpp" #include "nebula_common/nebula_common.hpp" +#include "nebula_common/nebula_status.hpp" +#include "nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" +#include "nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp" #include "nebula_ros/common/nebula_hw_monitor_ros_wrapper_base.hpp" -#include "boost_tcp_driver/tcp_driver.hpp" +#include "nebula_ros/common/nebula_ros_wrapper_base.hpp" #include #include #include #include -#include +#include "pandar_msgs/msg/pandar_packet.hpp" +#include "pandar_msgs/msg/pandar_scan.hpp" -#include #include #include #include +#include +#include #include #include @@ -26,6 +31,7 @@ namespace nebula { namespace ros { + /// @brief Get parameter from rclcpp::Parameter /// @tparam T /// @param p Parameter from rclcpp parameter callback @@ -45,39 +51,106 @@ bool get_param(const std::vector & p, const std::string & nam return false; } -/// @brief Hardware monitor ros wrapper of hesai driver -class HesaiHwMonitorRosWrapper final : public rclcpp::Node, NebulaHwMonitorWrapperBase +/// @brief Ros wrapper of hesai driver +class HesaiRosWrapper final : public rclcpp::Node, NebulaRosWrapperBase { - drivers::HesaiHwInterface hw_interface_; - Status interface_status_; +public: + explicit HesaiRosWrapper(const rclcpp::NodeOptions & options); + ~HesaiRosWrapper(); - drivers::HesaiSensorConfiguration sensor_configuration_; + /// @brief Callback for PandarScan subscriber + /// @param scan_msg Received PandarScan + void ReceiveScanMsgCallback(const pandar_msgs::msg::PandarScan::SharedPtr scan_msg); - /// @brief Initializing hardware monitor ros wrapper + /// @brief Get current status of this driver + /// @return Current status + Status GetStatus(); + + /// @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; + +private: + /// @brief Initializing ros wrapper /// @param sensor_configuration SensorConfiguration for this driver + /// @param calibration_configuration CalibrationConfiguration for this driver /// @return Resulting status - Status InitializeHwMonitor( - const drivers::SensorConfigurationBase & sensor_configuration) override; + Status InitializeCloudDriver( + std::shared_ptr sensor_configuration, + std::shared_ptr calibration_configuration) override; + + /// @brief Initializing ros wrapper for AT + /// @param sensor_configuration SensorConfiguration for this driver + /// @param calibration_configuration CalibrationConfiguration for this driver + /// @param correction_configuration CorrectionConfiguration for this driver + /// @return Resulting status + Status InitializeCloudDriver( + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & calibration_configuration, + const std::shared_ptr & correction_configuration); -public: - explicit HesaiHwMonitorRosWrapper(const rclcpp::NodeOptions & options); - ~HesaiHwMonitorRosWrapper() noexcept override; - /// @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::HesaiSensorConfiguration & sensor_configuration); + Status GetParameters( + drivers::HesaiSensorConfiguration & sensor_configuration); + + /// @brief Get calibration data from the sensor + /// @param calibration_configuration Output of CalibrationConfiguration + /// @param correction_configuration Output of CorrectionConfiguration (for AT) + /// @return Resulting status + Status GetCalibrationData( + drivers::HesaiCalibrationConfiguration & calibration_configuration, + drivers::HesaiCorrection & correction_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); + + /// @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 PandarScan + /// @param scan_buffer Received PandarScan + void ReceiveScanDataCallback(std::unique_ptr scan_buffer); + + /// @brief rclcpp parameter callback + /// @param parameters Received parameters + /// @return SetParametersResult + rcl_interfaces::msg::SetParametersResult paramCallback( + const std::vector & parameters); + /// @brief Updating rclcpp parameter + /// @return SetParametersResult + std::vector updateParameters(); + + /// @brief Initializing hardware monitor ros wrapper + /// @param sensor_configuration SensorConfiguration for this driver + /// @return Resulting status + Status InitializeHwMonitor( + const drivers::SensorConfigurationBase & sensor_configuration) override; -private: - diagnostic_updater::Updater diagnostics_updater_; /// @brief Initializing diagnostics void InitializeHesaiDiagnostics(); /// @brief Callback of the timer for getting the current lidar status @@ -107,6 +180,49 @@ class HesaiHwMonitorRosWrapper final : public rclcpp::Node, NebulaHwMonitorWrapp /// @brief Check voltage information from HesaiLidarStatus for diagnostic_updater via tcp /// @param diagnostics DiagnosticStatusWrapper void HesaiCheckVoltage(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + /// @brief Get value from property_tree + /// @param pt property_tree + /// @param key Pey string + /// @return Value + std::string GetPtreeValue(boost::property_tree::ptree * pt, const std::string & key); + /// @brief Making fixed precision string + /// @param val Target value + /// @param pre Precision + /// @return Created string + std::string GetFixedPrecisionString(double val, int pre = 2); + + std::shared_ptr driver_ptr_; + Status wrapper_status_; + rclcpp::Subscription::SharedPtr pandar_scan_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_; + std::shared_ptr correction_cfg_ptr_; + + Status interface_status_; + + //todo: temporary class member during single node refactoring + bool launch_hw_; + + //todo: temporary class member during single node refactoring + std::string calibration_file_path; + /// @brief File path of Correction data (Only required only for AT) + std::string correction_file_path; + + /// @brief Received Hesai message publisher + rclcpp::Publisher::SharedPtr pandar_scan_pub_; + + drivers::HesaiHwInterface hw_interface_; + + uint16_t delay_hw_ms_; + bool retry_hw_; + std::mutex mtx_config_; + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + + diagnostic_updater::Updater diagnostics_updater_; rclcpp::TimerBase::SharedPtr diagnostics_update_timer_; rclcpp::TimerBase::SharedPtr diagnostics_update_monitor_timer_; @@ -128,24 +244,6 @@ class HesaiHwMonitorRosWrapper final : public rclcpp::Node, NebulaHwMonitorWrapp std::mutex mtx_diag; std::mutex mtx_status; std::mutex mtx_lidar_monitor; - // std::timed_mutex mtx_lidar_monitor; - std::mutex mtx_config_; - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - /// @brief rclcpp parameter callback - /// @param parameters Received parameters - /// @return SetParametersResult - rcl_interfaces::msg::SetParametersResult paramCallback( - const std::vector & parameters); - /// @brief Get value from property_tree - /// @param pt property_tree - /// @param key Pey string - /// @return Value - std::string GetPtreeValue(boost::property_tree::ptree * pt, const std::string & key); - /// @brief Making fixed precision string - /// @param val Target value - /// @param pre Precision - /// @return Created string - std::string GetFixedPrecisionString(double val, int pre = 2); std::string info_model; std::string info_serial; @@ -158,9 +256,9 @@ class HesaiHwMonitorRosWrapper final : public rclcpp::Node, NebulaHwMonitorWrapp std::string message_sep; std::vector temperature_names; + + bool setup_sensor; }; } // namespace ros } // namespace nebula - -#endif // NEBULA_HesaiHwMonitorRosWrapper_H diff --git a/nebula_ros/launch/hesai_launch_all_hw.xml b/nebula_ros/launch/hesai_launch_all_hw.xml index f4a1cd41c..579852fb8 100644 --- a/nebula_ros/launch/hesai_launch_all_hw.xml +++ b/nebula_ros/launch/hesai_launch_all_hw.xml @@ -35,8 +35,8 @@ - + @@ -45,52 +45,24 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + diff --git a/nebula_ros/launch/hesai_launch_component.launch.xml b/nebula_ros/launch/hesai_launch_component.launch.xml deleted file mode 100644 index 75c7414da..000000000 --- a/nebula_ros/launch/hesai_launch_component.launch.xml +++ /dev/null @@ -1,115 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp deleted file mode 100644 index 6f27d547e..000000000 --- a/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp +++ /dev/null @@ -1,488 +0,0 @@ -#include "nebula_ros/hesai/hesai_decoder_ros_wrapper.hpp" - -namespace nebula -{ -namespace ros -{ -HesaiDriverRosWrapper::HesaiDriverRosWrapper(const rclcpp::NodeOptions & options) -: rclcpp::Node("hesai_driver_ros_wrapper", options), hw_interface_() -{ - drivers::HesaiCalibrationConfiguration calibration_configuration; - drivers::HesaiSensorConfiguration sensor_configuration; - drivers::HesaiCorrection correction_configuration; - - setvbuf(stdout, NULL, _IONBF, BUFSIZ); - - hw_interface_.SetLogger(std::make_shared(this->get_logger())); - - wrapper_status_ = - GetParameters(sensor_configuration, calibration_configuration, correction_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 "); - if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - correction_cfg_ptr_ = std::make_shared(correction_configuration); - wrapper_status_ = InitializeDriver( - std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_), - std::static_pointer_cast(correction_cfg_ptr_)); - } else { - wrapper_status_ = InitializeDriver( - std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_)); - } - - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Wrapper=" << wrapper_status_); - rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; - auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), - qos_profile); - pandar_scan_sub_ = create_subscription( - "pandar_packets", qos, - std::bind(&HesaiDriverRosWrapper::ReceiveScanMsgCallback, this, std::placeholders::_1)); - nebula_points_pub_ = - this->create_publisher("pandar_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()); -} - -void HesaiDriverRosWrapper::ReceiveScanMsgCallback( - const pandar_msgs::msg::PandarScan::SharedPtr scan_msg) -{ - 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 HesaiDriverRosWrapper::PublishCloud( - std::unique_ptr pointcloud, - const rclcpp::Publisher::SharedPtr & publisher) -{ - if (pointcloud->header.stamp.sec < 0) { - RCLCPP_WARN_STREAM(this->get_logger(), "Timestamp error, verify clock source."); - } - pointcloud->header.frame_id = sensor_cfg_ptr_->frame_id; - publisher->publish(std::move(pointcloud)); -} - -Status HesaiDriverRosWrapper::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) -{ - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration)); - return driver_ptr_->GetStatus(); -} - -Status HesaiDriverRosWrapper::InitializeDriver( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration, - const std::shared_ptr & correction_configuration) -{ - // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration), - std::static_pointer_cast(correction_configuration)); - return driver_ptr_->GetStatus(); -} - -Status HesaiDriverRosWrapper::GetStatus() { return wrapper_status_; } - -Status HesaiDriverRosWrapper::GetParameters( - drivers::HesaiSensorConfiguration & sensor_configuration, - drivers::HesaiCalibrationConfiguration & calibration_configuration, - drivers::HesaiCorrection & correction_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_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", ""); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("return_mode", "", descriptor); - sensor_configuration.return_mode = nebula::drivers::ReturnModeFromStringHesai( - this->get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); - } - { - 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", "pandar", 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(); - } - { - 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("calibration_file", "", descriptor); - calibration_configuration.calibration_file = - this->get_parameter("calibration_file").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 = ""; - this->declare_parameter("min_range", 0.3, descriptor); - sensor_configuration.min_range = this->get_parameter("min_range").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("max_range", 300., descriptor); - sensor_configuration.max_range = this->get_parameter("max_range").as_double(); - } - if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - 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("correction_file", "", descriptor); - correction_file_path = this->get_parameter("correction_file").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 = "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(); - } - bool launch_hw; - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("launch_hw", "", descriptor); - launch_hw = this->get_parameter("launch_hw").as_bool(); - } - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - if (sensor_configuration.return_mode == nebula::drivers::ReturnMode::UNKNOWN) { - return Status::INVALID_ECHO_MODE; - } - if (sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { - return Status::SENSOR_CONFIG_ERROR; - } - - std::shared_ptr sensor_cfg_ptr = - std::make_shared(sensor_configuration); - - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - - bool run_local = !launch_hw; - if (sensor_configuration.sensor_model != drivers::SensorModel::HESAI_PANDARAT128) { - std::string calibration_file_path_from_sensor; - if (launch_hw && !calibration_configuration.calibration_file.empty()) { - int ext_pos = calibration_configuration.calibration_file.find_last_of('.'); - calibration_file_path_from_sensor += calibration_configuration.calibration_file.substr(0, ext_pos); - calibration_file_path_from_sensor += "_from_sensor"; - calibration_file_path_from_sensor += calibration_configuration.calibration_file.substr(ext_pos, calibration_configuration.calibration_file.size() - ext_pos); - } - if(launch_hw) { - run_local = false; - RCLCPP_INFO_STREAM( - this->get_logger(), "Trying to acquire calibration data from sensor: '" - << sensor_configuration.sensor_ip << "'"); - std::future future = std::async(std::launch::async, - [this, &calibration_configuration, &calibration_file_path_from_sensor, &run_local]() { - if (hw_interface_.InitializeTcpDriver() == Status::OK) { - auto str = hw_interface_.GetLidarCalibrationString(); - auto rt = calibration_configuration.SaveFileFromString( - calibration_file_path_from_sensor, str); - RCLCPP_ERROR_STREAM(get_logger(), str); - if (rt == Status::OK) { - RCLCPP_INFO_STREAM(get_logger(), "SaveFileFromString success:" - << calibration_file_path_from_sensor << "\n"); - } else { - RCLCPP_ERROR_STREAM(get_logger(), "SaveFileFromString failed:" - << calibration_file_path_from_sensor << "\n"); - } - rt = calibration_configuration.LoadFromString(str); - if (rt == Status::OK) { - RCLCPP_INFO_STREAM(get_logger(), - "LoadFromString success:" << str << "\n"); - } else { - RCLCPP_ERROR_STREAM(get_logger(), - "LoadFromString failed:" << str << "\n"); - } - } else { - run_local = true; - } - }); - std::future_status status; - status = future.wait_for(std::chrono::milliseconds(5000)); - if (status == std::future_status::timeout) { - std::cerr << "# std::future_status::timeout\n"; - RCLCPP_ERROR_STREAM(get_logger(), "GetCalibration Timeout"); - run_local = true; - } else if (status == std::future_status::ready && !run_local) { - RCLCPP_INFO_STREAM( - this->get_logger(), "Acquired calibration data from sensor: '" - << sensor_configuration.sensor_ip << "'"); - RCLCPP_INFO_STREAM( - this->get_logger(), "The calibration has been saved in '" - << calibration_file_path_from_sensor << "'"); - } - } - if(run_local) { - RCLCPP_WARN_STREAM(get_logger(), "Running locally"); - bool run_org = false; - if (calibration_file_path_from_sensor.empty()) { - run_org = true; - } else { - RCLCPP_INFO_STREAM(get_logger(),"Trying to load file: " << calibration_file_path_from_sensor); - auto cal_status = - calibration_configuration.LoadFromFile(calibration_file_path_from_sensor); - - if (cal_status != Status::OK) { - run_org = true; - }else{ - RCLCPP_INFO_STREAM( - this->get_logger(), "Load calibration data from: '" - << calibration_file_path_from_sensor << "'"); - } - } - if(run_org) { - RCLCPP_INFO_STREAM(get_logger(),"Trying to load file: " << calibration_configuration.calibration_file); - if (calibration_configuration.calibration_file.empty()) { - RCLCPP_ERROR_STREAM( - this->get_logger(), "Empty Calibration_file File: '" << calibration_configuration.calibration_file << "'"); - return Status::INVALID_CALIBRATION_FILE; - } else { - auto cal_status = - calibration_configuration.LoadFromFile(calibration_configuration.calibration_file); - - if (cal_status != Status::OK) { - RCLCPP_ERROR_STREAM( - this->get_logger(), - "Given Calibration File: '" << calibration_configuration.calibration_file << "'"); - return cal_status; - }else{ - RCLCPP_INFO_STREAM( - this->get_logger(), "Load calibration data from: '" - << calibration_configuration.calibration_file << "'"); - } - } - } - } - } else { // sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128 - std::string correction_file_path_from_sensor; - if (launch_hw && !correction_file_path.empty()) { - int ext_pos = correction_file_path.find_last_of('.'); - correction_file_path_from_sensor += correction_file_path.substr(0, ext_pos); - correction_file_path_from_sensor += "_from_sensor"; - correction_file_path_from_sensor += correction_file_path.substr(ext_pos, correction_file_path.size() - ext_pos); - } - std::future future = std::async(std::launch::async, [this, &correction_configuration, &correction_file_path_from_sensor, &run_local, &launch_hw]() { - if (launch_hw && hw_interface_.InitializeTcpDriver() == Status::OK) { - RCLCPP_INFO_STREAM( - this->get_logger(), "Trying to acquire calibration data from sensor"); - auto received_bytes = hw_interface_.GetLidarCalibrationBytes(); - RCLCPP_INFO_STREAM(get_logger(), "AT128 calibration size:" << received_bytes.size() << "\n"); - auto rt = correction_configuration.SaveFileFromBinary(correction_file_path_from_sensor, received_bytes); - if(rt == Status::OK) - { - RCLCPP_INFO_STREAM(get_logger(), "SaveFileFromBinary success:" << correction_file_path_from_sensor << "\n"); - } - else - { - RCLCPP_ERROR_STREAM(get_logger(), "SaveFileFromBinary failed:" << correction_file_path_from_sensor << ". Falling back to offline calibration file."); - run_local = true; - } - rt = correction_configuration.LoadFromBinary(received_bytes); - if(rt == Status::OK) - { - RCLCPP_INFO_STREAM(get_logger(), "LoadFromBinary success" << "\n"); - run_local = false; - } - else - { - RCLCPP_ERROR_STREAM(get_logger(), "LoadFromBinary failed" << ". Falling back to offline calibration file."); - run_local = true; - } - }else{ - RCLCPP_ERROR_STREAM(get_logger(), "Falling back to offline calibration file."); - run_local = true; - } - }); - if (!run_local) { - std::future_status status; - status = future.wait_for(std::chrono::milliseconds(8000)); - if (status == std::future_status::timeout) { - std::cerr << "# std::future_status::timeout\n"; - run_local = true; - } else if (status == std::future_status::ready && !run_local) { - RCLCPP_INFO_STREAM( - this->get_logger(), "Acquired correction data from sensor: '" - << sensor_configuration.sensor_ip << "'"); - RCLCPP_INFO_STREAM( - this->get_logger(), "The correction has been saved in '" - << correction_file_path_from_sensor << "'"); - } - } - if(run_local) { - bool run_org = false; - if (correction_file_path_from_sensor.empty()) { - run_org = true; - } else { - auto cal_status = - correction_configuration.LoadFromFile(correction_file_path_from_sensor); - - if (cal_status != Status::OK) { - run_org = true; - }else{ - RCLCPP_INFO_STREAM( - this->get_logger(), "Load correction data from: '" - << correction_file_path_from_sensor << "'"); - } - } - if(run_org) { - if (correction_file_path.empty()) { - RCLCPP_ERROR_STREAM( - this->get_logger(), "Empty Correction File: '" << correction_file_path << "'"); - return Status::INVALID_CALIBRATION_FILE; - } else { - auto cal_status = correction_configuration.LoadFromFile(correction_file_path); - - if (cal_status != Status::OK) { - RCLCPP_ERROR_STREAM( - this->get_logger(), "Given Correction File: '" << correction_file_path << "'"); - return cal_status; - }else{ - RCLCPP_INFO_STREAM( - this->get_logger(), "Load correction data from: '" - << correction_file_path << "'"); - } - } - } - } - } // end AT128 - // Do not use outside of this location - hw_interface_.~HesaiHwInterface(); - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; -} - -RCLCPP_COMPONENTS_REGISTER_NODE(HesaiDriverRosWrapper) -} // namespace ros -} // namespace nebula diff --git a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp deleted file mode 100644 index 1ac59b55b..000000000 --- a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp +++ /dev/null @@ -1,510 +0,0 @@ -#include "nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp" - -namespace nebula -{ -namespace ros -{ -HesaiHwInterfaceRosWrapper::HesaiHwInterfaceRosWrapper(const rclcpp::NodeOptions & options) -: rclcpp::Node("hesai_hw_interface_ros_wrapper", options), hw_interface_() -{ - if (mtx_config_.try_lock()) { - interface_status_ = GetParameters(sensor_configuration_); - mtx_config_.unlock(); - } - if (Status::OK != interface_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); - return; - } - std::this_thread::sleep_for(std::chrono::milliseconds(this->delay_hw_ms_)); - 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)); -#if not defined(TEST_PCAP) - Status rt = hw_interface_.InitializeTcpDriver(); - if(this->retry_hw_) - { - int cnt = 0; - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << " Retry: " << cnt); - while(rt == Status::ERROR_1) - { - cnt++; - std::this_thread::sleep_for(std::chrono::milliseconds(8000));// >5000 - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Retry: " << cnt); - rt = hw_interface_.InitializeTcpDriver(); - } - } - - if(rt != Status::ERROR_1){ - try{ - std::vector thread_pool{}; - thread_pool.emplace_back([this] { - auto result = hw_interface_.GetInventory(); - RCLCPP_INFO_STREAM(get_logger(), result); - hw_interface_.SetTargetModel(result.model); - }); - for (std::thread & th : thread_pool) { - th.join(); - } - - } - catch (...) - { - std::cout << "catch (...) in parent" << std::endl; - RCLCPP_ERROR_STREAM(get_logger(), "Failed to get model from sensor..."); - } - if (this->setup_sensor) { - hw_interface_.CheckAndSetConfig(); - updateParameters(); - } - } - else - { - RCLCPP_ERROR_STREAM(get_logger(), "Failed to get model from sensor... Set from config: " << sensor_cfg_ptr->sensor_model); - hw_interface_.SetTargetModel(sensor_cfg_ptr->sensor_model); - } -#endif - - hw_interface_.RegisterScanCallback( - std::bind(&HesaiHwInterfaceRosWrapper::ReceiveScanDataCallback, this, std::placeholders::_1)); - pandar_scan_pub_ = - this->create_publisher("pandar_packets", rclcpp::SensorDataQoS()); - -#if not defined(TEST_PCAP) - if (this->setup_sensor) { - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&HesaiHwInterfaceRosWrapper::paramCallback, this, std::placeholders::_1)); - } -#endif - -#ifdef WITH_DEBUG_STDOUT_HesaiHwInterfaceRosWrapper - if (false) { - std::vector thread_pool{}; - thread_pool.emplace_back([this] { hw_interface_.SetStandbyMode(0); }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetLidarCalib(ios); - }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetPtpDiagStatus(ios); - }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetPtpDiagPort(ios); - }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetPtpDiagTime(ios); - }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetPtpDiagGrandmaster(ios); - }); - // thread_pool.emplace_back([&hw_interface_]{ - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetInventory(ios); - }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetConfig(ios); - }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetLidarStatus(ios); - }); - thread_pool.emplace_back([this] { hw_interface_.SetStandbyMode(1); }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetPtpConfig(ios); - }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetLidarRange(ios); - }); - for (std::thread & th : thread_pool) { - // hw_interface_.IOServiceRun(); - th.join(); - } - } - if (false) { - std::vector thread_pool{}; - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - std::cout << "GetLidarCalib" << std::endl; - hw_interface_.GetLidarCalib(ios); - }); - for (std::thread & th : thread_pool) { - th.join(); - } - } -#endif - - RCLCPP_DEBUG(this->get_logger(), "Starting stream"); - StreamStart(); -} - -HesaiHwInterfaceRosWrapper::~HesaiHwInterfaceRosWrapper() { - RCLCPP_INFO_STREAM(get_logger(), "Closing TcpDriver"); - hw_interface_.FinalizeTcpDriver(); -} - -Status HesaiHwInterfaceRosWrapper::StreamStart() -{ - if (Status::OK == interface_status_) { - interface_status_ = hw_interface_.SensorInterfaceStart(); - } - return interface_status_; -} - -Status HesaiHwInterfaceRosWrapper::StreamStop() { return Status::OK; } -Status HesaiHwInterfaceRosWrapper::Shutdown() { return Status::OK; } - -Status HesaiHwInterfaceRosWrapper::InitializeHwInterface( // todo: don't think this is needed - const drivers::SensorConfigurationBase & sensor_configuration) -{ - std::stringstream ss; - ss << sensor_configuration; - RCLCPP_DEBUG_STREAM(this->get_logger(), ss.str()); - return Status::OK; -} - -Status HesaiHwInterfaceRosWrapper::GetParameters( - drivers::HesaiSensorConfiguration & 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 = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("return_mode", "", descriptor); - sensor_configuration.return_mode = nebula::drivers::ReturnModeFromStringHesai( - this->get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); - } - { - 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_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "pandar", 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", 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", 2369, 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(); - } - { - 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("packet_mtu_size", 1500, descriptor); - sensor_configuration.packet_mtu_size = this->get_parameter("packet_mtu_size").as_int(); - } - - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - rcl_interfaces::msg::IntegerRange range; - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::HESAI_PANDARAT128) { - descriptor.additional_constraints = "200, 300, 400, 500"; - range.set__from_value(200).set__to_value(500).set__step(100); - descriptor.integer_range = {range}; - this->declare_parameter("rotation_speed", 200, descriptor); - } else { - descriptor.additional_constraints = "300, 600, 1200"; - range.set__from_value(300).set__to_value(1200).set__step(300); - descriptor.integer_range = {range}; - this->declare_parameter("rotation_speed", 600, descriptor); - } - sensor_configuration.rotation_speed = this->get_parameter("rotation_speed").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(360).set__step(1); - descriptor.integer_range = {range}; - this->declare_parameter("cloud_min_angle", 0, descriptor); - sensor_configuration.cloud_min_angle = this->get_parameter("cloud_min_angle").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(360).set__step(1); - descriptor.integer_range = {range}; - this->declare_parameter("cloud_max_angle", 360, descriptor); - sensor_configuration.cloud_max_angle = this->get_parameter("cloud_max_angle").as_int(); - } - { - 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_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("setup_sensor", true, descriptor); - this->setup_sensor = this->get_parameter("setup_sensor").as_bool(); - } - - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "milliseconds"; - this->declare_parameter("delay_hw_ms", 0, descriptor); - this->delay_hw_ms_ = this->get_parameter("delay_hw_ms").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("retry_hw", true, descriptor); - this->retry_hw_ = this->get_parameter("retry_hw").as_bool(); - } - { - 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("ptp_profile", ""); - sensor_configuration.ptp_profile = - nebula::drivers::PtpProfileFromString(this->get_parameter("ptp_profile").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("ptp_transport_type", ""); - sensor_configuration.ptp_transport_type = - nebula::drivers::PtpTransportTypeFromString(this->get_parameter("ptp_transport_type").as_string()); - if(static_cast(sensor_configuration.ptp_profile) > 0) { - sensor_configuration.ptp_transport_type = nebula::drivers::PtpTransportType::L2; - } - } - { - 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("ptp_switch_type", ""); - sensor_configuration.ptp_switch_type = - nebula::drivers::PtpSwitchTypeFromString(this->get_parameter("ptp_switch_type").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(127).set__step(1); - descriptor.integer_range = {range}; - this->declare_parameter("ptp_domain", 0, descriptor); - sensor_configuration.ptp_domain = this->get_parameter("ptp_domain").as_int(); - } - - if(sensor_configuration.ptp_profile == nebula::drivers::PtpProfile::PROFILE_UNKNOWN) { - RCLCPP_ERROR_STREAM(get_logger(), "Invalid PTP Profile Provided. Please use '1588v2', '802.1as' or 'automotive'"); - return Status::SENSOR_CONFIG_ERROR; - } - if(sensor_configuration.ptp_transport_type == nebula::drivers::PtpTransportType::UNKNOWN_TRANSPORT) { - RCLCPP_ERROR_STREAM(get_logger(), - "Invalid PTP Transport Provided. Please use 'udp' or 'l2', 'udp' is only available when using the '1588v2' PTP Profile"); - return Status::SENSOR_CONFIG_ERROR; - } - if(sensor_configuration.ptp_switch_type == nebula::drivers::PtpSwitchType::UNKNOWN_SWITCH) { - RCLCPP_ERROR_STREAM(get_logger(), - "Invalid PTP Switch Type Provided. Please use 'tsn' or 'non_tsn'"); - return Status::SENSOR_CONFIG_ERROR; - } - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - if (sensor_configuration.return_mode == nebula::drivers::ReturnMode::UNKNOWN) { - return Status::INVALID_ECHO_MODE; - } - 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 HesaiHwInterfaceRosWrapper::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; - pandar_scan_pub_->publish(std::move(scan_buffer)); -} - -rcl_interfaces::msg::SetParametersResult HesaiHwInterfaceRosWrapper::paramCallback( - const std::vector & p) -{ - std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback"); - RCLCPP_DEBUG_STREAM(this->get_logger(), p); - RCLCPP_DEBUG_STREAM(this->get_logger(), sensor_configuration_); - RCLCPP_INFO_STREAM(this->get_logger(), p); - - drivers::HesaiSensorConfiguration new_param{sensor_configuration_}; - RCLCPP_INFO_STREAM(this->get_logger(), new_param); - std::string sensor_model_str; - std::string return_mode_str; - if ( - get_param(p, "sensor_model", sensor_model_str) || - get_param(p, "return_mode", return_mode_str) || get_param(p, "host_ip", new_param.host_ip) || - get_param(p, "sensor_ip", new_param.sensor_ip) || - get_param(p, "frame_id", new_param.frame_id) || - get_param(p, "data_port", new_param.data_port) || - get_param(p, "gnss_port", new_param.gnss_port) || - get_param(p, "scan_phase", new_param.scan_phase) || - get_param(p, "packet_mtu_size", new_param.packet_mtu_size) || - get_param(p, "rotation_speed", new_param.rotation_speed) || - get_param(p, "cloud_min_angle", new_param.cloud_min_angle) || - get_param(p, "cloud_max_angle", new_param.cloud_max_angle) || - get_param(p, "dual_return_distance_threshold", new_param.dual_return_distance_threshold)) { - if (0 < sensor_model_str.length()) - new_param.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_str); - if (0 < return_mode_str.length()) - new_param.return_mode = nebula::drivers::ReturnModeFromString(return_mode_str); - - sensor_configuration_ = new_param; - RCLCPP_INFO_STREAM(this->get_logger(), "Update sensor_configuration"); - std::shared_ptr sensor_cfg_ptr = - std::make_shared(sensor_configuration_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - hw_interface_.CheckAndSetConfig(); - } - - auto result = std::make_shared(); - result->successful = true; - result->reason = "success"; - - RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback success"); - - return *result; -} - -std::vector HesaiHwInterfaceRosWrapper::updateParameters() -{ - std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters start"); - std::ostringstream os_sensor_model; - os_sensor_model << sensor_configuration_.sensor_model; - std::ostringstream os_return_mode; - os_return_mode << sensor_configuration_.return_mode; - RCLCPP_INFO_STREAM(this->get_logger(), "set_parameters"); - auto results = set_parameters( - {rclcpp::Parameter("sensor_model", os_sensor_model.str()), - rclcpp::Parameter("return_mode", os_return_mode.str()), - rclcpp::Parameter("host_ip", sensor_configuration_.host_ip), - rclcpp::Parameter("sensor_ip", sensor_configuration_.sensor_ip), - rclcpp::Parameter("frame_id", sensor_configuration_.frame_id), - rclcpp::Parameter("data_port", sensor_configuration_.data_port), - rclcpp::Parameter("gnss_port", sensor_configuration_.gnss_port), - rclcpp::Parameter("scan_phase", sensor_configuration_.scan_phase), - rclcpp::Parameter("packet_mtu_size", sensor_configuration_.packet_mtu_size), - rclcpp::Parameter("rotation_speed", sensor_configuration_.rotation_speed), - rclcpp::Parameter("cloud_min_angle", sensor_configuration_.cloud_min_angle), - rclcpp::Parameter("cloud_max_angle", sensor_configuration_.cloud_max_angle), - rclcpp::Parameter( - "dual_return_distance_threshold", sensor_configuration_.dual_return_distance_threshold)}); - RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters end"); - return results; -} - -RCLCPP_COMPONENTS_REGISTER_NODE(HesaiHwInterfaceRosWrapper) -} // namespace ros -} // namespace nebula diff --git a/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp deleted file mode 100644 index e64bf443b..000000000 --- a/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp +++ /dev/null @@ -1,620 +0,0 @@ -#include "nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp" - -namespace nebula -{ -namespace ros -{ -HesaiHwMonitorRosWrapper::HesaiHwMonitorRosWrapper(const rclcpp::NodeOptions & options) -: rclcpp::Node("hesai_hw_monitor_ros_wrapper", options), hw_interface_(), diagnostics_updater_(this) -{ - cbg_r_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant); - cbg_m_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - cbg_m2_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - if (mtx_config_.try_lock()) { - interface_status_ = GetParameters(sensor_configuration_); - mtx_config_.unlock(); - } - if (Status::OK != interface_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); - return; - } - std::this_thread::sleep_for(std::chrono::milliseconds(this->delay_monitor_ms_)); - std::shared_ptr sensor_cfg_ptr = - std::make_shared(sensor_configuration_); - - message_sep = ": "; - not_supported_message = "Not supported"; - error_message = "Error"; - - switch (sensor_cfg_ptr->sensor_model) { - case nebula::drivers::SensorModel::HESAI_PANDARXT32: - case nebula::drivers::SensorModel::HESAI_PANDARXT32M: - case nebula::drivers::SensorModel::HESAI_PANDARAT128: - temperature_names.emplace_back("Bottom circuit board T1"); - temperature_names.emplace_back("Bottom circuit board T2"); - temperature_names.emplace_back("Laser emitting board RT_L1(Internal)"); - temperature_names.emplace_back("Laser emitting board RT_L2"); - temperature_names.emplace_back("Receiving board RT_R"); - temperature_names.emplace_back("Receiving board RT2"); - temperature_names.emplace_back("Top circuit RT3"); - temperature_names.emplace_back("Not used"); - break; - case nebula::drivers::SensorModel::HESAI_PANDAR64: - case nebula::drivers::SensorModel::HESAI_PANDAR40P: - case nebula::drivers::SensorModel::HESAI_PANDAR40M: - case nebula::drivers::SensorModel::HESAI_PANDARQT64: - case nebula::drivers::SensorModel::HESAI_PANDARQT128: - case nebula::drivers::SensorModel::HESAI_PANDAR128_E3X: - case nebula::drivers::SensorModel::HESAI_PANDAR128_E4X: - default: - temperature_names.emplace_back("Bottom circuit RT1"); - temperature_names.emplace_back("Bottom circuit RT2"); - temperature_names.emplace_back("Internal Temperature"); - temperature_names.emplace_back("Laser emitting board RT1"); - temperature_names.emplace_back("Laser emitting board RT2"); - temperature_names.emplace_back("Receiving board RT1"); - temperature_names.emplace_back("Top circuit RT1"); - temperature_names.emplace_back("Top circuit RT2"); - break; - } - - hw_interface_.SetLogger(std::make_shared(this->get_logger())); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - while(hw_interface_.InitializeTcpDriver() == Status::ERROR_1) - { - RCLCPP_WARN(this->get_logger(), "Could not initialize TCP driver, retrying in 8s..."); - std::this_thread::sleep_for(std::chrono::milliseconds(8000));// >5000 - } - std::vector thread_pool{}; - thread_pool.emplace_back([this] { - auto result = hw_interface_.GetInventory(); - current_inventory.reset(new HesaiInventory(result)); - current_inventory_time.reset(new rclcpp::Time(this->get_clock()->now())); - RCLCPP_INFO_STREAM(this->get_logger(), "HesaiInventory"); - RCLCPP_INFO_STREAM(this->get_logger(), result); - info_model = result.get_str_model(); - info_serial = std::string(std::begin(result.sn), std::end(result.sn)); - hw_interface_.SetTargetModel(result.model); - RCLCPP_INFO_STREAM(this->get_logger(), "Model:" << info_model); - RCLCPP_INFO_STREAM(this->get_logger(), "Serial:" << info_serial); - InitializeHesaiDiagnostics(); - }); - for (std::thread & th : thread_pool) { - th.join(); - } - - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&HesaiHwMonitorRosWrapper::paramCallback, this, std::placeholders::_1)); -} - -Status HesaiHwMonitorRosWrapper::MonitorStart() { return interface_status_; } - -Status HesaiHwMonitorRosWrapper::MonitorStop() { return Status::OK; } -Status HesaiHwMonitorRosWrapper::Shutdown() { return Status::OK; } - -Status HesaiHwMonitorRosWrapper::InitializeHwMonitor( // todo: don't think this is needed - const drivers::SensorConfigurationBase & sensor_configuration) -{ - std::stringstream ss; - ss << sensor_configuration; - RCLCPP_DEBUG_STREAM(this->get_logger(), ss.str()); - return Status::OK; -} - -Status HesaiHwMonitorRosWrapper::GetParameters( - drivers::HesaiSensorConfiguration & 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 = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("return_mode", "", descriptor); - sensor_configuration.return_mode = nebula::drivers::ReturnModeFromStringHesai( - this->get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); - } - { - 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_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "pandar", 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", 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", 2369, 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(); - } - { - 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("packet_mtu_size", 1500, descriptor); - sensor_configuration.packet_mtu_size = this->get_parameter("packet_mtu_size").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - rcl_interfaces::msg::IntegerRange range; - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::HESAI_PANDARAT128) { - descriptor.additional_constraints = "200, 300, 400, 500"; - range.set__from_value(200).set__to_value(500).set__step(100); - descriptor.integer_range = {range}; - this->declare_parameter("rotation_speed", 200, descriptor); - } else { - descriptor.additional_constraints = "300, 600, 1200"; - range.set__from_value(300).set__to_value(1200).set__step(300); - descriptor.integer_range = {range}; - this->declare_parameter("rotation_speed", 600, descriptor); - } - sensor_configuration.rotation_speed = this->get_parameter("rotation_speed").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(360).set__step(1); - descriptor.integer_range = {range}; - this->declare_parameter("cloud_min_angle", 0, descriptor); - sensor_configuration.cloud_min_angle = this->get_parameter("cloud_min_angle").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(360).set__step(1); - descriptor.integer_range = {range}; - this->declare_parameter("cloud_max_angle", 360, descriptor); - sensor_configuration.cloud_max_angle = this->get_parameter("cloud_max_angle").as_int(); - } - { - 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(); - } - - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - if (sensor_configuration.return_mode == nebula::drivers::ReturnMode::UNKNOWN) { - return Status::INVALID_ECHO_MODE; - } - if (sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { // || - return Status::SENSOR_CONFIG_ERROR; - } - - { - 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); - this->diag_span_ = this->get_parameter("diag_span").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 = "milliseconds"; - this->declare_parameter("delay_monitor_ms", 0, descriptor); - this->delay_monitor_ms_ = this->get_parameter("delay_monitor_ms").as_int(); - } - - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; -} - -void HesaiHwMonitorRosWrapper::InitializeHesaiDiagnostics() -{ - RCLCPP_INFO_STREAM(this->get_logger(), "InitializeHesaiDiagnostics"); - using std::chrono_literals::operator""s; - std::ostringstream os; - auto hardware_id = info_model + ": " + info_serial; - diagnostics_updater_.setHardwareID(hardware_id); - RCLCPP_INFO_STREAM(this->get_logger(), "hardware_id: " + hardware_id); - - diagnostics_updater_.add("hesai_status", this, &HesaiHwMonitorRosWrapper::HesaiCheckStatus); - diagnostics_updater_.add("hesai_ptp", this, &HesaiHwMonitorRosWrapper::HesaiCheckPtp); - diagnostics_updater_.add( - "hesai_temperature", this, &HesaiHwMonitorRosWrapper::HesaiCheckTemperature); - diagnostics_updater_.add("hesai_rpm", this, &HesaiHwMonitorRosWrapper::HesaiCheckRpm); - - current_status.reset(); - current_monitor.reset(); - current_status_time.reset(new rclcpp::Time(this->get_clock()->now())); - current_lidar_monitor_time.reset(new rclcpp::Time(this->get_clock()->now())); - current_diag_status = diagnostic_msgs::msg::DiagnosticStatus::STALE; - current_monitor_status = diagnostic_msgs::msg::DiagnosticStatus::STALE; - - auto fetch_diag_from_sensor = [this](){ - OnHesaiStatusTimer(); - if (hw_interface_.UseHttpGetLidarMonitor()) { - OnHesaiLidarMonitorTimerHttp(); - } else { - OnHesaiLidarMonitorTimer(); - } - }; - - fetch_diagnostics_timer_ = std::make_shared>( - this->get_clock(), std::chrono::milliseconds(diag_span_), std::move(fetch_diag_from_sensor), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(fetch_diagnostics_timer_, cbg_m_); - - if (hw_interface_.UseHttpGetLidarMonitor()) { - diagnostics_updater_.add( - "hesai_voltage", this, &HesaiHwMonitorRosWrapper::HesaiCheckVoltageHttp); - } else { - diagnostics_updater_.add("hesai_voltage", this, &HesaiHwMonitorRosWrapper::HesaiCheckVoltage); - } - - auto on_timer_update = [this] { - RCLCPP_DEBUG_STREAM(get_logger(), "OnUpdateTimer"); - auto now = this->get_clock()->now(); - auto dif = (now - *current_status_time).seconds(); - - RCLCPP_DEBUG_STREAM(get_logger(), "dif(status): " << dif); - - if (diag_span_ * 2.0 < dif * 1000) { - current_diag_status = diagnostic_msgs::msg::DiagnosticStatus::STALE; - RCLCPP_DEBUG_STREAM(get_logger(), "STALE"); - } else { - current_diag_status = diagnostic_msgs::msg::DiagnosticStatus::OK; - RCLCPP_DEBUG_STREAM(get_logger(), "OK"); - } - dif = (now - *current_lidar_monitor_time).seconds(); - RCLCPP_DEBUG_STREAM(get_logger(), "dif(monitor): " << dif); - if (diag_span_ * 2.0 < dif * 1000) { - current_monitor_status = diagnostic_msgs::msg::DiagnosticStatus::STALE; - RCLCPP_DEBUG_STREAM(get_logger(), "STALE"); - } else { - current_monitor_status = diagnostic_msgs::msg::DiagnosticStatus::OK; - RCLCPP_DEBUG_STREAM(get_logger(), "OK"); - } - diagnostics_updater_.force_update(); - }; - diagnostics_update_timer_ = std::make_shared>( - this->get_clock(), std::chrono::milliseconds(1000), std::move(on_timer_update), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(diagnostics_update_timer_, cbg_r_); - - RCLCPP_DEBUG_STREAM(get_logger(), "add_timer"); -} - -std::string HesaiHwMonitorRosWrapper::GetPtreeValue( - boost::property_tree::ptree * pt, const std::string & key) -{ - boost::optional value = pt->get_optional(key); - if (value) { - return value.get(); - } else { - return not_supported_message; - } -} -std::string HesaiHwMonitorRosWrapper::GetFixedPrecisionString(double val, int pre) -{ - std::stringstream ss; - ss << std::fixed << std::setprecision(pre) << val; - return ss.str(); -} - -rcl_interfaces::msg::SetParametersResult HesaiHwMonitorRosWrapper::paramCallback( - const std::vector & p) -{ - std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(get_logger(), "add_on_set_parameters_callback"); - RCLCPP_DEBUG_STREAM(get_logger(), p); - RCLCPP_DEBUG_STREAM(get_logger(), sensor_configuration_); - RCLCPP_INFO_STREAM(this->get_logger(), p); - - drivers::HesaiSensorConfiguration new_param{sensor_configuration_}; - RCLCPP_INFO_STREAM(this->get_logger(), new_param); - uint16_t new_diag_span = 0; - if (get_param(p, "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 HesaiHwMonitorRosWrapper::OnHesaiStatusTimer() -{ - RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiStatusTimer" << std::endl); - try { - auto ios = std::make_shared(); - auto result = hw_interface_.GetLidarStatus(); - std::scoped_lock lock(mtx_status); - current_status_time.reset(new rclcpp::Time(this->get_clock()->now())); - current_status.reset(new HesaiLidarStatus(result)); - } catch (const std::system_error & error) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("HesaiHwMonitorRosWrapper::OnHesaiStatusTimer(std::system_error)"), - error.what()); - } catch (const boost::system::system_error & error) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger( - "HesaiHwMonitorRosWrapper::OnHesaiStatusTimer(boost::system::system_error)"), - error.what()); - } - RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiStatusTimer END" << std::endl); -} - -void HesaiHwMonitorRosWrapper::OnHesaiLidarMonitorTimerHttp() -{ - RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiLidarMonitorTimerHttp"); - try { - hw_interface_.GetLidarMonitorAsyncHttp([this](const std::string & str) { - std::scoped_lock lock(mtx_lidar_monitor); - current_lidar_monitor_time.reset(new rclcpp::Time(this->get_clock()->now())); - current_lidar_monitor_tree = - std::make_unique(hw_interface_.ParseJson(str)); - }); - } catch (const std::system_error & error) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger( - "HesaiHwMonitorRosWrapper::OnHesaiLidarMonitorTimerHttp(std::system_error)"), - error.what()); - } catch (const boost::system::system_error & error) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger( - "HesaiHwMonitorRosWrapper::OnHesaiLidarMonitorTimerHttp(boost::system::system_error)"), - error.what()); - } - - RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiLidarMonitorTimerHttp END"); -} - -void HesaiHwMonitorRosWrapper::OnHesaiLidarMonitorTimer() -{ - RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiLidarMonitorTimer"); - try { - auto ios = std::make_shared(); - auto result = hw_interface_.GetLidarMonitor(); - std::scoped_lock lock(mtx_lidar_monitor); - current_lidar_monitor_time.reset(new rclcpp::Time(this->get_clock()->now())); - current_monitor.reset(new HesaiLidarMonitor(result)); - } catch (const std::system_error & error) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("HesaiHwMonitorRosWrapper::OnHesaiLidarMonitorTimer(std::system_error)"), - error.what()); - } catch (const boost::system::system_error & error) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger( - "HesaiHwMonitorRosWrapper::OnHesaiLidarMonitorTimer(boost::system::system_error)"), - error.what()); - } - - RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiLidarMonitorTimer END"); -} - -void HesaiHwMonitorRosWrapper::HesaiCheckStatus( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - std::scoped_lock lock(mtx_status); - if (current_status) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::vector msg; - - diagnostics.add("system_uptime", std::to_string(current_status->system_uptime.value())); - diagnostics.add("startup_times", std::to_string(current_status->startup_times.value())); - diagnostics.add("total_operation_time", std::to_string(current_status->total_operation_time.value())); - - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } else { - diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); - } -} - -void HesaiHwMonitorRosWrapper::HesaiCheckPtp( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - std::scoped_lock lock(mtx_status); - if (current_status) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::vector msg; - auto gps_status = current_status->get_str_gps_pps_lock(); - auto gprmc_status = current_status->get_str_gps_gprmc_status(); - auto ptp_status = current_status->get_str_ptp_clock_status(); - std::transform(gps_status.cbegin(), gps_status.cend(), gps_status.begin(), toupper); - std::transform(gprmc_status.cbegin(), gprmc_status.cend(), gprmc_status.begin(), toupper); - std::transform(ptp_status.cbegin(), ptp_status.cend(), ptp_status.begin(), toupper); - diagnostics.add("gps_pps_lock", gps_status); - diagnostics.add("gps_gprmc_status", gprmc_status); - diagnostics.add("ptp_clock_status", ptp_status); - if (gps_status != "UNKNOWN") { - msg.emplace_back("gps_pps_lock: " + gps_status); - } - if (gprmc_status != "UNKNOWN") { - msg.emplace_back("gprmc_status: " + gprmc_status); - } - if (ptp_status != "UNKNOWN") { - msg.emplace_back("ptp_status: " + ptp_status); - } - if (ptp_status == "FREE RUN" && gps_status == "UNKNOWN") { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - } - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } else { - diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); - } -} - -void HesaiHwMonitorRosWrapper::HesaiCheckTemperature( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - std::scoped_lock lock(mtx_status); - if (current_status) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::vector msg; - for (size_t i = 0; i < std::size(current_status->temperature); i++) { - diagnostics.add( - temperature_names[i], GetFixedPrecisionString(current_status->temperature[i].value() * 0.01)); - } - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } else { - diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); - } -} - -void HesaiHwMonitorRosWrapper::HesaiCheckRpm( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - std::scoped_lock lock(mtx_status); - if (current_status) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::vector msg; - diagnostics.add("motor_speed", std::to_string(current_status->motor_speed.value())); - - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } else { - diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); - } -} - -void HesaiHwMonitorRosWrapper::HesaiCheckVoltageHttp( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - std::scoped_lock lock(mtx_lidar_monitor); - if (current_lidar_monitor_tree) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::vector msg; - std::string key = ""; - - std::string mes; - key = "lidarInCur"; - try { - mes = GetPtreeValue(current_lidar_monitor_tree.get(), "Body." + key); - } catch (boost::bad_lexical_cast & ex) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message + std::string(ex.what()); - } - diagnostics.add(key, mes); - key = "lidarInVol"; - try { - mes = GetPtreeValue(current_lidar_monitor_tree.get(), "Body." + key); - } catch (boost::bad_lexical_cast & ex) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message + std::string(ex.what()); - } - diagnostics.add(key, mes); - - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } else { - diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); - } -} - -void HesaiHwMonitorRosWrapper::HesaiCheckVoltage( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - std::scoped_lock lock(mtx_lidar_monitor); - if (current_monitor) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::vector msg; - diagnostics.add( - "input_voltage", GetFixedPrecisionString(current_monitor->input_voltage.value() * 0.01) + " V"); - diagnostics.add( - "input_current", GetFixedPrecisionString(current_monitor->input_current.value() * 0.01) + " mA"); - diagnostics.add( - "input_power", GetFixedPrecisionString(current_monitor->input_power.value() * 0.01) + " W"); - - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } else { - diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); - } -} - - HesaiHwMonitorRosWrapper::~HesaiHwMonitorRosWrapper() { - RCLCPP_INFO_STREAM(get_logger(), "Closing TcpDriver"); - hw_interface_.FinalizeTcpDriver(); - } - - RCLCPP_COMPONENTS_REGISTER_NODE(HesaiHwMonitorRosWrapper) -} // namespace ros -} // namespace nebula diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp new file mode 100644 index 000000000..4fc2a12df --- /dev/null +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -0,0 +1,1234 @@ +#include "nebula_ros/hesai/hesai_ros_wrapper.hpp" + +namespace nebula +{ +namespace ros +{ +HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) +: rclcpp::Node("hesai_ros_wrapper", options), hw_interface_(), diagnostics_updater_(this) +{ + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + + drivers::HesaiSensorConfiguration sensor_configuration; + wrapper_status_ = GetParameters(sensor_configuration); + sensor_cfg_ptr_ = std::make_shared(sensor_configuration); + + // hwiface +{ + if (Status::OK != interface_status_) { + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); + return; + } + std::this_thread::sleep_for(std::chrono::milliseconds(this->delay_hw_ms_)); + hw_interface_.SetLogger(std::make_shared(this->get_logger())); + hw_interface_.SetSensorConfiguration( + std::static_pointer_cast(sensor_cfg_ptr_)); +#if not defined(TEST_PCAP) + Status rt = hw_interface_.InitializeTcpDriver(); + if(this->retry_hw_) + { + int cnt = 0; + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << " Retry: " << cnt); + while(rt == Status::ERROR_1) + { + cnt++; + std::this_thread::sleep_for(std::chrono::milliseconds(8000));// >5000 + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Retry: " << cnt); + rt = hw_interface_.InitializeTcpDriver(); + } + } + + if(rt != Status::ERROR_1){ + try{ + std::vector thread_pool{}; + thread_pool.emplace_back([this] { + auto result = hw_interface_.GetInventory(); + RCLCPP_INFO_STREAM(get_logger(), result); + hw_interface_.SetTargetModel(result.model); + }); + for (std::thread & th : thread_pool) { + th.join(); + } + + } + catch (...) + { + std::cout << "catch (...) in parent" << std::endl; + RCLCPP_ERROR_STREAM(get_logger(), "Failed to get model from sensor..."); + } + if (this->setup_sensor) { + hw_interface_.CheckAndSetConfig(); + updateParameters(); + } + } + else + { + RCLCPP_ERROR_STREAM(get_logger(), "Failed to get model from sensor... Set from config: " << sensor_cfg_ptr_->sensor_model); + hw_interface_.SetTargetModel(sensor_cfg_ptr_->sensor_model); + } +#endif + + hw_interface_.RegisterScanCallback( + std::bind(&HesaiRosWrapper::ReceiveScanDataCallback, this, std::placeholders::_1)); + pandar_scan_pub_ = + this->create_publisher("pandar_packets", rclcpp::SensorDataQoS()); + +#if not defined(TEST_PCAP) + if (this->setup_sensor) { + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&HesaiRosWrapper::paramCallback, this, std::placeholders::_1)); + } +#endif + + RCLCPP_DEBUG(this->get_logger(), "Starting stream"); + StreamStart(); +} + // decoder + { + drivers::HesaiCalibrationConfiguration calibration_configuration; + drivers::HesaiCorrection correction_configuration; + + wrapper_status_ = + GetCalibrationData(calibration_configuration, correction_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); + + + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); + if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { + correction_cfg_ptr_ = std::make_shared(correction_configuration); + wrapper_status_ = InitializeCloudDriver( + std::static_pointer_cast(sensor_cfg_ptr_), + std::static_pointer_cast(calibration_cfg_ptr_), + std::static_pointer_cast(correction_cfg_ptr_)); + } else { + wrapper_status_ = InitializeCloudDriver( + std::static_pointer_cast(sensor_cfg_ptr_), + std::static_pointer_cast(calibration_cfg_ptr_)); + } + + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Wrapper=" << wrapper_status_); + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), + qos_profile); + pandar_scan_sub_ = create_subscription( + "pandar_packets", qos, + std::bind(&HesaiRosWrapper::ReceiveScanMsgCallback, this, std::placeholders::_1)); + nebula_points_pub_ = + this->create_publisher("pandar_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()); +} + +//hwmon +{ +cbg_r_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant); + cbg_m_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + cbg_m2_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + if (Status::OK != interface_status_) { + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); + return; + } + std::this_thread::sleep_for(std::chrono::milliseconds(this->delay_monitor_ms_)); + + message_sep = ": "; + not_supported_message = "Not supported"; + error_message = "Error"; + + switch (sensor_cfg_ptr_->sensor_model) { + case nebula::drivers::SensorModel::HESAI_PANDARXT32: + case nebula::drivers::SensorModel::HESAI_PANDARXT32M: + case nebula::drivers::SensorModel::HESAI_PANDARAT128: + temperature_names.emplace_back("Bottom circuit board T1"); + temperature_names.emplace_back("Bottom circuit board T2"); + temperature_names.emplace_back("Laser emitting board RT_L1(Internal)"); + temperature_names.emplace_back("Laser emitting board RT_L2"); + temperature_names.emplace_back("Receiving board RT_R"); + temperature_names.emplace_back("Receiving board RT2"); + temperature_names.emplace_back("Top circuit RT3"); + temperature_names.emplace_back("Not used"); + break; + case nebula::drivers::SensorModel::HESAI_PANDAR64: + case nebula::drivers::SensorModel::HESAI_PANDAR40P: + case nebula::drivers::SensorModel::HESAI_PANDAR40M: + case nebula::drivers::SensorModel::HESAI_PANDARQT64: + case nebula::drivers::SensorModel::HESAI_PANDARQT128: + case nebula::drivers::SensorModel::HESAI_PANDAR128_E3X: + case nebula::drivers::SensorModel::HESAI_PANDAR128_E4X: + default: + temperature_names.emplace_back("Bottom circuit RT1"); + temperature_names.emplace_back("Bottom circuit RT2"); + temperature_names.emplace_back("Internal Temperature"); + temperature_names.emplace_back("Laser emitting board RT1"); + temperature_names.emplace_back("Laser emitting board RT2"); + temperature_names.emplace_back("Receiving board RT1"); + temperature_names.emplace_back("Top circuit RT1"); + temperature_names.emplace_back("Top circuit RT2"); + break; + } + + std::vector thread_pool{}; + thread_pool.emplace_back([this] { + auto result = hw_interface_.GetInventory(); + current_inventory.reset(new HesaiInventory(result)); + current_inventory_time.reset(new rclcpp::Time(this->get_clock()->now())); + RCLCPP_INFO_STREAM(this->get_logger(), "HesaiInventory"); + RCLCPP_INFO_STREAM(this->get_logger(), result); + info_model = result.get_str_model(); + info_serial = std::string(result.sn.begin(), result.sn.end()); + hw_interface_.SetTargetModel(result.model); + RCLCPP_INFO_STREAM(this->get_logger(), "Model:" << info_model); + RCLCPP_INFO_STREAM(this->get_logger(), "Serial:" << info_serial); + InitializeHesaiDiagnostics(); + }); + for (std::thread & th : thread_pool) { + th.join(); + } + + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&HesaiRosWrapper::paramCallback, this, std::placeholders::_1)); +} +} + +void HesaiRosWrapper::ReceiveScanMsgCallback( + const pandar_msgs::msg::PandarScan::SharedPtr scan_msg) +{ + 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 HesaiRosWrapper::PublishCloud( + std::unique_ptr pointcloud, + const rclcpp::Publisher::SharedPtr & publisher) +{ + if (pointcloud->header.stamp.sec < 0) { + RCLCPP_WARN_STREAM(this->get_logger(), "Timestamp error, verify clock source."); + } + pointcloud->header.frame_id = sensor_cfg_ptr_->frame_id; + publisher->publish(std::move(pointcloud)); +} + +Status HesaiRosWrapper::InitializeCloudDriver( + std::shared_ptr sensor_configuration, + std::shared_ptr calibration_configuration) +{ + driver_ptr_ = std::make_shared( + std::static_pointer_cast(sensor_configuration), + std::static_pointer_cast(calibration_configuration)); + return driver_ptr_->GetStatus(); +} + +Status HesaiRosWrapper::InitializeCloudDriver( + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & calibration_configuration, + const std::shared_ptr & correction_configuration) +{ + // driver should be initialized here with proper decoder + driver_ptr_ = std::make_shared( + std::static_pointer_cast(sensor_configuration), + std::static_pointer_cast(calibration_configuration), + std::static_pointer_cast(correction_configuration)); + return driver_ptr_->GetStatus(); +} + +Status HesaiRosWrapper::GetStatus() { return wrapper_status_; } + +/// decoder +Status HesaiRosWrapper::GetParameters( + drivers::HesaiSensorConfiguration & sensor_configuration) +{ + /////////////////////////////////////////////// + // Define and get ROS parameters + /////////////////////////////////////////////// + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("sensor_model", ""); + sensor_configuration.sensor_model = + nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("return_mode", "", descriptor); + sensor_configuration.return_mode = nebula::drivers::ReturnModeFromStringHesai( + this->get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); + } + { + 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", 2369, 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 = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("frame_id", "pandar", 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(); + } + { + 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("calibration_file", "", descriptor); + calibration_file_path = + this->get_parameter("calibration_file").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 = ""; + this->declare_parameter("min_range", 0.3, descriptor); + sensor_configuration.min_range = this->get_parameter("min_range").as_double(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("max_range", 300., descriptor); + sensor_configuration.max_range = this->get_parameter("max_range").as_double(); + } + { + 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("packet_mtu_size", 1500, descriptor); + sensor_configuration.packet_mtu_size = this->get_parameter("packet_mtu_size").as_int(); + } + + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + rcl_interfaces::msg::IntegerRange range; + RCLCPP_DEBUG_STREAM(get_logger(), sensor_configuration.sensor_model); + if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::HESAI_PANDARAT128) { + descriptor.additional_constraints = "200, 300, 400, 500"; + // range.set__from_value(200).set__to_value(500).set__step(100); + // descriptor.integer_range = {range}; //todo + this->declare_parameter("rotation_speed", 200, descriptor); + } else { + descriptor.additional_constraints = "300, 600, 1200"; + // range.set__from_value(300).set__to_value(1200).set__step(300); + // descriptor.integer_range = {range}; //todo + this->declare_parameter("rotation_speed", 600, descriptor); + } + sensor_configuration.rotation_speed = this->get_parameter("rotation_speed").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + rcl_interfaces::msg::IntegerRange range; + range.set__from_value(0).set__to_value(360).set__step(1); + descriptor.integer_range = {range}; + this->declare_parameter("cloud_min_angle", 0, descriptor); + sensor_configuration.cloud_min_angle = this->get_parameter("cloud_min_angle").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + rcl_interfaces::msg::IntegerRange range; + range.set__from_value(0).set__to_value(360).set__step(1); + descriptor.integer_range = {range}; + this->declare_parameter("cloud_max_angle", 360, descriptor); + sensor_configuration.cloud_max_angle = this->get_parameter("cloud_max_angle").as_int(); + } + if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { + 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("correction_file", "", descriptor); + correction_file_path = this->get_parameter("correction_file").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 = "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_BOOL; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("launch_hw", "", descriptor); + launch_hw_ = this->get_parameter("launch_hw").as_bool(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("setup_sensor", true, descriptor); + this->setup_sensor = this->get_parameter("setup_sensor").as_bool(); + } + + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = "milliseconds"; + this->declare_parameter("delay_hw_ms", 0, descriptor); + this->delay_hw_ms_ = this->get_parameter("delay_hw_ms").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("retry_hw", true, descriptor); + this->retry_hw_ = this->get_parameter("retry_hw").as_bool(); + } + { + 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("ptp_profile", ""); + sensor_configuration.ptp_profile = + nebula::drivers::PtpProfileFromString(this->get_parameter("ptp_profile").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("ptp_transport_type", ""); + sensor_configuration.ptp_transport_type = + nebula::drivers::PtpTransportTypeFromString(this->get_parameter("ptp_transport_type").as_string()); + if(static_cast(sensor_configuration.ptp_profile) > 0) { + sensor_configuration.ptp_transport_type = nebula::drivers::PtpTransportType::L2; + } + } + { + 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("ptp_switch_type", ""); + sensor_configuration.ptp_switch_type = + nebula::drivers::PtpSwitchTypeFromString(this->get_parameter("ptp_switch_type").as_string()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + rcl_interfaces::msg::IntegerRange range; + range.set__from_value(0).set__to_value(127).set__step(1); + descriptor.integer_range = {range}; + this->declare_parameter("ptp_domain", 0, descriptor); + sensor_configuration.ptp_domain = this->get_parameter("ptp_domain").as_int(); + } + { + 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); + this->diag_span_ = this->get_parameter("diag_span").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 = "milliseconds"; + this->declare_parameter("delay_monitor_ms", 0, descriptor); + this->delay_monitor_ms_ = this->get_parameter("delay_monitor_ms").as_int(); + } + + /////////////////////////////////////////////// + // Validate ROS parameters + /////////////////////////////////////////////// + + if(sensor_configuration.ptp_profile == nebula::drivers::PtpProfile::PROFILE_UNKNOWN) { + RCLCPP_ERROR_STREAM(get_logger(), "Invalid PTP Profile Provided. Please use '1588v2', '802.1as' or 'automotive'"); + return Status::SENSOR_CONFIG_ERROR; + } + if(sensor_configuration.ptp_transport_type == nebula::drivers::PtpTransportType::UNKNOWN_TRANSPORT) { + RCLCPP_ERROR_STREAM(get_logger(), + "Invalid PTP Transport Provided. Please use 'udp' or 'l2', 'udp' is only available when using the '1588v2' PTP Profile"); + return Status::SENSOR_CONFIG_ERROR; + } + if(sensor_configuration.ptp_switch_type == nebula::drivers::PtpSwitchType::UNKNOWN_SWITCH) { + RCLCPP_ERROR_STREAM(get_logger(), + "Invalid PTP Switch Type Provided. Please use 'tsn' or 'non_tsn'"); + return Status::SENSOR_CONFIG_ERROR; + } + if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + if (sensor_configuration.return_mode == nebula::drivers::ReturnMode::UNKNOWN) { + return Status::INVALID_ECHO_MODE; + } + if (sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { + return Status::SENSOR_CONFIG_ERROR; + } + + /////////////////////////////////////////////// + // Decoder-specific setup + /////////////////////////////////////////////// + + std::shared_ptr sensor_cfg_ptr = + std::make_shared(sensor_configuration); + + hw_interface_.SetSensorConfiguration( + std::static_pointer_cast(sensor_cfg_ptr)); + + /////////////////////////////////////////////// + // HW Interface specific setup + /////////////////////////////////////////////// + + /////////////////////////////////////////////// + // HW Monitor specific setup + /////////////////////////////////////////////// + + RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); + return Status::OK; +} + +Status HesaiRosWrapper::GetCalibrationData( + drivers::HesaiCalibrationConfiguration & calibration_configuration, + drivers::HesaiCorrection & correction_configuration) { + calibration_configuration.calibration_file = calibration_file_path; //todo + + bool run_local = !launch_hw_; + if (sensor_cfg_ptr_->sensor_model != drivers::SensorModel::HESAI_PANDARAT128) { + std::string calibration_file_path_from_sensor; + if (launch_hw_ && !calibration_configuration.calibration_file.empty()) { + int ext_pos = calibration_configuration.calibration_file.find_last_of('.'); + calibration_file_path_from_sensor += calibration_configuration.calibration_file.substr(0, ext_pos); + calibration_file_path_from_sensor += "_from_sensor"; + calibration_file_path_from_sensor += calibration_configuration.calibration_file.substr(ext_pos, calibration_configuration.calibration_file.size() - ext_pos); + } + if(launch_hw_) { + run_local = false; + RCLCPP_INFO_STREAM( + this->get_logger(), "Trying to acquire calibration data from sensor: '" + << sensor_cfg_ptr_->sensor_ip << "'"); + std::future future = std::async(std::launch::async, + [this, &calibration_configuration, &calibration_file_path_from_sensor, &run_local]() { + auto str = hw_interface_.GetLidarCalibrationString(); + auto rt = calibration_configuration.SaveFileFromString( + calibration_file_path_from_sensor, str); + RCLCPP_ERROR_STREAM(get_logger(), str); + if (rt == Status::OK) { + RCLCPP_INFO_STREAM(get_logger(), "SaveFileFromString success:" + << calibration_file_path_from_sensor << "\n"); + } else { + RCLCPP_ERROR_STREAM(get_logger(), "SaveFileFromString failed:" + << calibration_file_path_from_sensor << "\n"); + } + rt = calibration_configuration.LoadFromString(str); + if (rt == Status::OK) { + RCLCPP_INFO_STREAM(get_logger(), + "LoadFromString success:" << str << "\n"); + } else { + RCLCPP_ERROR_STREAM(get_logger(), + "LoadFromString failed:" << str << "\n"); + } + }); + std::future_status status; + status = future.wait_for(std::chrono::milliseconds(5000)); + if (status == std::future_status::timeout) { + std::cerr << "# std::future_status::timeout\n"; + RCLCPP_ERROR_STREAM(get_logger(), "GetCalibration Timeout"); + run_local = true; + } else if (status == std::future_status::ready && !run_local) { + RCLCPP_INFO_STREAM( + this->get_logger(), "Acquired calibration data from sensor: '" + << sensor_cfg_ptr_->sensor_ip << "'"); + RCLCPP_INFO_STREAM( + this->get_logger(), "The calibration has been saved in '" + << calibration_file_path_from_sensor << "'"); + } + } + if(run_local) { + RCLCPP_WARN_STREAM(get_logger(), "Running locally"); + bool run_org = false; + if (calibration_file_path_from_sensor.empty()) { + run_org = true; + } else { + RCLCPP_INFO_STREAM(get_logger(),"Trying to load file: " << calibration_file_path_from_sensor); + auto cal_status = + calibration_configuration.LoadFromFile(calibration_file_path_from_sensor); + + if (cal_status != Status::OK) { + run_org = true; + }else{ + RCLCPP_INFO_STREAM( + this->get_logger(), "Load calibration data from: '" + << calibration_file_path_from_sensor << "'"); + } + } + if(run_org) { + RCLCPP_INFO_STREAM(get_logger(),"Trying to load file: " << calibration_configuration.calibration_file); + if (calibration_configuration.calibration_file.empty()) { + RCLCPP_ERROR_STREAM( + this->get_logger(), "Empty Calibration_file File: '" << calibration_configuration.calibration_file << "'"); + return Status::INVALID_CALIBRATION_FILE; + } else { + auto cal_status = + calibration_configuration.LoadFromFile(calibration_configuration.calibration_file); + + if (cal_status != Status::OK) { + RCLCPP_ERROR_STREAM( + this->get_logger(), + "Given Calibration File: '" << calibration_configuration.calibration_file << "'"); + return cal_status; + }else{ + RCLCPP_INFO_STREAM( + this->get_logger(), "Load calibration data from: '" + << calibration_configuration.calibration_file << "'"); + } + } + } + } + } else { // sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128 + std::string correction_file_path_from_sensor; + if (launch_hw_ && !correction_file_path.empty()) { + int ext_pos = correction_file_path.find_last_of('.'); + correction_file_path_from_sensor += correction_file_path.substr(0, ext_pos); + correction_file_path_from_sensor += "_from_sensor"; + correction_file_path_from_sensor += correction_file_path.substr(ext_pos, correction_file_path.size() - ext_pos); + } + std::future future = std::async(std::launch::async, [this, &correction_configuration, &correction_file_path_from_sensor, &run_local]() { + if (launch_hw_) { + RCLCPP_INFO_STREAM( + this->get_logger(), "Trying to acquire calibration data from sensor"); + auto received_bytes = hw_interface_.GetLidarCalibrationBytes(); + RCLCPP_INFO_STREAM(get_logger(), "AT128 calibration size:" << received_bytes.size() << "\n"); + auto rt = correction_configuration.SaveFileFromBinary(correction_file_path_from_sensor, received_bytes); + if(rt == Status::OK) + { + RCLCPP_INFO_STREAM(get_logger(), "SaveFileFromBinary success:" << correction_file_path_from_sensor << "\n"); + } + else + { + RCLCPP_ERROR_STREAM(get_logger(), "SaveFileFromBinary failed:" << correction_file_path_from_sensor << ". Falling back to offline calibration file."); + run_local = true; + } + rt = correction_configuration.LoadFromBinary(received_bytes); + if(rt == Status::OK) + { + RCLCPP_INFO_STREAM(get_logger(), "LoadFromBinary success" << "\n"); + run_local = false; + } + else + { + RCLCPP_ERROR_STREAM(get_logger(), "LoadFromBinary failed" << ". Falling back to offline calibration file."); + run_local = true; + } + }else{ + RCLCPP_ERROR_STREAM(get_logger(), "Falling back to offline calibration file."); + run_local = true; + } + }); + if (!run_local) { + std::future_status status; + status = future.wait_for(std::chrono::milliseconds(8000)); + if (status == std::future_status::timeout) { + std::cerr << "# std::future_status::timeout\n"; + run_local = true; + } else if (status == std::future_status::ready && !run_local) { + RCLCPP_INFO_STREAM( + this->get_logger(), "Acquired correction data from sensor: '" + << sensor_cfg_ptr_->sensor_ip << "'"); + RCLCPP_INFO_STREAM( + this->get_logger(), "The correction has been saved in '" + << correction_file_path_from_sensor << "'"); + } + } + if(run_local) { + bool run_org = false; + if (correction_file_path_from_sensor.empty()) { + run_org = true; + } else { + auto cal_status = + correction_configuration.LoadFromFile(correction_file_path_from_sensor); + + if (cal_status != Status::OK) { + run_org = true; + }else{ + RCLCPP_INFO_STREAM( + this->get_logger(), "Load correction data from: '" + << correction_file_path_from_sensor << "'"); + } + } + if(run_org) { + if (correction_file_path.empty()) { + RCLCPP_ERROR_STREAM( + this->get_logger(), "Empty Correction File: '" << correction_file_path << "'"); + return Status::INVALID_CALIBRATION_FILE; + } else { + auto cal_status = correction_configuration.LoadFromFile(correction_file_path); + + if (cal_status != Status::OK) { + RCLCPP_ERROR_STREAM( + this->get_logger(), "Given Correction File: '" << correction_file_path << "'"); + return cal_status; + }else{ + RCLCPP_INFO_STREAM( + this->get_logger(), "Load correction data from: '" + << correction_file_path << "'"); + } + } + } + } + } // end AT128 + + return Status::OK; + } + +HesaiRosWrapper::~HesaiRosWrapper() { + RCLCPP_INFO_STREAM(get_logger(), "Closing TcpDriver"); + hw_interface_.FinalizeTcpDriver(); +} + +Status HesaiRosWrapper::StreamStart() +{ + if (Status::OK == interface_status_) { + interface_status_ = hw_interface_.CloudInterfaceStart(); + } + return interface_status_; +} + +Status HesaiRosWrapper::StreamStop() { return Status::OK; } +Status HesaiRosWrapper::Shutdown() { return Status::OK; } + +Status HesaiRosWrapper::InitializeHwInterface( // todo: don't think this is needed + const drivers::SensorConfigurationBase & sensor_configuration) +{ + std::stringstream ss; + ss << sensor_configuration; + RCLCPP_DEBUG_STREAM(this->get_logger(), ss.str()); + return Status::OK; +} + +Status HesaiRosWrapper::InitializeHwMonitor( // todo: don't think this is needed + const drivers::SensorConfigurationBase & sensor_configuration) +{ + return Status::OK; +} + +void HesaiRosWrapper::ReceiveScanDataCallback( + std::unique_ptr scan_buffer) +{ + // Publish + scan_buffer->header.frame_id = sensor_cfg_ptr_->frame_id; + scan_buffer->header.stamp = scan_buffer->packets.front().stamp; + pandar_scan_pub_->publish(std::move(scan_buffer)); +} + +rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::paramCallback( + const std::vector & p) +{ + std::scoped_lock lock(mtx_config_); + RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback"); + RCLCPP_DEBUG_STREAM(this->get_logger(), p); + RCLCPP_DEBUG_STREAM(this->get_logger(), *sensor_cfg_ptr_); + RCLCPP_INFO_STREAM(this->get_logger(), p); + + std::shared_ptr new_param = + std::make_shared(*sensor_cfg_ptr_); + RCLCPP_INFO_STREAM(this->get_logger(), new_param); + std::string sensor_model_str; + std::string return_mode_str; + if ( + get_param(p, "sensor_model", sensor_model_str) | + get_param(p, "return_mode", return_mode_str) | + get_param(p, "host_ip", new_param->host_ip) | + get_param(p, "sensor_ip", new_param->sensor_ip) | + get_param(p, "frame_id", new_param->frame_id) | + get_param(p, "data_port", new_param->data_port) | + get_param(p, "gnss_port", new_param->gnss_port) | + get_param(p, "scan_phase", new_param->scan_phase) | + get_param(p, "packet_mtu_size", new_param->packet_mtu_size) | + get_param(p, "rotation_speed", new_param->rotation_speed) | + get_param(p, "cloud_min_angle", new_param->cloud_min_angle) | + get_param(p, "cloud_max_angle", new_param->cloud_max_angle) | + get_param(p, "dual_return_distance_threshold", new_param->dual_return_distance_threshold)) { + if (0 < sensor_model_str.length()) + new_param->sensor_model = nebula::drivers::SensorModelFromString(sensor_model_str); + if (0 < return_mode_str.length()) + new_param->return_mode = nebula::drivers::ReturnModeFromString(return_mode_str); + + sensor_cfg_ptr_.swap(new_param); + RCLCPP_INFO_STREAM(this->get_logger(), "Update sensor_configuration"); + RCLCPP_DEBUG_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); + hw_interface_.SetSensorConfiguration( + std::static_pointer_cast(sensor_cfg_ptr_)); + hw_interface_.CheckAndSetConfig(); + } + + auto result = std::make_shared(); + result->successful = true; + result->reason = "success"; + + RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback success"); + + return *result; +} + +std::vector HesaiRosWrapper::updateParameters() +{ + std::scoped_lock lock(mtx_config_); + RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters start"); + std::ostringstream os_sensor_model; + os_sensor_model << sensor_cfg_ptr_->sensor_model; + std::ostringstream os_return_mode; + os_return_mode << sensor_cfg_ptr_->return_mode; + RCLCPP_INFO_STREAM(this->get_logger(), "set_parameters"); + auto results = set_parameters( + {rclcpp::Parameter("sensor_model", os_sensor_model.str()), + rclcpp::Parameter("return_mode", os_return_mode.str()), + rclcpp::Parameter("host_ip", sensor_cfg_ptr_->host_ip), + rclcpp::Parameter("sensor_ip", sensor_cfg_ptr_->sensor_ip), + rclcpp::Parameter("frame_id", sensor_cfg_ptr_->frame_id), + rclcpp::Parameter("data_port", sensor_cfg_ptr_->data_port), + rclcpp::Parameter("gnss_port", sensor_cfg_ptr_->gnss_port), + rclcpp::Parameter("scan_phase", sensor_cfg_ptr_->scan_phase), + rclcpp::Parameter("packet_mtu_size", sensor_cfg_ptr_->packet_mtu_size), + rclcpp::Parameter("rotation_speed", sensor_cfg_ptr_->rotation_speed), + rclcpp::Parameter("cloud_min_angle", sensor_cfg_ptr_->cloud_min_angle), + rclcpp::Parameter("cloud_max_angle", sensor_cfg_ptr_->cloud_max_angle), + rclcpp::Parameter( + "dual_return_distance_threshold", sensor_cfg_ptr_->dual_return_distance_threshold)}); + RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters end"); + return results; +} + +void HesaiRosWrapper::InitializeHesaiDiagnostics() +{ + RCLCPP_INFO_STREAM(this->get_logger(), "InitializeHesaiDiagnostics"); + using std::chrono_literals::operator""s; + std::ostringstream os; + auto hardware_id = info_model + ": " + info_serial; + diagnostics_updater_.setHardwareID(hardware_id); + RCLCPP_INFO_STREAM(this->get_logger(), "hardware_id: " + hardware_id); + + diagnostics_updater_.add("hesai_status", this, &HesaiRosWrapper::HesaiCheckStatus); + diagnostics_updater_.add("hesai_ptp", this, &HesaiRosWrapper::HesaiCheckPtp); + diagnostics_updater_.add( + "hesai_temperature", this, &HesaiRosWrapper::HesaiCheckTemperature); + diagnostics_updater_.add("hesai_rpm", this, &HesaiRosWrapper::HesaiCheckRpm); + + current_status.reset(); + current_monitor.reset(); + current_status_time.reset(new rclcpp::Time(this->get_clock()->now())); + current_lidar_monitor_time.reset(new rclcpp::Time(this->get_clock()->now())); + current_diag_status = diagnostic_msgs::msg::DiagnosticStatus::STALE; + current_monitor_status = diagnostic_msgs::msg::DiagnosticStatus::STALE; + + auto fetch_diag_from_sensor = [this](){ + OnHesaiStatusTimer(); + if (hw_interface_.UseHttpGetLidarMonitor()) { + OnHesaiLidarMonitorTimerHttp(); + } else { + OnHesaiLidarMonitorTimer(); + } + }; + + fetch_diagnostics_timer_ = std::make_shared>( + this->get_clock(), std::chrono::milliseconds(diag_span_), std::move(fetch_diag_from_sensor), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(fetch_diagnostics_timer_, cbg_m_); + + if (hw_interface_.UseHttpGetLidarMonitor()) { + diagnostics_updater_.add( + "hesai_voltage", this, &HesaiRosWrapper::HesaiCheckVoltageHttp); + } else { + diagnostics_updater_.add("hesai_voltage", this, &HesaiRosWrapper::HesaiCheckVoltage); + } + + auto on_timer_update = [this] { + RCLCPP_DEBUG_STREAM(get_logger(), "OnUpdateTimer"); + auto now = this->get_clock()->now(); + auto dif = (now - *current_status_time).seconds(); + + RCLCPP_DEBUG_STREAM(get_logger(), "dif(status): " << dif); + + if (diag_span_ * 2.0 < dif * 1000) { + current_diag_status = diagnostic_msgs::msg::DiagnosticStatus::STALE; + RCLCPP_DEBUG_STREAM(get_logger(), "STALE"); + } else { + current_diag_status = diagnostic_msgs::msg::DiagnosticStatus::OK; + RCLCPP_DEBUG_STREAM(get_logger(), "OK"); + } + dif = (now - *current_lidar_monitor_time).seconds(); + RCLCPP_DEBUG_STREAM(get_logger(), "dif(monitor): " << dif); + if (diag_span_ * 2.0 < dif * 1000) { + current_monitor_status = diagnostic_msgs::msg::DiagnosticStatus::STALE; + RCLCPP_DEBUG_STREAM(get_logger(), "STALE"); + } else { + current_monitor_status = diagnostic_msgs::msg::DiagnosticStatus::OK; + RCLCPP_DEBUG_STREAM(get_logger(), "OK"); + } + diagnostics_updater_.force_update(); + }; + diagnostics_update_timer_ = std::make_shared>( + this->get_clock(), std::chrono::milliseconds(1000), std::move(on_timer_update), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(diagnostics_update_timer_, cbg_r_); + + RCLCPP_DEBUG_STREAM(get_logger(), "add_timer"); +} + +std::string HesaiRosWrapper::GetPtreeValue( + boost::property_tree::ptree * pt, const std::string & key) +{ + boost::optional value = pt->get_optional(key); + if (value) { + return value.get(); + } else { + return not_supported_message; + } +} +std::string HesaiRosWrapper::GetFixedPrecisionString(double val, int pre) +{ + std::stringstream ss; + ss << std::fixed << std::setprecision(pre) << val; + return ss.str(); +} + +void HesaiRosWrapper::OnHesaiStatusTimer() +{ + RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiStatusTimer" << std::endl); + try { + auto result = hw_interface_.GetLidarStatus(); + std::scoped_lock lock(mtx_status); + current_status_time.reset(new rclcpp::Time(this->get_clock()->now())); + current_status.reset(new HesaiLidarStatus(result)); + } catch (const std::system_error & error) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("HesaiRosWrapper::OnHesaiStatusTimer(std::system_error)"), + error.what()); + } catch (const boost::system::system_error & error) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger( + "HesaiRosWrapper::OnHesaiStatusTimer(boost::system::system_error)"), + error.what()); + } + RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiStatusTimer END" << std::endl); +} + +void HesaiRosWrapper::OnHesaiLidarMonitorTimerHttp() +{ + RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiLidarMonitorTimerHttp"); + try { + hw_interface_.GetLidarMonitorAsyncHttp([this](const std::string & str) { + std::scoped_lock lock(mtx_lidar_monitor); + current_lidar_monitor_time.reset(new rclcpp::Time(this->get_clock()->now())); + current_lidar_monitor_tree = + std::make_unique(hw_interface_.ParseJson(str)); + }); + } catch (const std::system_error & error) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger( + "HesaiRosWrapper::OnHesaiLidarMonitorTimerHttp(std::system_error)"), + error.what()); + } catch (const boost::system::system_error & error) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger( + "HesaiRosWrapper::OnHesaiLidarMonitorTimerHttp(boost::system::system_error)"), + error.what()); + } + RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiLidarMonitorTimerHttp END"); +} + +void HesaiRosWrapper::OnHesaiLidarMonitorTimer() +{ + RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiLidarMonitorTimer"); + try { + auto ios = std::make_shared(); + auto result = hw_interface_.GetLidarMonitor(); + std::scoped_lock lock(mtx_lidar_monitor); + current_lidar_monitor_time.reset(new rclcpp::Time(this->get_clock()->now())); + current_monitor.reset(new HesaiLidarMonitor(result)); + } catch (const std::system_error & error) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("HesaiRosWrapper::OnHesaiLidarMonitorTimer(std::system_error)"), + error.what()); + } catch (const boost::system::system_error & error) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger( + "HesaiRosWrapper::OnHesaiLidarMonitorTimer(boost::system::system_error)"), + error.what()); + } + RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiLidarMonitorTimer END"); +} + +void HesaiRosWrapper::HesaiCheckStatus( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + std::scoped_lock lock(mtx_status); + if (current_status) { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; + + diagnostics.add("system_uptime", std::to_string(current_status->system_uptime)); + diagnostics.add("startup_times", std::to_string(current_status->startup_times)); + diagnostics.add("total_operation_time", std::to_string(current_status->total_operation_time)); + + diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + } else { + diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); + } +} + +void HesaiRosWrapper::HesaiCheckPtp( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + std::scoped_lock lock(mtx_status); + if (current_status) { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; + auto gps_status = current_status->get_str_gps_pps_lock(); + auto gprmc_status = current_status->get_str_gps_gprmc_status(); + auto ptp_status = current_status->get_str_ptp_clock_status(); + std::transform(gps_status.cbegin(), gps_status.cend(), gps_status.begin(), toupper); + std::transform(gprmc_status.cbegin(), gprmc_status.cend(), gprmc_status.begin(), toupper); + std::transform(ptp_status.cbegin(), ptp_status.cend(), ptp_status.begin(), toupper); + diagnostics.add("gps_pps_lock", gps_status); + diagnostics.add("gps_gprmc_status", gprmc_status); + diagnostics.add("ptp_clock_status", ptp_status); + if (gps_status != "UNKNOWN") { + msg.emplace_back("gps_pps_lock: " + gps_status); + } + if (gprmc_status != "UNKNOWN") { + msg.emplace_back("gprmc_status: " + gprmc_status); + } + if (ptp_status != "UNKNOWN") { + msg.emplace_back("ptp_status: " + ptp_status); + } + if (ptp_status == "FREE RUN" && gps_status == "UNKNOWN") { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + } + diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + } else { + diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); + } +} + +void HesaiRosWrapper::HesaiCheckTemperature( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + std::scoped_lock lock(mtx_status); + if (current_status) { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; + for (size_t i = 0; i < current_status->temperature.size(); i++) { + diagnostics.add( + temperature_names[i], GetFixedPrecisionString(current_status->temperature[i] * 0.01)); + } + diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + } else { + diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); + } +} + +void HesaiRosWrapper::HesaiCheckRpm( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + std::scoped_lock lock(mtx_status); + if (current_status) { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; + diagnostics.add("motor_speed", std::to_string(current_status->motor_speed)); + + diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + } else { + diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); + } +} + +void HesaiRosWrapper::HesaiCheckVoltageHttp( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + std::scoped_lock lock(mtx_lidar_monitor); + if (current_lidar_monitor_tree) { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; + std::string key = ""; + + std::string mes; + key = "lidarInCur"; + try { + mes = GetPtreeValue(current_lidar_monitor_tree.get(), "Body." + key); + } catch (boost::bad_lexical_cast & ex) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = error_message + std::string(ex.what()); + } + diagnostics.add(key, mes); + key = "lidarInVol"; + try { + mes = GetPtreeValue(current_lidar_monitor_tree.get(), "Body." + key); + } catch (boost::bad_lexical_cast & ex) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = error_message + std::string(ex.what()); + } + diagnostics.add(key, mes); + + diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + } else { + diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); + } +} + +void HesaiRosWrapper::HesaiCheckVoltage( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + std::scoped_lock lock(mtx_lidar_monitor); + if (current_monitor) { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; + diagnostics.add( + "input_voltage", GetFixedPrecisionString(current_monitor->input_voltage * 0.01) + " V"); + diagnostics.add( + "input_current", GetFixedPrecisionString(current_monitor->input_current * 0.01) + " mA"); + diagnostics.add( + "input_power", GetFixedPrecisionString(current_monitor->input_power * 0.01) + " W"); + + diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + } else { + diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); + } +} + + RCLCPP_COMPONENTS_REGISTER_NODE(HesaiRosWrapper) +} // namespace ros +} // namespace nebula From c10c6abdd8ac2ec076db71e33aac0792eca9be28 Mon Sep 17 00:00:00 2001 From: Maximilian Schmeller Date: Thu, 21 Mar 2024 18:34:02 +0900 Subject: [PATCH 03/57] refactor: remove pandarScan pub/sub, decode one packet at a time --- .../decoders/hesai_decoder.hpp | 14 +++--- .../decoders/hesai_scan_decoder.hpp | 4 +- .../nebula_decoders_hesai/hesai_driver.hpp | 4 +- .../nebula_decoders_hesai/hesai_driver.cpp | 25 +++++----- .../hesai_hw_interface.hpp | 6 +-- .../hesai_hw_interface.cpp | 48 ++----------------- .../nebula_ros/hesai/hesai_ros_wrapper.hpp | 6 +-- nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 29 +++++------ 8 files changed, 41 insertions(+), 95 deletions(-) diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp index b81b45d44..8f28856e8 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp @@ -52,17 +52,17 @@ class HesaiDecoder : public HesaiScanDecoder block_firing_offset_ns_; /// @brief Validates and parse PandarPacket. Currently only checks size, not checksums etc. - /// @param pandar_packet The incoming PandarPacket + /// @param packet The incoming PandarPacket /// @return Whether the packet was parsed successfully - bool parsePacket(const pandar_msgs::msg::PandarPacket & pandar_packet) + bool parsePacket(const std::vector & packet) { - if (pandar_packet.size < sizeof(typename SensorT::packet_t)) { + if (packet.size() < sizeof(typename SensorT::packet_t)) { RCLCPP_ERROR_STREAM( - logger_, "Packet size mismatch:" << pandar_packet.size << " | Expected at least:" + logger_, "Packet size mismatch:" << packet.size() << " | Expected at least:" << sizeof(typename SensorT::packet_t)); return false; } - if (std::memcpy(&packet_, pandar_packet.data.data(), sizeof(typename SensorT::packet_t))) { + if (std::memcpy(&packet_, packet.data(), sizeof(typename SensorT::packet_t))) { // FIXME(mojomex) do validation? // RCLCPP_DEBUG(logger_, "Packet parsed successfully"); return true; @@ -222,9 +222,9 @@ class HesaiDecoder : public HesaiScanDecoder output_pc_->reserve(SensorT::MAX_SCAN_BUFFER_POINTS); } - int unpack(const pandar_msgs::msg::PandarPacket & pandar_packet) override + int unpack(const std::vector & packet) override { - if (!parsePacket(pandar_packet)) { + if (!parsePacket(packet)) { return -1; } diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp index f0b4e3f8b..9d06c2702 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp @@ -26,9 +26,9 @@ class HesaiScanDecoder HesaiScanDecoder() = default; /// @brief Parses PandarPacket and add its points to the point cloud - /// @param pandar_packet The incoming PandarPacket + /// @param packet The incoming PandarPacket /// @return The last azimuth processed - virtual int unpack(const pandar_msgs::msg::PandarPacket & pandar_packet) = 0; + virtual int unpack(const std::vector & packet) = 0; /// @brief Indicates whether one full scan is ready /// @return Whether a scan is ready diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp index c83788965..a3f37cd32 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp @@ -54,8 +54,8 @@ class HesaiDriver : NebulaDriverBase /// @brief Convert PandarScan message to point cloud /// @param pandar_scan Message /// @return tuple of Point cloud and timestamp - std::tuple ConvertScanToPointcloud( - const std::shared_ptr & pandar_scan); + std::tuple ParseCloudPacket( + const std::vector & packet); }; } // namespace drivers diff --git a/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp b/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp index 49be4adac..61b969ed7 100644 --- a/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp +++ b/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp @@ -72,8 +72,8 @@ HesaiDriver::HesaiDriver( } } -std::tuple HesaiDriver::ConvertScanToPointcloud( - const std::shared_ptr & pandar_scan) +std::tuple HesaiDriver::ParseCloudPacket( + const std::vector & packet) { std::tuple pointcloud; auto logger = rclcpp::get_logger("HesaiDriver"); @@ -83,20 +83,17 @@ std::tuple HesaiDriver::ConvertScanToPoint return pointcloud; } - int cnt = 0, last_azimuth = 0; - for (auto & packet : pandar_scan->packets) { - last_azimuth = scan_decoder_->unpack(packet); - if (scan_decoder_->hasScanned()) { - pointcloud = scan_decoder_->getPointcloud(); - cnt++; - } + scan_decoder_->unpack(packet); + if (scan_decoder_->hasScanned()) { + pointcloud = scan_decoder_->getPointcloud(); } - if (cnt == 0) { - RCLCPP_ERROR_STREAM( - logger, "Scanned " << pandar_scan->packets.size() << " packets, but no " - << "pointclouds were generated. Last azimuth: " << last_azimuth); - } + // todo + // if (cnt == 0) { + // RCLCPP_ERROR_STREAM( + // logger, "Scanned " << pandar_scan->packets.size() << " packets, but no " + // << "pointclouds were generated. Last azimuth: " << last_azimuth); + // } return pointcloud; } diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp index 1b21ddae2..34c5208ee 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp @@ -127,8 +127,8 @@ class HesaiHwInterface : NebulaHwInterfaceBase std::unique_ptr scan_cloud_ptr_; std::function is_valid_packet_; /*Lambda Function Array to verify proper packet size*/ - std::function buffer)> - scan_reception_callback_; /**This function pointer is called when the scan is complete*/ + std::function & buffer)> + cloud_packet_callback_; /**This function pointer is called when the scan is complete*/ int prev_phase_{}; @@ -222,7 +222,7 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @param scan_callback Callback function /// @return Resulting status Status RegisterScanCallback( - std::function)> scan_callback); + std::function &)> scan_callback); /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION /// @return Resulting status std::string GetLidarCalibrationString(); diff --git a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp index fccb2a622..4fbb020b0 100644 --- a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp @@ -239,61 +239,19 @@ Status HesaiHwInterface::SensorInterfaceStart() } Status HesaiHwInterface::RegisterScanCallback( - std::function)> scan_callback) + std::function &)> scan_callback) { - scan_reception_callback_ = std::move(scan_callback); + cloud_packet_callback_ = std::move(scan_callback); return Status::OK; } void HesaiHwInterface::ReceiveSensorPacketCallback(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; } - const uint32_t buffer_size = buffer.size(); - pandar_msgs::msg::PandarPacket pandar_packet; - std::copy_n(std::make_move_iterator(buffer.begin()), buffer_size, pandar_packet.data.begin()); - pandar_packet.size = buffer_size; - auto now = std::chrono::system_clock::now(); - auto now_secs = std::chrono::duration_cast(now.time_since_epoch()).count(); - auto now_nanosecs = - std::chrono::duration_cast(now.time_since_epoch()).count(); - pandar_packet.stamp.sec = static_cast(now_secs); - pandar_packet.stamp.nanosec = static_cast(now_nanosecs % 1'000'000'000); - scan_cloud_ptr_->packets.emplace_back(pandar_packet); - - int current_phase = 0; - bool comp_flg = false; - - const auto & data = scan_cloud_ptr_->packets.back().data; - current_phase = (data[azimuth_index_] & 0xff) + ((data[azimuth_index_ + 1] & 0xff) << 8); - if (is_solid_state) { - current_phase = (static_cast(current_phase) + 36000 - 0) % 12000; - if (current_phase >= prev_phase_ || scan_cloud_ptr_->packets.size() < 2) { - prev_phase_ = current_phase; - } else { - comp_flg = true; - } - } else { - 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(); - } - } + cloud_packet_callback_(buffer); } Status HesaiHwInterface::SensorInterfaceStop() { diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp index 283f63c68..e3b1cc8e8 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp @@ -58,10 +58,6 @@ class HesaiRosWrapper final : public rclcpp::Node, NebulaRosWrapperBase explicit HesaiRosWrapper(const rclcpp::NodeOptions & options); ~HesaiRosWrapper(); - /// @brief Callback for PandarScan subscriber - /// @param scan_msg Received PandarScan - void ReceiveScanMsgCallback(const pandar_msgs::msg::PandarScan::SharedPtr scan_msg); - /// @brief Get current status of this driver /// @return Current status Status GetStatus(); @@ -134,7 +130,7 @@ class HesaiRosWrapper final : public rclcpp::Node, NebulaRosWrapperBase const drivers::SensorConfigurationBase & sensor_configuration) override; /// @brief Callback for receiving PandarScan /// @param scan_buffer Received PandarScan - void ReceiveScanDataCallback(std::unique_ptr scan_buffer); + void ReceiveCloudPacketCallback(const std::vector & scan_buffer); /// @brief rclcpp parameter callback /// @param parameters Received parameters diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index 4fc2a12df..b809bccf8 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -69,7 +69,7 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) #endif hw_interface_.RegisterScanCallback( - std::bind(&HesaiRosWrapper::ReceiveScanDataCallback, this, std::placeholders::_1)); + std::bind(&HesaiRosWrapper::ReceiveCloudPacketCallback, this, std::placeholders::_1)); pandar_scan_pub_ = this->create_publisher("pandar_packets", rclcpp::SensorDataQoS()); @@ -117,9 +117,7 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); - pandar_scan_sub_ = create_subscription( - "pandar_packets", qos, - std::bind(&HesaiRosWrapper::ReceiveScanMsgCallback, this, std::placeholders::_1)); + nebula_points_pub_ = this->create_publisher("pandar_points", rclcpp::SensorDataQoS()); aw_points_base_pub_ = @@ -198,16 +196,22 @@ cbg_r_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant); } } -void HesaiRosWrapper::ReceiveScanMsgCallback( - const pandar_msgs::msg::PandarScan::SharedPtr scan_msg) +void HesaiRosWrapper::ReceiveCloudPacketCallback( + const std::vector & packet) { + // Driver is not initialized yet + if (!driver_ptr_) { + return; + } + auto t_start = std::chrono::high_resolution_clock::now(); std::tuple pointcloud_ts = - driver_ptr_->ConvertScanToPointcloud(scan_msg); + driver_ptr_->ParseCloudPacket(packet); nebula::drivers::NebulaPointCloudPtr pointcloud = std::get<0>(pointcloud_ts); if (pointcloud == nullptr) { - RCLCPP_WARN_STREAM(get_logger(), "Empty cloud parsed."); + // todo + // RCLCPP_WARN_STREAM(get_logger(), "Empty cloud parsed."); return; }; if ( @@ -845,15 +849,6 @@ Status HesaiRosWrapper::InitializeHwMonitor( // todo: don't think this is neede return Status::OK; } -void HesaiRosWrapper::ReceiveScanDataCallback( - std::unique_ptr scan_buffer) -{ - // Publish - scan_buffer->header.frame_id = sensor_cfg_ptr_->frame_id; - scan_buffer->header.stamp = scan_buffer->packets.front().stamp; - pandar_scan_pub_->publish(std::move(scan_buffer)); -} - rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::paramCallback( const std::vector & p) { From 4f014c32356a51ed60a083298877a127b42fc159 Mon Sep 17 00:00:00 2001 From: Maximilian Schmeller Date: Tue, 26 Mar 2024 16:31:25 +0900 Subject: [PATCH 04/57] reword later --- nebula_ros/CMakeLists.txt | 1 + .../nebula_ros/hesai/hesai_ros_wrapper.hpp | 28 +- nebula_ros/launch/hesai_launch_all_hw.xml | 4 - nebula_ros/package.xml | 1 + nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 557 +++++++++--------- scripts/plot_times.py | 7 +- 6 files changed, 309 insertions(+), 289 deletions(-) diff --git a/nebula_ros/CMakeLists.txt b/nebula_ros/CMakeLists.txt index 18ce06aad..1bd697624 100644 --- a/nebula_ros/CMakeLists.txt +++ b/nebula_ros/CMakeLists.txt @@ -23,6 +23,7 @@ find_package(nebula_decoders REQUIRED) find_package(nebula_hw_interfaces REQUIRED) find_package(yaml-cpp REQUIRED) find_package(robosense_msgs REQUIRED) +find_package(nebula_msgs REQUIRED) include_directories( include diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp index e3b1cc8e8..e91d7fcaf 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp @@ -15,8 +15,7 @@ #include #include -#include "pandar_msgs/msg/pandar_packet.hpp" -#include "pandar_msgs/msg/pandar_scan.hpp" +#include "nebula_msgs/msg/nebula_packet.hpp" #include #include @@ -73,23 +72,21 @@ class HesaiRosWrapper final : public rclcpp::Node, NebulaRosWrapperBase Status Shutdown() override; private: - /// @brief Initializing ros wrapper + /// @brief Initialize pointcloud decoder /// @param sensor_configuration SensorConfiguration for this driver /// @param calibration_configuration CalibrationConfiguration for this driver - /// @return Resulting status - Status InitializeCloudDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) override; - - /// @brief Initializing ros wrapper for AT - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @param correction_configuration CorrectionConfiguration for this driver + /// @param correction_configuration CorrectionConfiguration for this driver (only for AT128, ignored otherwise) /// @return Resulting status Status InitializeCloudDriver( const std::shared_ptr & sensor_configuration, const std::shared_ptr & calibration_configuration, - const std::shared_ptr & correction_configuration); + const std::shared_ptr & correction_configuration = nullptr); + + Status InitializeCloudDriver( + std::shared_ptr sensor_configuration, + std::shared_ptr calibration_configuration) { + return InitializeCloudDriver(sensor_configuration, calibration_configuration, nullptr); + } /// @brief Get configurations from ros parameters /// @param sensor_configuration Output of SensorConfiguration @@ -189,7 +186,10 @@ class HesaiRosWrapper final : public rclcpp::Node, NebulaRosWrapperBase std::shared_ptr driver_ptr_; Status wrapper_status_; - rclcpp::Subscription::SharedPtr pandar_scan_sub_; + + rclcpp::Publisher::SharedPtr packet_pub_; + rclcpp::Subscription::SharedPtr packet_sub_; + rclcpp::Publisher::SharedPtr nebula_points_pub_; rclcpp::Publisher::SharedPtr aw_points_ex_pub_; rclcpp::Publisher::SharedPtr aw_points_base_pub_; diff --git a/nebula_ros/launch/hesai_launch_all_hw.xml b/nebula_ros/launch/hesai_launch_all_hw.xml index 579852fb8..a74eb9813 100644 --- a/nebula_ros/launch/hesai_launch_all_hw.xml +++ b/nebula_ros/launch/hesai_launch_all_hw.xml @@ -27,8 +27,6 @@ - - @@ -55,7 +53,6 @@ - @@ -63,6 +60,5 @@ - diff --git a/nebula_ros/package.xml b/nebula_ros/package.xml index 87bc000c9..54df70ea9 100644 --- a/nebula_ros/package.xml +++ b/nebula_ros/package.xml @@ -31,6 +31,7 @@ velodyne_msgs visualization_msgs yaml-cpp + nebula_msgs ament_cmake_gtest ament_lint_auto diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index b809bccf8..ce81d6b3e 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -5,7 +5,9 @@ namespace nebula namespace ros { HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) -: rclcpp::Node("hesai_ros_wrapper", options), hw_interface_(), diagnostics_updater_(this) +: rclcpp::Node("hesai_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)), + hw_interface_(), + diagnostics_updater_(this) { setvbuf(stdout, NULL, _IONBF, BUFSIZ); @@ -14,33 +16,29 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) sensor_cfg_ptr_ = std::make_shared(sensor_configuration); // hwiface -{ - if (Status::OK != interface_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); - return; - } - std::this_thread::sleep_for(std::chrono::milliseconds(this->delay_hw_ms_)); - hw_interface_.SetLogger(std::make_shared(this->get_logger())); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr_)); -#if not defined(TEST_PCAP) - Status rt = hw_interface_.InitializeTcpDriver(); - if(this->retry_hw_) { - int cnt = 0; - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << " Retry: " << cnt); - while(rt == Status::ERROR_1) - { - cnt++; - std::this_thread::sleep_for(std::chrono::milliseconds(8000));// >5000 - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Retry: " << cnt); - rt = hw_interface_.InitializeTcpDriver(); + 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())); + hw_interface_.SetSensorConfiguration( + std::static_pointer_cast(sensor_cfg_ptr_)); + Status rt = hw_interface_.InitializeTcpDriver(); + if (this->retry_hw_) { + int cnt = 0; + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << " Retry: " << cnt); + while (rt == Status::ERROR_1) { + cnt++; + std::this_thread::sleep_for(std::chrono::milliseconds(8000)); // >5000 + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Retry: " << cnt); + rt = hw_interface_.InitializeTcpDriver(); + } } - } - if(rt != Status::ERROR_1){ - try{ - std::vector thread_pool{}; + if (rt != Status::ERROR_1) { + try { + std::vector thread_pool{}; thread_pool.emplace_back([this] { auto result = hw_interface_.GetInventory(); RCLCPP_INFO_STREAM(get_logger(), result); @@ -50,74 +48,60 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) th.join(); } + } catch (...) { + std::cout << "catch (...) in parent" << std::endl; + RCLCPP_ERROR_STREAM(get_logger(), "Failed to get model from sensor..."); + } + if (this->setup_sensor) { + hw_interface_.CheckAndSetConfig(); + updateParameters(); + } + } else { + RCLCPP_ERROR_STREAM( + get_logger(), + "Failed to get model from sensor... Set from config: " << sensor_cfg_ptr_->sensor_model); + hw_interface_.SetTargetModel(sensor_cfg_ptr_->sensor_model); } - catch (...) - { - std::cout << "catch (...) in parent" << std::endl; - RCLCPP_ERROR_STREAM(get_logger(), "Failed to get model from sensor..."); - } - if (this->setup_sensor) { - hw_interface_.CheckAndSetConfig(); - updateParameters(); - } - } - else - { - RCLCPP_ERROR_STREAM(get_logger(), "Failed to get model from sensor... Set from config: " << sensor_cfg_ptr_->sensor_model); - hw_interface_.SetTargetModel(sensor_cfg_ptr_->sensor_model); - } -#endif - - hw_interface_.RegisterScanCallback( - std::bind(&HesaiRosWrapper::ReceiveCloudPacketCallback, this, std::placeholders::_1)); - pandar_scan_pub_ = - this->create_publisher("pandar_packets", rclcpp::SensorDataQoS()); - -#if not defined(TEST_PCAP) - if (this->setup_sensor) { - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&HesaiRosWrapper::paramCallback, this, std::placeholders::_1)); } -#endif - - RCLCPP_DEBUG(this->get_logger(), "Starting stream"); - StreamStart(); -} // decoder { - drivers::HesaiCalibrationConfiguration calibration_configuration; - drivers::HesaiCorrection correction_configuration; - - wrapper_status_ = - GetCalibrationData(calibration_configuration, correction_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); + drivers::HesaiCalibrationConfiguration calibration_configuration; + drivers::HesaiCorrection correction_configuration; + wrapper_status_ = GetCalibrationData(calibration_configuration, correction_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..."); - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); - if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { + calibration_cfg_ptr_ = + std::make_shared(calibration_configuration); + // Will be left empty for AT128, and ignored by all decoders except AT128 correction_cfg_ptr_ = std::make_shared(correction_configuration); + + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); wrapper_status_ = InitializeCloudDriver( std::static_pointer_cast(sensor_cfg_ptr_), std::static_pointer_cast(calibration_cfg_ptr_), std::static_pointer_cast(correction_cfg_ptr_)); - } else { - wrapper_status_ = InitializeCloudDriver( - std::static_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_)); - } + + RCLCPP_DEBUG(this->get_logger(), "Starting stream"); + StreamStart(); + hw_interface_.RegisterScanCallback( + std::bind(&HesaiRosWrapper::ReceiveCloudPacketCallback, this, std::placeholders::_1)); RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Wrapper=" << wrapper_status_); rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); + packet_pub_ = create_publisher( + "hesai_packets", rclcpp::SensorDataQoS()); + + packet_sub_ = create_subscription( + "hesai_packets", rclcpp::SensorDataQoS(), [](nebula_msgs::msg::NebulaPacket::UniquePtr){}); + nebula_points_pub_ = this->create_publisher("pandar_points", rclcpp::SensorDataQoS()); aw_points_base_pub_ = @@ -194,11 +178,18 @@ cbg_r_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant); set_param_res_ = this->add_on_set_parameters_callback( std::bind(&HesaiRosWrapper::paramCallback, this, std::placeholders::_1)); } -} -void HesaiRosWrapper::ReceiveCloudPacketCallback( - const std::vector & packet) +void HesaiRosWrapper::ReceiveCloudPacketCallback(const std::vector & packet) { + auto t_received = std::chrono::high_resolution_clock::now().time_since_epoch().count(); + auto msg_ptr = std::make_unique(packet.size()); + msg_ptr->stamp.sec = t_received / 1'000'000'000; + msg_ptr->stamp.nanosec = t_received % 1'000'000'000; + // TODO(mojomex) this copy could be avoided if transport_drivers would give us a non-const vector + std::copy(packet.begin(), packet.end(), msg_ptr->data.begin()); + packet_pub_->publish(std::move(msg_ptr)); + + //todo // Driver is not initialized yet if (!driver_ptr_) { return; @@ -209,6 +200,12 @@ void HesaiRosWrapper::ReceiveCloudPacketCallback( driver_ptr_->ParseCloudPacket(packet); nebula::drivers::NebulaPointCloudPtr pointcloud = std::get<0>(pointcloud_ts); + auto t_end = std::chrono::high_resolution_clock::now(); + auto runtime = t_end - t_start; + t_start = t_end; + RCLCPP_DEBUG( + get_logger(), "PROFILING {'d_decode_packet': %lu}", runtime.count()); + if (pointcloud == nullptr) { // todo // RCLCPP_WARN_STREAM(get_logger(), "Empty cloud parsed."); @@ -237,8 +234,8 @@ void HesaiRosWrapper::ReceiveCloudPacketCallback( 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)); + 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 = @@ -246,8 +243,9 @@ void HesaiRosWrapper::ReceiveCloudPacketCallback( 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()); + runtime = std::chrono::high_resolution_clock::now() - t_start; + RCLCPP_DEBUG( + get_logger(), "PROFILING {'d_convert_pc': %lu, 'n_out': %lu}", runtime.count(), pointcloud->size()); } void HesaiRosWrapper::PublishCloud( @@ -261,22 +259,11 @@ void HesaiRosWrapper::PublishCloud( publisher->publish(std::move(pointcloud)); } -Status HesaiRosWrapper::InitializeCloudDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) -{ - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration)); - return driver_ptr_->GetStatus(); -} - Status HesaiRosWrapper::InitializeCloudDriver( const std::shared_ptr & sensor_configuration, const std::shared_ptr & calibration_configuration, const std::shared_ptr & correction_configuration) { - // driver should be initialized here with proper decoder driver_ptr_ = std::make_shared( std::static_pointer_cast(sensor_configuration), std::static_pointer_cast(calibration_configuration), @@ -284,11 +271,12 @@ Status HesaiRosWrapper::InitializeCloudDriver( return driver_ptr_->GetStatus(); } -Status HesaiRosWrapper::GetStatus() { return wrapper_status_; } +Status HesaiRosWrapper::GetStatus() +{ + return wrapper_status_; +} -/// decoder -Status HesaiRosWrapper::GetParameters( - drivers::HesaiSensorConfiguration & sensor_configuration) +Status HesaiRosWrapper::GetParameters(drivers::HesaiSensorConfiguration & sensor_configuration) { /////////////////////////////////////////////// // Define and get ROS parameters @@ -377,8 +365,7 @@ Status HesaiRosWrapper::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; this->declare_parameter("calibration_file", "", descriptor); - calibration_file_path = - this->get_parameter("calibration_file").as_string(); + calibration_file_path = this->get_parameter("calibration_file").as_string(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -492,16 +479,6 @@ Status HesaiRosWrapper::GetParameters( this->declare_parameter("setup_sensor", true, descriptor); this->setup_sensor = this->get_parameter("setup_sensor").as_bool(); } - - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "milliseconds"; - this->declare_parameter("delay_hw_ms", 0, descriptor); - this->delay_hw_ms_ = this->get_parameter("delay_hw_ms").as_int(); - } { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; @@ -528,9 +505,9 @@ Status HesaiRosWrapper::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; this->declare_parameter("ptp_transport_type", ""); - sensor_configuration.ptp_transport_type = - nebula::drivers::PtpTransportTypeFromString(this->get_parameter("ptp_transport_type").as_string()); - if(static_cast(sensor_configuration.ptp_profile) > 0) { + sensor_configuration.ptp_transport_type = nebula::drivers::PtpTransportTypeFromString( + this->get_parameter("ptp_transport_type").as_string()); + if (static_cast(sensor_configuration.ptp_profile) > 0) { sensor_configuration.ptp_transport_type = nebula::drivers::PtpTransportType::L2; } } @@ -565,32 +542,27 @@ Status HesaiRosWrapper::GetParameters( this->diag_span_ = this->get_parameter("diag_span").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 = "milliseconds"; - this->declare_parameter("delay_monitor_ms", 0, descriptor); - this->delay_monitor_ms_ = this->get_parameter("delay_monitor_ms").as_int(); - } - /////////////////////////////////////////////// // Validate ROS parameters /////////////////////////////////////////////// - if(sensor_configuration.ptp_profile == nebula::drivers::PtpProfile::PROFILE_UNKNOWN) { - RCLCPP_ERROR_STREAM(get_logger(), "Invalid PTP Profile Provided. Please use '1588v2', '802.1as' or 'automotive'"); + if (sensor_configuration.ptp_profile == nebula::drivers::PtpProfile::PROFILE_UNKNOWN) { + RCLCPP_ERROR_STREAM( + get_logger(), "Invalid PTP Profile Provided. Please use '1588v2', '802.1as' or 'automotive'"); return Status::SENSOR_CONFIG_ERROR; } - if(sensor_configuration.ptp_transport_type == nebula::drivers::PtpTransportType::UNKNOWN_TRANSPORT) { - RCLCPP_ERROR_STREAM(get_logger(), - "Invalid PTP Transport Provided. Please use 'udp' or 'l2', 'udp' is only available when using the '1588v2' PTP Profile"); + if ( + sensor_configuration.ptp_transport_type == + nebula::drivers::PtpTransportType::UNKNOWN_TRANSPORT) { + RCLCPP_ERROR_STREAM( + get_logger(), + "Invalid PTP Transport Provided. Please use 'udp' or 'l2', 'udp' is only available when " + "using the '1588v2' PTP Profile"); return Status::SENSOR_CONFIG_ERROR; } - if(sensor_configuration.ptp_switch_type == nebula::drivers::PtpSwitchType::UNKNOWN_SWITCH) { - RCLCPP_ERROR_STREAM(get_logger(), - "Invalid PTP Switch Type Provided. Please use 'tsn' or 'non_tsn'"); + if (sensor_configuration.ptp_switch_type == nebula::drivers::PtpSwitchType::UNKNOWN_SWITCH) { + RCLCPP_ERROR_STREAM( + get_logger(), "Invalid PTP Switch Type Provided. Please use 'tsn' or 'non_tsn'"); return Status::SENSOR_CONFIG_ERROR; } if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { @@ -603,69 +575,61 @@ Status HesaiRosWrapper::GetParameters( return Status::SENSOR_CONFIG_ERROR; } - /////////////////////////////////////////////// - // Decoder-specific setup - /////////////////////////////////////////////// - std::shared_ptr sensor_cfg_ptr = std::make_shared(sensor_configuration); hw_interface_.SetSensorConfiguration( std::static_pointer_cast(sensor_cfg_ptr)); - /////////////////////////////////////////////// - // HW Interface specific setup - /////////////////////////////////////////////// - - /////////////////////////////////////////////// - // HW Monitor specific setup - /////////////////////////////////////////////// - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); return Status::OK; } Status HesaiRosWrapper::GetCalibrationData( drivers::HesaiCalibrationConfiguration & calibration_configuration, - drivers::HesaiCorrection & correction_configuration) { - calibration_configuration.calibration_file = calibration_file_path; //todo + drivers::HesaiCorrection & correction_configuration) +{ + calibration_configuration.calibration_file = calibration_file_path; // todo - bool run_local = !launch_hw_; + bool run_local = !launch_hw_; if (sensor_cfg_ptr_->sensor_model != drivers::SensorModel::HESAI_PANDARAT128) { std::string calibration_file_path_from_sensor; if (launch_hw_ && !calibration_configuration.calibration_file.empty()) { int ext_pos = calibration_configuration.calibration_file.find_last_of('.'); - calibration_file_path_from_sensor += calibration_configuration.calibration_file.substr(0, ext_pos); + calibration_file_path_from_sensor += + calibration_configuration.calibration_file.substr(0, ext_pos); calibration_file_path_from_sensor += "_from_sensor"; - calibration_file_path_from_sensor += calibration_configuration.calibration_file.substr(ext_pos, calibration_configuration.calibration_file.size() - ext_pos); + calibration_file_path_from_sensor += calibration_configuration.calibration_file.substr( + ext_pos, calibration_configuration.calibration_file.size() - ext_pos); } - if(launch_hw_) { + if (launch_hw_) { run_local = false; RCLCPP_INFO_STREAM( - this->get_logger(), "Trying to acquire calibration data from sensor: '" - << sensor_cfg_ptr_->sensor_ip << "'"); - std::future future = std::async(std::launch::async, - [this, &calibration_configuration, &calibration_file_path_from_sensor, &run_local]() { - auto str = hw_interface_.GetLidarCalibrationString(); - auto rt = calibration_configuration.SaveFileFromString( - calibration_file_path_from_sensor, str); - RCLCPP_ERROR_STREAM(get_logger(), str); - if (rt == Status::OK) { - RCLCPP_INFO_STREAM(get_logger(), "SaveFileFromString success:" - << calibration_file_path_from_sensor << "\n"); - } else { - RCLCPP_ERROR_STREAM(get_logger(), "SaveFileFromString failed:" - << calibration_file_path_from_sensor << "\n"); - } - rt = calibration_configuration.LoadFromString(str); - if (rt == Status::OK) { - RCLCPP_INFO_STREAM(get_logger(), - "LoadFromString success:" << str << "\n"); - } else { - RCLCPP_ERROR_STREAM(get_logger(), - "LoadFromString failed:" << str << "\n"); - } - }); + this->get_logger(), + "Trying to acquire calibration data from sensor: '" << sensor_cfg_ptr_->sensor_ip << "'"); + std::future future = std::async( + std::launch::async, + [this, &calibration_configuration, &calibration_file_path_from_sensor, &run_local]() { + auto str = hw_interface_.GetLidarCalibrationString(); + auto rt = + calibration_configuration.SaveFileFromString(calibration_file_path_from_sensor, str); + RCLCPP_ERROR_STREAM(get_logger(), str); + if (rt == Status::OK) { + RCLCPP_INFO_STREAM( + get_logger(), + "SaveFileFromString success:" << calibration_file_path_from_sensor << "\n"); + } else { + RCLCPP_ERROR_STREAM( + get_logger(), + "SaveFileFromString failed:" << calibration_file_path_from_sensor << "\n"); + } + rt = calibration_configuration.LoadFromString(str); + if (rt == Status::OK) { + RCLCPP_INFO_STREAM(get_logger(), "LoadFromString success:" << str << "\n"); + } else { + RCLCPP_ERROR_STREAM(get_logger(), "LoadFromString failed:" << str << "\n"); + } + }); std::future_status status; status = future.wait_for(std::chrono::milliseconds(5000)); if (status == std::future_status::timeout) { @@ -674,36 +638,38 @@ Status HesaiRosWrapper::GetCalibrationData( run_local = true; } else if (status == std::future_status::ready && !run_local) { RCLCPP_INFO_STREAM( - this->get_logger(), "Acquired calibration data from sensor: '" - << sensor_cfg_ptr_->sensor_ip << "'"); + this->get_logger(), + "Acquired calibration data from sensor: '" << sensor_cfg_ptr_->sensor_ip << "'"); RCLCPP_INFO_STREAM( - this->get_logger(), "The calibration has been saved in '" - << calibration_file_path_from_sensor << "'"); + this->get_logger(), + "The calibration has been saved in '" << calibration_file_path_from_sensor << "'"); } } - if(run_local) { + if (run_local) { RCLCPP_WARN_STREAM(get_logger(), "Running locally"); bool run_org = false; if (calibration_file_path_from_sensor.empty()) { run_org = true; } else { - RCLCPP_INFO_STREAM(get_logger(),"Trying to load file: " << calibration_file_path_from_sensor); - auto cal_status = - calibration_configuration.LoadFromFile(calibration_file_path_from_sensor); + RCLCPP_INFO_STREAM( + get_logger(), "Trying to load file: " << calibration_file_path_from_sensor); + auto cal_status = calibration_configuration.LoadFromFile(calibration_file_path_from_sensor); if (cal_status != Status::OK) { run_org = true; - }else{ + } else { RCLCPP_INFO_STREAM( - this->get_logger(), "Load calibration data from: '" - << calibration_file_path_from_sensor << "'"); + this->get_logger(), + "Load calibration data from: '" << calibration_file_path_from_sensor << "'"); } } - if(run_org) { - RCLCPP_INFO_STREAM(get_logger(),"Trying to load file: " << calibration_configuration.calibration_file); + if (run_org) { + RCLCPP_INFO_STREAM( + get_logger(), "Trying to load file: " << calibration_configuration.calibration_file); if (calibration_configuration.calibration_file.empty()) { RCLCPP_ERROR_STREAM( - this->get_logger(), "Empty Calibration_file File: '" << calibration_configuration.calibration_file << "'"); + this->get_logger(), + "Empty Calibration_file File: '" << calibration_configuration.calibration_file << "'"); return Status::INVALID_CALIBRATION_FILE; } else { auto cal_status = @@ -714,54 +680,61 @@ Status HesaiRosWrapper::GetCalibrationData( this->get_logger(), "Given Calibration File: '" << calibration_configuration.calibration_file << "'"); return cal_status; - }else{ + } else { RCLCPP_INFO_STREAM( - this->get_logger(), "Load calibration data from: '" - << calibration_configuration.calibration_file << "'"); + this->get_logger(), + "Load calibration data from: '" << calibration_configuration.calibration_file << "'"); } } } } - } else { // sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128 + } else { // sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128 std::string correction_file_path_from_sensor; if (launch_hw_ && !correction_file_path.empty()) { int ext_pos = correction_file_path.find_last_of('.'); correction_file_path_from_sensor += correction_file_path.substr(0, ext_pos); correction_file_path_from_sensor += "_from_sensor"; - correction_file_path_from_sensor += correction_file_path.substr(ext_pos, correction_file_path.size() - ext_pos); + correction_file_path_from_sensor += + correction_file_path.substr(ext_pos, correction_file_path.size() - ext_pos); } - std::future future = std::async(std::launch::async, [this, &correction_configuration, &correction_file_path_from_sensor, &run_local]() { - if (launch_hw_) { - RCLCPP_INFO_STREAM( - this->get_logger(), "Trying to acquire calibration data from sensor"); - auto received_bytes = hw_interface_.GetLidarCalibrationBytes(); - RCLCPP_INFO_STREAM(get_logger(), "AT128 calibration size:" << received_bytes.size() << "\n"); - auto rt = correction_configuration.SaveFileFromBinary(correction_file_path_from_sensor, received_bytes); - if(rt == Status::OK) - { - RCLCPP_INFO_STREAM(get_logger(), "SaveFileFromBinary success:" << correction_file_path_from_sensor << "\n"); - } - else - { - RCLCPP_ERROR_STREAM(get_logger(), "SaveFileFromBinary failed:" << correction_file_path_from_sensor << ". Falling back to offline calibration file."); - run_local = true; - } - rt = correction_configuration.LoadFromBinary(received_bytes); - if(rt == Status::OK) - { - RCLCPP_INFO_STREAM(get_logger(), "LoadFromBinary success" << "\n"); - run_local = false; - } - else - { - RCLCPP_ERROR_STREAM(get_logger(), "LoadFromBinary failed" << ". Falling back to offline calibration file."); + std::future future = std::async( + std::launch::async, + [this, &correction_configuration, &correction_file_path_from_sensor, &run_local]() { + if (launch_hw_) { + RCLCPP_INFO_STREAM(this->get_logger(), "Trying to acquire calibration data from sensor"); + auto received_bytes = hw_interface_.GetLidarCalibrationBytes(); + RCLCPP_INFO_STREAM( + get_logger(), "AT128 calibration size:" << received_bytes.size() << "\n"); + auto rt = correction_configuration.SaveFileFromBinary( + correction_file_path_from_sensor, received_bytes); + if (rt == Status::OK) { + RCLCPP_INFO_STREAM( + get_logger(), + "SaveFileFromBinary success:" << correction_file_path_from_sensor << "\n"); + } else { + RCLCPP_ERROR_STREAM( + get_logger(), + "SaveFileFromBinary failed:" << correction_file_path_from_sensor + << ". Falling back to offline calibration file."); + run_local = true; + } + rt = correction_configuration.LoadFromBinary(received_bytes); + if (rt == Status::OK) { + RCLCPP_INFO_STREAM( + get_logger(), "LoadFromBinary success" + << "\n"); + run_local = false; + } else { + RCLCPP_ERROR_STREAM( + get_logger(), "LoadFromBinary failed" + << ". Falling back to offline calibration file."); + run_local = true; + } + } else { + RCLCPP_ERROR_STREAM(get_logger(), "Falling back to offline calibration file."); run_local = true; } - }else{ - RCLCPP_ERROR_STREAM(get_logger(), "Falling back to offline calibration file."); - run_local = true; - } - }); + }); if (!run_local) { std::future_status status; status = future.wait_for(std::chrono::milliseconds(8000)); @@ -770,30 +743,29 @@ Status HesaiRosWrapper::GetCalibrationData( run_local = true; } else if (status == std::future_status::ready && !run_local) { RCLCPP_INFO_STREAM( - this->get_logger(), "Acquired correction data from sensor: '" - << sensor_cfg_ptr_->sensor_ip << "'"); + this->get_logger(), + "Acquired correction data from sensor: '" << sensor_cfg_ptr_->sensor_ip << "'"); RCLCPP_INFO_STREAM( - this->get_logger(), "The correction has been saved in '" - << correction_file_path_from_sensor << "'"); + this->get_logger(), + "The correction has been saved in '" << correction_file_path_from_sensor << "'"); } } - if(run_local) { + if (run_local) { bool run_org = false; if (correction_file_path_from_sensor.empty()) { run_org = true; } else { - auto cal_status = - correction_configuration.LoadFromFile(correction_file_path_from_sensor); + auto cal_status = correction_configuration.LoadFromFile(correction_file_path_from_sensor); if (cal_status != Status::OK) { run_org = true; - }else{ + } else { RCLCPP_INFO_STREAM( - this->get_logger(), "Load correction data from: '" - << correction_file_path_from_sensor << "'"); + this->get_logger(), + "Load correction data from: '" << correction_file_path_from_sensor << "'"); } } - if(run_org) { + if (run_org) { if (correction_file_path.empty()) { RCLCPP_ERROR_STREAM( this->get_logger(), "Empty Correction File: '" << correction_file_path << "'"); @@ -805,20 +777,20 @@ Status HesaiRosWrapper::GetCalibrationData( RCLCPP_ERROR_STREAM( this->get_logger(), "Given Correction File: '" << correction_file_path << "'"); return cal_status; - }else{ + } else { RCLCPP_INFO_STREAM( - this->get_logger(), "Load correction data from: '" - << correction_file_path << "'"); + this->get_logger(), "Load correction data from: '" << correction_file_path << "'"); } } } } - } // end AT128 + } // end AT128 return Status::OK; - } +} -HesaiRosWrapper::~HesaiRosWrapper() { +HesaiRosWrapper::~HesaiRosWrapper() +{ RCLCPP_INFO_STREAM(get_logger(), "Closing TcpDriver"); hw_interface_.FinalizeTcpDriver(); } @@ -831,8 +803,14 @@ Status HesaiRosWrapper::StreamStart() return interface_status_; } -Status HesaiRosWrapper::StreamStop() { return Status::OK; } -Status HesaiRosWrapper::Shutdown() { return Status::OK; } +Status HesaiRosWrapper::StreamStop() +{ + return Status::OK; +} +Status HesaiRosWrapper::Shutdown() +{ + return Status::OK; +} Status HesaiRosWrapper::InitializeHwInterface( // todo: don't think this is needed const drivers::SensorConfigurationBase & sensor_configuration) @@ -844,8 +822,63 @@ Status HesaiRosWrapper::InitializeHwInterface( // todo: don't think this is nee } Status HesaiRosWrapper::InitializeHwMonitor( // todo: don't think this is needed - const drivers::SensorConfigurationBase & sensor_configuration) + const drivers::SensorConfigurationBase & /* sensor_configuration */) { + cbg_r_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant); + cbg_m_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + cbg_m2_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + if (Status::OK != interface_status_) { + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); + return Status::ERROR_1; + } + + message_sep = ": "; + not_supported_message = "Not supported"; + error_message = "Error"; + + switch (sensor_cfg_ptr_->sensor_model) { + case nebula::drivers::SensorModel::HESAI_PANDARXT32: + case nebula::drivers::SensorModel::HESAI_PANDARXT32M: + case nebula::drivers::SensorModel::HESAI_PANDARAT128: + temperature_names.emplace_back("Bottom circuit board T1"); + temperature_names.emplace_back("Bottom circuit board T2"); + temperature_names.emplace_back("Laser emitting board RT_L1(Internal)"); + temperature_names.emplace_back("Laser emitting board RT_L2"); + temperature_names.emplace_back("Receiving board RT_R"); + temperature_names.emplace_back("Receiving board RT2"); + temperature_names.emplace_back("Top circuit RT3"); + temperature_names.emplace_back("Not used"); + break; + case nebula::drivers::SensorModel::HESAI_PANDAR64: + case nebula::drivers::SensorModel::HESAI_PANDAR40P: + case nebula::drivers::SensorModel::HESAI_PANDAR40M: + case nebula::drivers::SensorModel::HESAI_PANDARQT64: + case nebula::drivers::SensorModel::HESAI_PANDARQT128: + case nebula::drivers::SensorModel::HESAI_PANDAR128_E3X: + case nebula::drivers::SensorModel::HESAI_PANDAR128_E4X: + default: + temperature_names.emplace_back("Bottom circuit RT1"); + temperature_names.emplace_back("Bottom circuit RT2"); + temperature_names.emplace_back("Internal Temperature"); + temperature_names.emplace_back("Laser emitting board RT1"); + temperature_names.emplace_back("Laser emitting board RT2"); + temperature_names.emplace_back("Receiving board RT1"); + temperature_names.emplace_back("Top circuit RT1"); + temperature_names.emplace_back("Top circuit RT2"); + break; + } + + auto result = hw_interface_.GetInventory(); + current_inventory.reset(new HesaiInventory(result)); + current_inventory_time.reset(new rclcpp::Time(this->get_clock()->now())); + std::cout << "HesaiInventory" << std::endl; + std::cout << result << std::endl; + info_model = result.get_str_model(); + info_serial = std::string(result.sn.begin(), result.sn.end()); + hw_interface_.SetTargetModel(result.model); + RCLCPP_INFO_STREAM(this->get_logger(), "Model:" << info_model); + RCLCPP_INFO_STREAM(this->get_logger(), "Serial:" << info_serial); + InitializeHesaiDiagnostics(); return Status::OK; } @@ -858,16 +891,14 @@ rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::paramCallback( RCLCPP_DEBUG_STREAM(this->get_logger(), *sensor_cfg_ptr_); RCLCPP_INFO_STREAM(this->get_logger(), p); - std::shared_ptr new_param = + std::shared_ptr new_param = std::make_shared(*sensor_cfg_ptr_); RCLCPP_INFO_STREAM(this->get_logger(), new_param); std::string sensor_model_str; std::string return_mode_str; if ( - get_param(p, "sensor_model", sensor_model_str) | - get_param(p, "return_mode", return_mode_str) | - get_param(p, "host_ip", new_param->host_ip) | - get_param(p, "sensor_ip", new_param->sensor_ip) | + get_param(p, "sensor_model", sensor_model_str) | get_param(p, "return_mode", return_mode_str) | + get_param(p, "host_ip", new_param->host_ip) | get_param(p, "sensor_ip", new_param->sensor_ip) | get_param(p, "frame_id", new_param->frame_id) | get_param(p, "data_port", new_param->data_port) | get_param(p, "gnss_port", new_param->gnss_port) | @@ -938,8 +969,7 @@ void HesaiRosWrapper::InitializeHesaiDiagnostics() diagnostics_updater_.add("hesai_status", this, &HesaiRosWrapper::HesaiCheckStatus); diagnostics_updater_.add("hesai_ptp", this, &HesaiRosWrapper::HesaiCheckPtp); - diagnostics_updater_.add( - "hesai_temperature", this, &HesaiRosWrapper::HesaiCheckTemperature); + diagnostics_updater_.add("hesai_temperature", this, &HesaiRosWrapper::HesaiCheckTemperature); diagnostics_updater_.add("hesai_rpm", this, &HesaiRosWrapper::HesaiCheckRpm); current_status.reset(); @@ -964,8 +994,7 @@ void HesaiRosWrapper::InitializeHesaiDiagnostics() this->get_node_timers_interface()->add_timer(fetch_diagnostics_timer_, cbg_m_); if (hw_interface_.UseHttpGetLidarMonitor()) { - diagnostics_updater_.add( - "hesai_voltage", this, &HesaiRosWrapper::HesaiCheckVoltageHttp); + diagnostics_updater_.add("hesai_voltage", this, &HesaiRosWrapper::HesaiCheckVoltageHttp); } else { diagnostics_updater_.add("hesai_voltage", this, &HesaiRosWrapper::HesaiCheckVoltage); } @@ -1030,12 +1059,10 @@ void HesaiRosWrapper::OnHesaiStatusTimer() current_status.reset(new HesaiLidarStatus(result)); } catch (const std::system_error & error) { RCLCPP_ERROR_STREAM( - rclcpp::get_logger("HesaiRosWrapper::OnHesaiStatusTimer(std::system_error)"), - error.what()); + rclcpp::get_logger("HesaiRosWrapper::OnHesaiStatusTimer(std::system_error)"), error.what()); } catch (const boost::system::system_error & error) { RCLCPP_ERROR_STREAM( - rclcpp::get_logger( - "HesaiRosWrapper::OnHesaiStatusTimer(boost::system::system_error)"), + rclcpp::get_logger("HesaiRosWrapper::OnHesaiStatusTimer(boost::system::system_error)"), error.what()); } RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiStatusTimer END" << std::endl); @@ -1053,8 +1080,7 @@ void HesaiRosWrapper::OnHesaiLidarMonitorTimerHttp() }); } catch (const std::system_error & error) { RCLCPP_ERROR_STREAM( - rclcpp::get_logger( - "HesaiRosWrapper::OnHesaiLidarMonitorTimerHttp(std::system_error)"), + rclcpp::get_logger("HesaiRosWrapper::OnHesaiLidarMonitorTimerHttp(std::system_error)"), error.what()); } catch (const boost::system::system_error & error) { RCLCPP_ERROR_STREAM( @@ -1080,15 +1106,13 @@ void HesaiRosWrapper::OnHesaiLidarMonitorTimer() error.what()); } catch (const boost::system::system_error & error) { RCLCPP_ERROR_STREAM( - rclcpp::get_logger( - "HesaiRosWrapper::OnHesaiLidarMonitorTimer(boost::system::system_error)"), + rclcpp::get_logger("HesaiRosWrapper::OnHesaiLidarMonitorTimer(boost::system::system_error)"), error.what()); } RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiLidarMonitorTimer END"); } -void HesaiRosWrapper::HesaiCheckStatus( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +void HesaiRosWrapper::HesaiCheckStatus(diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { std::scoped_lock lock(mtx_status); if (current_status) { @@ -1105,8 +1129,7 @@ void HesaiRosWrapper::HesaiCheckStatus( } } -void HesaiRosWrapper::HesaiCheckPtp( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +void HesaiRosWrapper::HesaiCheckPtp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { std::scoped_lock lock(mtx_status); if (current_status) { @@ -1156,8 +1179,7 @@ void HesaiRosWrapper::HesaiCheckTemperature( } } -void HesaiRosWrapper::HesaiCheckRpm( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +void HesaiRosWrapper::HesaiCheckRpm(diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { std::scoped_lock lock(mtx_status); if (current_status) { @@ -1204,8 +1226,7 @@ void HesaiRosWrapper::HesaiCheckVoltageHttp( } } -void HesaiRosWrapper::HesaiCheckVoltage( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +void HesaiRosWrapper::HesaiCheckVoltage(diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { std::scoped_lock lock(mtx_lidar_monitor); if (current_monitor) { @@ -1224,6 +1245,6 @@ void HesaiRosWrapper::HesaiCheckVoltage( } } - RCLCPP_COMPONENTS_REGISTER_NODE(HesaiRosWrapper) +RCLCPP_COMPONENTS_REGISTER_NODE(HesaiRosWrapper) } // namespace ros } // namespace nebula diff --git a/scripts/plot_times.py b/scripts/plot_times.py index 521dfd347..3cbf93f61 100644 --- a/scripts/plot_times.py +++ b/scripts/plot_times.py @@ -24,7 +24,6 @@ def parse_logs(run_name): for col in [c for c in df.columns if c.startswith("d_")]: df[col] /= 1e6 # ns to ms - df['d_total'] = sum([df[c] for c in df.columns if c.startswith("d_")]) # type: ignore return df def plot_timing_comparison(run_names): @@ -41,11 +40,13 @@ def plot_timing_comparison(run_names): boxes = axs[1:] for i, (label, df) in enumerate(scenario_dfs.items()): - durations = df['d_total'] + # durations = df['d_total'] - ax_d.plot(durations.index, durations, label=label, linestyle='', marker='.') + # ax_d.plot(durations.index, durations, label=label, linestyle='', marker='.') for col in filter(lambda col: col.startswith("n_"), df.columns): ax_n.plot(df.index, df[col], label=f"{label}::{col}", linestyle='', marker='.', color='black') + for col in filter(lambda col: col.startswith("d_"), df.columns): + ax_d.plot(df.index, df[col], label=f"{label}::{col}", linestyle='', marker='.') d_columns = [col for col in df.columns if col.startswith("d_")] n_cols = len(d_columns) From be47714b35579a0d25daa624cdfb57d828b5e4d4 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 26 Mar 2024 22:02:51 +0900 Subject: [PATCH 05/57] temporary progress --- nebula_messages/nebula_msgs/CMakeLists.txt | 2 +- .../nebula_msgs/msg/NebulaPacket.msg | 2 +- nebula_messages/nebula_msgs/package.xml | 2 +- .../nebula_ros/hesai/hesai_ros_wrapper.hpp | 6 +- nebula_ros/launch/hesai_launch_all_hw.xml | 59 ++++---- nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 134 +++++------------- 6 files changed, 78 insertions(+), 127 deletions(-) diff --git a/nebula_messages/nebula_msgs/CMakeLists.txt b/nebula_messages/nebula_msgs/CMakeLists.txt index 2ffa068a5..e2b798d9d 100644 --- a/nebula_messages/nebula_msgs/CMakeLists.txt +++ b/nebula_messages/nebula_msgs/CMakeLists.txt @@ -21,4 +21,4 @@ rosidl_generate_interfaces(${PROJECT_NAME} std_msgs ) -ament_package() +ament_package() \ No newline at end of file diff --git a/nebula_messages/nebula_msgs/msg/NebulaPacket.msg b/nebula_messages/nebula_msgs/msg/NebulaPacket.msg index f6ba285d8..be5898eb1 100644 --- a/nebula_messages/nebula_msgs/msg/NebulaPacket.msg +++ b/nebula_messages/nebula_msgs/msg/NebulaPacket.msg @@ -1,2 +1,2 @@ builtin_interfaces/Time stamp -uint8[] data +uint8[] data \ No newline at end of file diff --git a/nebula_messages/nebula_msgs/package.xml b/nebula_messages/nebula_msgs/package.xml index cdd135c37..3eb05e603 100644 --- a/nebula_messages/nebula_msgs/package.xml +++ b/nebula_messages/nebula_msgs/package.xml @@ -22,4 +22,4 @@ ament_cmake - + \ No newline at end of file diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp index e91d7fcaf..0681f9455 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp @@ -125,10 +125,14 @@ class HesaiRosWrapper final : public rclcpp::Node, NebulaRosWrapperBase /// @return Resulting status Status InitializeHwInterface( const drivers::SensorConfigurationBase & sensor_configuration) override; - /// @brief Callback for receiving PandarScan + /// @brief Callback for receiving a raw UDP packet /// @param scan_buffer Received PandarScan void ReceiveCloudPacketCallback(const std::vector & scan_buffer); + /// @brief Decodes a nebula packet and, if it completes the scan, publishes the pointcloud. + /// @param packet_msg The received packet message + void ProcessCloudPacket(std::unique_ptr packet_msg); + /// @brief rclcpp parameter callback /// @param parameters Received parameters /// @return SetParametersResult diff --git a/nebula_ros/launch/hesai_launch_all_hw.xml b/nebula_ros/launch/hesai_launch_all_hw.xml index a74eb9813..b7c1459ec 100644 --- a/nebula_ros/launch/hesai_launch_all_hw.xml +++ b/nebula_ros/launch/hesai_launch_all_hw.xml @@ -33,32 +33,35 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index ce81d6b3e..8957a30d2 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -86,94 +86,31 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) std::static_pointer_cast(calibration_cfg_ptr_), std::static_pointer_cast(correction_cfg_ptr_)); - RCLCPP_DEBUG(this->get_logger(), "Starting stream"); - StreamStart(); - hw_interface_.RegisterScanCallback( - std::bind(&HesaiRosWrapper::ReceiveCloudPacketCallback, this, std::placeholders::_1)); - - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Wrapper=" << wrapper_status_); - rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; - auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), - qos_profile); - - packet_pub_ = create_publisher( - "hesai_packets", rclcpp::SensorDataQoS()); - - packet_sub_ = create_subscription( - "hesai_packets", rclcpp::SensorDataQoS(), [](nebula_msgs::msg::NebulaPacket::UniquePtr){}); - - nebula_points_pub_ = - this->create_publisher("pandar_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()); -} - -//hwmon -{ -cbg_r_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant); - cbg_m_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - cbg_m2_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - if (Status::OK != interface_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); - return; + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Wrapper=" << wrapper_status_); + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), + qos_profile); + + packet_pub_ = create_publisher( + "hesai_packets", rclcpp::SensorDataQoS()); + + packet_sub_ = create_subscription( + "hesai_packets", rclcpp::SensorDataQoS(), std::bind(&HesaiRosWrapper::ProcessCloudPacket, this, std::placeholders::_1)); + + nebula_points_pub_ = this->create_publisher( + "pandar_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()); } - std::this_thread::sleep_for(std::chrono::milliseconds(this->delay_monitor_ms_)); - message_sep = ": "; - not_supported_message = "Not supported"; - error_message = "Error"; + RCLCPP_DEBUG(this->get_logger(), "Starting stream"); + StreamStart(); + hw_interface_.RegisterScanCallback( + std::bind(&HesaiRosWrapper::ReceiveCloudPacketCallback, this, std::placeholders::_1)); - switch (sensor_cfg_ptr_->sensor_model) { - case nebula::drivers::SensorModel::HESAI_PANDARXT32: - case nebula::drivers::SensorModel::HESAI_PANDARXT32M: - case nebula::drivers::SensorModel::HESAI_PANDARAT128: - temperature_names.emplace_back("Bottom circuit board T1"); - temperature_names.emplace_back("Bottom circuit board T2"); - temperature_names.emplace_back("Laser emitting board RT_L1(Internal)"); - temperature_names.emplace_back("Laser emitting board RT_L2"); - temperature_names.emplace_back("Receiving board RT_R"); - temperature_names.emplace_back("Receiving board RT2"); - temperature_names.emplace_back("Top circuit RT3"); - temperature_names.emplace_back("Not used"); - break; - case nebula::drivers::SensorModel::HESAI_PANDAR64: - case nebula::drivers::SensorModel::HESAI_PANDAR40P: - case nebula::drivers::SensorModel::HESAI_PANDAR40M: - case nebula::drivers::SensorModel::HESAI_PANDARQT64: - case nebula::drivers::SensorModel::HESAI_PANDARQT128: - case nebula::drivers::SensorModel::HESAI_PANDAR128_E3X: - case nebula::drivers::SensorModel::HESAI_PANDAR128_E4X: - default: - temperature_names.emplace_back("Bottom circuit RT1"); - temperature_names.emplace_back("Bottom circuit RT2"); - temperature_names.emplace_back("Internal Temperature"); - temperature_names.emplace_back("Laser emitting board RT1"); - temperature_names.emplace_back("Laser emitting board RT2"); - temperature_names.emplace_back("Receiving board RT1"); - temperature_names.emplace_back("Top circuit RT1"); - temperature_names.emplace_back("Top circuit RT2"); - break; - } - - std::vector thread_pool{}; - thread_pool.emplace_back([this] { - auto result = hw_interface_.GetInventory(); - current_inventory.reset(new HesaiInventory(result)); - current_inventory_time.reset(new rclcpp::Time(this->get_clock()->now())); - RCLCPP_INFO_STREAM(this->get_logger(), "HesaiInventory"); - RCLCPP_INFO_STREAM(this->get_logger(), result); - info_model = result.get_str_model(); - info_serial = std::string(result.sn.begin(), result.sn.end()); - hw_interface_.SetTargetModel(result.model); - RCLCPP_INFO_STREAM(this->get_logger(), "Model:" << info_model); - RCLCPP_INFO_STREAM(this->get_logger(), "Serial:" << info_serial); - InitializeHesaiDiagnostics(); - }); - for (std::thread & th : thread_pool) { - th.join(); - } + InitializeHwMonitor(*sensor_cfg_ptr_); set_param_res_ = this->add_on_set_parameters_callback( std::bind(&HesaiRosWrapper::paramCallback, this, std::placeholders::_1)); @@ -181,23 +118,30 @@ cbg_r_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant); void HesaiRosWrapper::ReceiveCloudPacketCallback(const std::vector & packet) { - auto t_received = std::chrono::high_resolution_clock::now().time_since_epoch().count(); - auto msg_ptr = std::make_unique(packet.size()); - msg_ptr->stamp.sec = t_received / 1'000'000'000; - msg_ptr->stamp.nanosec = t_received % 1'000'000'000; - // TODO(mojomex) this copy could be avoided if transport_drivers would give us a non-const vector - std::copy(packet.begin(), packet.end(), msg_ptr->data.begin()); - packet_pub_->publish(std::move(msg_ptr)); - - //todo // Driver is not initialized yet if (!driver_ptr_) { return; } + const auto now = std::chrono::system_clock::now(); + const auto timestamp_ns = + std::chrono::duration_cast(now.time_since_epoch()).count(); + + auto msg_ptr = std::make_unique(); + msg_ptr->stamp.sec = static_cast(timestamp_ns / 1'000'000'000); + msg_ptr->stamp.nanosec = static_cast(timestamp_ns % 1'000'000'000); + std::copy(packet.begin(), packet.end(), std::back_inserter(msg_ptr->data)); + + packet_pub_->publish(std::move(msg_ptr)); +} + +void HesaiRosWrapper::ProcessCloudPacket(std::unique_ptr packet_msg) +{ + std::lock_guard lock(mtx_decode_); + auto t_start = std::chrono::high_resolution_clock::now(); std::tuple pointcloud_ts = - driver_ptr_->ParseCloudPacket(packet); + driver_ptr_->ParseCloudPacket(packet_msg->data); nebula::drivers::NebulaPointCloudPtr pointcloud = std::get<0>(pointcloud_ts); auto t_end = std::chrono::high_resolution_clock::now(); From 17c8163ed943bbf089fe121738b5f1c62f37fff1 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 4 Apr 2024 19:35:25 +0900 Subject: [PATCH 06/57] fix(hesai_ros_wrapper): remove changes wrongly merged during rebase --- nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index 8957a30d2..9aef30110 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -91,6 +91,8 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); + //TODO(mojomex): create high-frequency QoS with large buffer to prevent packet loss + packet_pub_ = create_publisher( "hesai_packets", rclcpp::SensorDataQoS()); @@ -137,8 +139,6 @@ void HesaiRosWrapper::ReceiveCloudPacketCallback(const std::vector & pa void HesaiRosWrapper::ProcessCloudPacket(std::unique_ptr packet_msg) { - std::lock_guard lock(mtx_decode_); - auto t_start = std::chrono::high_resolution_clock::now(); std::tuple pointcloud_ts = driver_ptr_->ParseCloudPacket(packet_msg->data); @@ -742,7 +742,7 @@ HesaiRosWrapper::~HesaiRosWrapper() Status HesaiRosWrapper::StreamStart() { if (Status::OK == interface_status_) { - interface_status_ = hw_interface_.CloudInterfaceStart(); + interface_status_ = hw_interface_.SensorInterfaceStart(); } return interface_status_; } From c7dcab93dff1770a534d391b6caf8b140c3a4ce0 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 4 Apr 2024 20:16:36 +0900 Subject: [PATCH 07/57] fix(hesai_ros_wrapper): increase packet buffering to stop internal packet loss --- nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 43 +++++++++++----------- 1 file changed, 21 insertions(+), 22 deletions(-) diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index 9aef30110..15f7eca09 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -88,23 +88,23 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Wrapper=" << wrapper_status_); rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; - auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), - qos_profile); + auto pointcloud_qos = + rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); - //TODO(mojomex): create high-frequency QoS with large buffer to prevent packet loss - - packet_pub_ = create_publisher( - "hesai_packets", rclcpp::SensorDataQoS()); + auto packet_qos = + rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 1000), qos_profile); + packet_pub_ = create_publisher("hesai_packets", packet_qos); packet_sub_ = create_subscription( - "hesai_packets", rclcpp::SensorDataQoS(), std::bind(&HesaiRosWrapper::ProcessCloudPacket, this, std::placeholders::_1)); + "hesai_packets", packet_qos, + std::bind(&HesaiRosWrapper::ProcessCloudPacket, this, std::placeholders::_1)); - nebula_points_pub_ = this->create_publisher( - "pandar_points", rclcpp::SensorDataQoS()); + nebula_points_pub_ = + this->create_publisher("pandar_points", pointcloud_qos); aw_points_base_pub_ = - this->create_publisher("aw_points", rclcpp::SensorDataQoS()); - aw_points_ex_pub_ = this->create_publisher( - "aw_points_ex", rclcpp::SensorDataQoS()); + this->create_publisher("aw_points", pointcloud_qos); + aw_points_ex_pub_ = + this->create_publisher("aw_points_ex", pointcloud_qos); } RCLCPP_DEBUG(this->get_logger(), "Starting stream"); @@ -664,14 +664,12 @@ Status HesaiRosWrapper::GetCalibrationData( } rt = correction_configuration.LoadFromBinary(received_bytes); if (rt == Status::OK) { - RCLCPP_INFO_STREAM( - get_logger(), "LoadFromBinary success" - << "\n"); + RCLCPP_INFO_STREAM(get_logger(), "LoadFromBinary success" << "\n"); run_local = false; } else { RCLCPP_ERROR_STREAM( - get_logger(), "LoadFromBinary failed" - << ". Falling back to offline calibration file."); + get_logger(), + "LoadFromBinary failed" << ". Falling back to offline calibration file."); run_local = true; } } else { @@ -923,8 +921,8 @@ void HesaiRosWrapper::InitializeHesaiDiagnostics() current_diag_status = diagnostic_msgs::msg::DiagnosticStatus::STALE; current_monitor_status = diagnostic_msgs::msg::DiagnosticStatus::STALE; - auto fetch_diag_from_sensor = [this](){ - OnHesaiStatusTimer(); + auto fetch_diag_from_sensor = [this]() { + OnHesaiStatusTimer(); if (hw_interface_.UseHttpGetLidarMonitor()) { OnHesaiLidarMonitorTimerHttp(); } else { @@ -932,9 +930,10 @@ void HesaiRosWrapper::InitializeHesaiDiagnostics() } }; - fetch_diagnostics_timer_ = std::make_shared>( - this->get_clock(), std::chrono::milliseconds(diag_span_), std::move(fetch_diag_from_sensor), - this->get_node_base_interface()->get_context()); + fetch_diagnostics_timer_ = + std::make_shared>( + this->get_clock(), std::chrono::milliseconds(diag_span_), std::move(fetch_diag_from_sensor), + this->get_node_base_interface()->get_context()); this->get_node_timers_interface()->add_timer(fetch_diagnostics_timer_, cbg_m_); if (hw_interface_.UseHttpGetLidarMonitor()) { From 991100dbc0f403f377bb3eac382f546323007442 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 4 Apr 2024 20:17:59 +0900 Subject: [PATCH 08/57] feat(nebula_common): add instrumentation tools --- .../nebula_common/util/instrumentation.hpp | 109 ++++++++++++++++++ 1 file changed, 109 insertions(+) create mode 100644 nebula_common/include/nebula_common/util/instrumentation.hpp diff --git a/nebula_common/include/nebula_common/util/instrumentation.hpp b/nebula_common/include/nebula_common/util/instrumentation.hpp new file mode 100644 index 000000000..ab9357286 --- /dev/null +++ b/nebula_common/include/nebula_common/util/instrumentation.hpp @@ -0,0 +1,109 @@ +#pragma once + +#include +#include +#include + +namespace nebula +{ +namespace util +{ + +using namespace std::chrono_literals; + +class Instrumentation +{ + using clock = std::chrono::high_resolution_clock; + using time_point = clock::time_point; + using duration = std::chrono::nanoseconds; + +public: + Instrumentation(std::string tag) : tag(tag), last_print(clock::now()) {} + + void tick() + { + auto now = clock::now(); + + if (last_tick != time_point::min()) { + auto rtt = now - last_tick; + rtts_.update(rtt); + } + + last_tick = now; + } + + void tock() + { + auto now = clock::now(); + auto delay = now - last_tick; + delays_.update(delay); + + if (now - last_print >= PRINT_INTERVAL) { + auto del = delays_.reset(); + auto rtt = rtts_.reset(); + + last_print = now; + + std::cout << "{\"tag\": \"" << tag << "\", \"del_min\": " << del.min.count() * 1e-9 + << ", \"del_avg\": " << del.avg.count() * 1e-9 + << ", \"del_max\": " << del.max.count() * 1e-9 + << ", \"rtt_min\": " << rtt.min.count() * 1e-9 + << ", \"rtt_avg\": " << rtt.avg.count() * 1e-9 + << ", \"rtt_max\": " << rtt.max.count() * 1e-9 << ", \"window\": " << rtt.count + << '}' << std::endl; + } + } + +private: + class Counter + { + public: + struct Stats + { + duration min; + duration avg; + duration max; + uint64_t count; + }; + + Stats reset() + { + auto avg_measurement = sum_measurements / num_measurements; + + Stats result{min_measurement, avg_measurement, max_measurement, num_measurements}; + + sum_measurements = duration::zero(); + num_measurements = 0; + min_measurement = duration::zero(); + max_measurement = duration::max(); + } + + void update(duration measurement) + { + sum_measurements += measurement; + num_measurements++; + + min_measurement = std::min(min_measurement, measurement); + max_measurement = std::max(max_measurement, measurement); + } + + private: + duration sum_measurements{duration::zero()}; + uint64_t num_measurements{0}; + duration min_measurement{duration::max()}; + duration max_measurement{duration::zero()}; + }; + + std::string tag; + + time_point last_tick{time_point::min()}; + time_point last_print{time_point::min()}; + + Counter delays_; + Counter rtts_; + + const duration PRINT_INTERVAL = 1s; +}; + +} // namespace util +} // namespace nebula \ No newline at end of file From cbaccee7dcd82a4ecce10e8279c1f5ea9b6e4082 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 4 Apr 2024 20:18:20 +0900 Subject: [PATCH 09/57] feat(hesai_ros_wrapper): instrument code on critical path --- .../nebula_ros/hesai/hesai_ros_wrapper.hpp | 1 + nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 37 +++++++++++++------ 2 files changed, 27 insertions(+), 11 deletions(-) diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp index 0681f9455..781b64198 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp @@ -4,6 +4,7 @@ #include "nebula_common/hesai/hesai_common.hpp" #include "nebula_common/nebula_common.hpp" #include "nebula_common/nebula_status.hpp" +#include "nebula_common/util/instrumentation.hpp" #include "nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" #include "nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp" diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index 15f7eca09..1835e1de6 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -120,11 +120,14 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) void HesaiRosWrapper::ReceiveCloudPacketCallback(const std::vector & packet) { + static nebula::util::Instrumentation convert{"ReceiveCloudPacketCallback.convert"}; + static nebula::util::Instrumentation publish{"ReceiveCloudPacketCallback.publish"}; // Driver is not initialized yet if (!driver_ptr_) { return; } + convert.tick(); const auto now = std::chrono::system_clock::now(); const auto timestamp_ns = std::chrono::duration_cast(now.time_since_epoch()).count(); @@ -133,22 +136,24 @@ void HesaiRosWrapper::ReceiveCloudPacketCallback(const std::vector & pa msg_ptr->stamp.sec = static_cast(timestamp_ns / 1'000'000'000); msg_ptr->stamp.nanosec = static_cast(timestamp_ns % 1'000'000'000); std::copy(packet.begin(), packet.end(), std::back_inserter(msg_ptr->data)); + convert.tock(); + publish.tick(); packet_pub_->publish(std::move(msg_ptr)); + publish.tock(); } void HesaiRosWrapper::ProcessCloudPacket(std::unique_ptr packet_msg) { - auto t_start = std::chrono::high_resolution_clock::now(); + static nebula::util::Instrumentation parse{"ProcessCloudPacket.parse"}; + static nebula::util::Instrumentation convert{"ProcessCloudPacket.convert"}; + static nebula::util::Instrumentation publish{"ProcessCloudPacket.publish"}; + + parse.tick(); std::tuple pointcloud_ts = driver_ptr_->ParseCloudPacket(packet_msg->data); nebula::drivers::NebulaPointCloudPtr pointcloud = std::get<0>(pointcloud_ts); - - auto t_end = std::chrono::high_resolution_clock::now(); - auto runtime = t_end - t_start; - t_start = t_end; - RCLCPP_DEBUG( - get_logger(), "PROFILING {'d_decode_packet': %lu}", runtime.count()); + parse.tock(); if (pointcloud == nullptr) { // todo @@ -158,38 +163,48 @@ void HesaiRosWrapper::ProcessCloudPacket(std::unique_ptrget_subscription_count() > 0 || nebula_points_pub_->get_intra_process_subscription_count() > 0) { + convert.tick(); 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()); + convert.tock(); + publish.tick(); PublishCloud(std::move(ros_pc_msg_ptr), nebula_points_pub_); + publish.tock(); } if ( aw_points_base_pub_->get_subscription_count() > 0 || aw_points_base_pub_->get_intra_process_subscription_count() > 0) { + convert.tick(); + 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()); + convert.tock(); + publish.tick(); PublishCloud(std::move(ros_pc_msg_ptr), aw_points_base_pub_); + publish.tock(); } if ( aw_points_ex_pub_->get_subscription_count() > 0 || aw_points_ex_pub_->get_intra_process_subscription_count() > 0) { + convert.tick(); + 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()); + convert.tock(); + publish.tick(); PublishCloud(std::move(ros_pc_msg_ptr), aw_points_ex_pub_); + publish.tock(); } - - runtime = std::chrono::high_resolution_clock::now() - t_start; - RCLCPP_DEBUG( - get_logger(), "PROFILING {'d_convert_pc': %lu, 'n_out': %lu}", runtime.count(), pointcloud->size()); } void HesaiRosWrapper::PublishCloud( From 5db3474bf27cfcc9c1c8fabe9821cc363fdea8a1 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Fri, 5 Apr 2024 17:18:57 +0900 Subject: [PATCH 10/57] disable instrumentation --- .../nebula_ros/hesai/hesai_ros_wrapper.hpp | 3 +- nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 55 ++++++++++--------- 2 files changed, 32 insertions(+), 26 deletions(-) diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp index 781b64198..149232e98 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp @@ -4,7 +4,8 @@ #include "nebula_common/hesai/hesai_common.hpp" #include "nebula_common/nebula_common.hpp" #include "nebula_common/nebula_status.hpp" -#include "nebula_common/util/instrumentation.hpp" +// #include "nebula_common/util/instrumentation.hpp" +// #include "nebula_common/util/topic_delay.hpp" #include "nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" #include "nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp" diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index 1835e1de6..533c3cf49 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -120,14 +120,15 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) void HesaiRosWrapper::ReceiveCloudPacketCallback(const std::vector & packet) { - static nebula::util::Instrumentation convert{"ReceiveCloudPacketCallback.convert"}; - static nebula::util::Instrumentation publish{"ReceiveCloudPacketCallback.publish"}; + // static auto convert = nebula::util::Instrumentation("ReceiveCloudPacketCallback.convert"); + // static auto publish = nebula::util::Instrumentation("ReceiveCloudPacketCallback.publish"); + // static auto delay = nebula::util::TopicDelay("ReceiveCloudPacketCallback"); // Driver is not initialized yet if (!driver_ptr_) { return; } - convert.tick(); + // convert.tick(); const auto now = std::chrono::system_clock::now(); const auto timestamp_ns = std::chrono::duration_cast(now.time_since_epoch()).count(); @@ -135,25 +136,29 @@ void HesaiRosWrapper::ReceiveCloudPacketCallback(const std::vector & pa auto msg_ptr = std::make_unique(); msg_ptr->stamp.sec = static_cast(timestamp_ns / 1'000'000'000); msg_ptr->stamp.nanosec = static_cast(timestamp_ns % 1'000'000'000); - std::copy(packet.begin(), packet.end(), std::back_inserter(msg_ptr->data)); - convert.tock(); + msg_ptr->data.swap(packet); + // convert.tock(); - publish.tick(); - packet_pub_->publish(std::move(msg_ptr)); - publish.tock(); + // delay.tick(msg_ptr->stamp); + + // publish.tick(); + // publish.tock(); } void HesaiRosWrapper::ProcessCloudPacket(std::unique_ptr packet_msg) { - static nebula::util::Instrumentation parse{"ProcessCloudPacket.parse"}; - static nebula::util::Instrumentation convert{"ProcessCloudPacket.convert"}; - static nebula::util::Instrumentation publish{"ProcessCloudPacket.publish"}; + // static auto parse = nebula::util::Instrumentation("ProcessCloudPacket.parse"); + // static auto convert = nebula::util::Instrumentation("ProcessCloudPacket.convert"); + // static auto publish = nebula::util::Instrumentation("ProcessCloudPacket.publish"); + // static auto delay = nebula::util::TopicDelay("ProcessCloudPacket"); + + // delay.tick(packet_msg->stamp); - parse.tick(); + // parse.tick(); std::tuple pointcloud_ts = driver_ptr_->ParseCloudPacket(packet_msg->data); nebula::drivers::NebulaPointCloudPtr pointcloud = std::get<0>(pointcloud_ts); - parse.tock(); + // parse.tock(); if (pointcloud == nullptr) { // todo @@ -163,20 +168,20 @@ void HesaiRosWrapper::ProcessCloudPacket(std::unique_ptrget_subscription_count() > 0 || nebula_points_pub_->get_intra_process_subscription_count() > 0) { - convert.tick(); + // convert.tick(); 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()); - convert.tock(); - publish.tick(); + // convert.tock(); + // publish.tick(); PublishCloud(std::move(ros_pc_msg_ptr), nebula_points_pub_); - publish.tock(); + // publish.tock(); } if ( aw_points_base_pub_->get_subscription_count() > 0 || aw_points_base_pub_->get_intra_process_subscription_count() > 0) { - convert.tick(); + // convert.tick(); const auto autoware_cloud_xyzi = nebula::drivers::convertPointXYZIRCAEDTToPointXYZIR(pointcloud); @@ -184,15 +189,15 @@ void HesaiRosWrapper::ProcessCloudPacket(std::unique_ptrheader.stamp = rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); - convert.tock(); - publish.tick(); + // convert.tock(); + // publish.tick(); PublishCloud(std::move(ros_pc_msg_ptr), aw_points_base_pub_); - publish.tock(); + // publish.tock(); } if ( aw_points_ex_pub_->get_subscription_count() > 0 || aw_points_ex_pub_->get_intra_process_subscription_count() > 0) { - convert.tick(); + // convert.tick(); const auto autoware_ex_cloud = nebula::drivers::convertPointXYZIRCAEDTToPointXYZIRADT( pointcloud, std::get<1>(pointcloud_ts)); @@ -200,10 +205,10 @@ void HesaiRosWrapper::ProcessCloudPacket(std::unique_ptrheader.stamp = rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); - convert.tock(); - publish.tick(); + // convert.tock(); + // publish.tick(); PublishCloud(std::move(ros_pc_msg_ptr), aw_points_ex_pub_); - publish.tock(); + // publish.tock(); } } From a723c29a3fdac8dc05b93896f9d3bbaa1f3d9e50 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Fri, 5 Apr 2024 17:20:44 +0900 Subject: [PATCH 11/57] feat(hesai): move received buffer into ros message instead of copy --- .../hesai_hw_interface.hpp | 6 +++--- .../hesai_hw_interface.cpp | 12 ++++++------ .../include/nebula_ros/hesai/hesai_ros_wrapper.hpp | 2 +- nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 3 ++- 4 files changed, 12 insertions(+), 11 deletions(-) diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp index 34c5208ee..d71a26998 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp @@ -127,7 +127,7 @@ class HesaiHwInterface : NebulaHwInterfaceBase std::unique_ptr scan_cloud_ptr_; std::function is_valid_packet_; /*Lambda Function Array to verify proper packet size*/ - std::function & buffer)> + std::function & buffer)> cloud_packet_callback_; /**This function pointer is called when the scan is complete*/ int prev_phase_{}; @@ -198,7 +198,7 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @brief Callback function to receive the Cloud Packet data from the UDP Driver /// @param buffer Buffer containing the data received from the UDP socket - void ReceiveSensorPacketCallback(const std::vector & buffer) final; + void ReceiveSensorPacketCallback(std::vector & buffer); /// @brief Starting the interface that handles UDP streams /// @return Resulting status Status SensorInterfaceStart() final; @@ -222,7 +222,7 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @param scan_callback Callback function /// @return Resulting status Status RegisterScanCallback( - std::function &)> scan_callback); + std::function &)> scan_callback); /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION /// @return Resulting status std::string GetLidarCalibrationString(); diff --git a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp index 4fbb020b0..e37d42a43 100644 --- a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp @@ -239,18 +239,18 @@ Status HesaiHwInterface::SensorInterfaceStart() } Status HesaiHwInterface::RegisterScanCallback( - std::function &)> scan_callback) + std::function &)> scan_callback) { cloud_packet_callback_ = std::move(scan_callback); return Status::OK; } -void HesaiHwInterface::ReceiveSensorPacketCallback(const std::vector & buffer) +void HesaiHwInterface::ReceiveSensorPacketCallback(std::vector & buffer) { - if (!is_valid_packet_(buffer.size())) { - PrintDebug("Invalid Packet: " + std::to_string(buffer.size())); - return; - } + // if (!is_valid_packet_(buffer.size())) { + // PrintDebug("Invalid Packet: " + std::to_string(buffer.size())); + // return; + // } TODO cloud_packet_callback_(buffer); } Status HesaiHwInterface::SensorInterfaceStop() diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp index 149232e98..3190b3b97 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp @@ -129,7 +129,7 @@ class HesaiRosWrapper final : public rclcpp::Node, NebulaRosWrapperBase const drivers::SensorConfigurationBase & sensor_configuration) override; /// @brief Callback for receiving a raw UDP packet /// @param scan_buffer Received PandarScan - void ReceiveCloudPacketCallback(const std::vector & scan_buffer); + void ReceiveCloudPacketCallback(std::vector & scan_buffer); /// @brief Decodes a nebula packet and, if it completes the scan, publishes the pointcloud. /// @param packet_msg The received packet message diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index 533c3cf49..c113f1f04 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -118,7 +118,7 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) std::bind(&HesaiRosWrapper::paramCallback, this, std::placeholders::_1)); } -void HesaiRosWrapper::ReceiveCloudPacketCallback(const std::vector & packet) +void HesaiRosWrapper::ReceiveCloudPacketCallback(std::vector & packet) { // static auto convert = nebula::util::Instrumentation("ReceiveCloudPacketCallback.convert"); // static auto publish = nebula::util::Instrumentation("ReceiveCloudPacketCallback.publish"); @@ -142,6 +142,7 @@ void HesaiRosWrapper::ReceiveCloudPacketCallback(const std::vector & pa // delay.tick(msg_ptr->stamp); // publish.tick(); + packet_pub_->publish(std::move(msg_ptr)); // publish.tock(); } From a7dc57b6a68786fc3ac7d031ec1d8707d9d532cd Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Fri, 5 Apr 2024 17:21:46 +0900 Subject: [PATCH 12/57] feat(hesai_ros_wrapper: add thread-safe queue between udp receiver and decoder, decode in separate thread --- .../nebula_ros/hesai/hesai_ros_wrapper.hpp | 5 +++ .../include/nebula_ros/hesai/mt_queue.hpp | 41 +++++++++++++++++++ nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 27 ++++++++---- 3 files changed, 65 insertions(+), 8 deletions(-) create mode 100644 nebula_ros/include/nebula_ros/hesai/mt_queue.hpp diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp index 3190b3b97..4e1a6be7d 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp @@ -28,6 +28,8 @@ #include #include +#include "nebula_ros/hesai/mt_queue.hpp" + namespace nebula { namespace ros @@ -204,6 +206,9 @@ class HesaiRosWrapper final : public rclcpp::Node, NebulaRosWrapperBase std::shared_ptr sensor_cfg_ptr_; std::shared_ptr correction_cfg_ptr_; + mt_queue> packet_queue_; + std::thread decoder_thread_; + Status interface_status_; //todo: temporary class member during single node refactoring diff --git a/nebula_ros/include/nebula_ros/hesai/mt_queue.hpp b/nebula_ros/include/nebula_ros/hesai/mt_queue.hpp new file mode 100644 index 000000000..49c08b957 --- /dev/null +++ b/nebula_ros/include/nebula_ros/hesai/mt_queue.hpp @@ -0,0 +1,41 @@ +#pragma once + +#include +#include +#include + +template +class mt_queue +{ +private: + std::mutex d_mutex; + std::condition_variable d_condition; + std::deque d_queue; + + size_t capacity_; + +public: + mt_queue(size_t capacity) : capacity_(capacity) {} + + bool push(T && value) + { + { + std::unique_lock lock(this->d_mutex); + if (d_queue.size() == capacity_) { + return false; + } + + d_queue.push_front(std::move(value)); + } + this->d_condition.notify_one(); + return true; + } + T pop() + { + std::unique_lock lock(this->d_mutex); + this->d_condition.wait(lock, [=] { return !this->d_queue.empty(); }); + T rc(std::move(this->d_queue.back())); + this->d_queue.pop_back(); + return rc; + } +}; \ No newline at end of file diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index c113f1f04..b86c05199 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -7,7 +7,8 @@ namespace ros HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) : rclcpp::Node("hesai_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)), hw_interface_(), - diagnostics_updater_(this) + diagnostics_updater_(this), + packet_queue_(300) { setvbuf(stdout, NULL, _IONBF, BUFSIZ); @@ -91,13 +92,20 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) auto pointcloud_qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); - auto packet_qos = - rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 1000), qos_profile); + // auto packet_qos = + // rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 300), qos_profile); - packet_pub_ = create_publisher("hesai_packets", packet_qos); - packet_sub_ = create_subscription( - "hesai_packets", packet_qos, - std::bind(&HesaiRosWrapper::ProcessCloudPacket, this, std::placeholders::_1)); + // packet_pub_ = create_publisher("hesai_packets", packet_qos); + // packet_sub_ = create_subscription( + // "hesai_packets", packet_qos, + // std::bind(&HesaiRosWrapper::ProcessCloudPacket, this, std::placeholders::_1)); + + decoder_thread_ = std::thread([this](){ + while(true) { + auto pkt = packet_queue_.pop(); + this->ProcessCloudPacket(std::move(pkt)); + } + }); nebula_points_pub_ = this->create_publisher("pandar_points", pointcloud_qos); @@ -142,7 +150,10 @@ void HesaiRosWrapper::ReceiveCloudPacketCallback(std::vector & packet) // delay.tick(msg_ptr->stamp); // publish.tick(); - packet_pub_->publish(std::move(msg_ptr)); + if (!packet_queue_.push(std::move(msg_ptr))) { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packets dropped"); + } + // packet_pub_->publish(std::move(msg_ptr)); // publish.tock(); } From d297b15dae7617cd2b433d618690c01d4b361475 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Fri, 5 Apr 2024 17:22:12 +0900 Subject: [PATCH 13/57] feat(nebula_common): more instrumentation tools --- .../nebula_common/util/instrumentation.hpp | 87 +++++++------------ .../util/performance_counter.hpp | 43 +++++++++ .../nebula_common/util/topic_delay.hpp | 68 +++++++++++++++ scripts/plot_instrumentation.py | 77 ++++++++++++++++ 4 files changed, 220 insertions(+), 55 deletions(-) create mode 100644 nebula_common/include/nebula_common/util/performance_counter.hpp create mode 100644 nebula_common/include/nebula_common/util/topic_delay.hpp create mode 100755 scripts/plot_instrumentation.py diff --git a/nebula_common/include/nebula_common/util/instrumentation.hpp b/nebula_common/include/nebula_common/util/instrumentation.hpp index ab9357286..ccce705d8 100644 --- a/nebula_common/include/nebula_common/util/instrumentation.hpp +++ b/nebula_common/include/nebula_common/util/instrumentation.hpp @@ -1,5 +1,7 @@ #pragma once +#include "nebula_common/util/performance_counter.hpp" + #include #include #include @@ -18,86 +20,61 @@ class Instrumentation using duration = std::chrono::nanoseconds; public: - Instrumentation(std::string tag) : tag(tag), last_print(clock::now()) {} + Instrumentation(std::string tag) + : tag_(std::move(tag)), last_print_(clock::now()), delays_(), rtts_() + { + } void tick() { auto now = clock::now(); - if (last_tick != time_point::min()) { - auto rtt = now - last_tick; + if (last_tick_ != time_point::min()) { + auto rtt = now - last_tick_; rtts_.update(rtt); } - last_tick = now; + last_tick_ = now; } void tock() { auto now = clock::now(); - auto delay = now - last_tick; + auto delay = now - last_tick_; delays_.update(delay); - if (now - last_print >= PRINT_INTERVAL) { + if (now - last_print_ >= PRINT_INTERVAL) { auto del = delays_.reset(); auto rtt = rtts_.reset(); - last_print = now; + last_print_ = now; - std::cout << "{\"tag\": \"" << tag << "\", \"del_min\": " << del.min.count() * 1e-9 - << ", \"del_avg\": " << del.avg.count() * 1e-9 - << ", \"del_max\": " << del.max.count() * 1e-9 - << ", \"rtt_min\": " << rtt.min.count() * 1e-9 - << ", \"rtt_avg\": " << rtt.avg.count() * 1e-9 - << ", \"rtt_max\": " << rtt.max.count() * 1e-9 << ", \"window\": " << rtt.count - << '}' << std::endl; - } - } + std::stringstream ss; -private: - class Counter - { - public: - struct Stats - { - duration min; - duration avg; - duration max; - uint64_t count; - }; - - Stats reset() - { - auto avg_measurement = sum_measurements / num_measurements; - - Stats result{min_measurement, avg_measurement, max_measurement, num_measurements}; - - sum_measurements = duration::zero(); - num_measurements = 0; - min_measurement = duration::zero(); - max_measurement = duration::max(); - } + ss << "{\"type\": \"instrumentation\", \"tag\": \"" << tag_ + << "\", \"del\": ["; + + for (duration & dt : *del) { + ss << dt.count() << ','; + } - void update(duration measurement) - { - sum_measurements += measurement; - num_measurements++; + ss <<"null], \"rtt\": ["; - min_measurement = std::min(min_measurement, measurement); - max_measurement = std::max(max_measurement, measurement); - } + for (duration & dt : *rtt) { + ss << dt.count() << ','; + } + + ss << "null]}" << std::endl; - private: - duration sum_measurements{duration::zero()}; - uint64_t num_measurements{0}; - duration min_measurement{duration::max()}; - duration max_measurement{duration::zero()}; - }; + std::cout << ss.str(); + } + } - std::string tag; +private: + std::string tag_; - time_point last_tick{time_point::min()}; - time_point last_print{time_point::min()}; + time_point last_tick_{time_point::min()}; + time_point last_print_{time_point::min()}; Counter delays_; Counter rtts_; diff --git a/nebula_common/include/nebula_common/util/performance_counter.hpp b/nebula_common/include/nebula_common/util/performance_counter.hpp new file mode 100644 index 000000000..cd6b618f1 --- /dev/null +++ b/nebula_common/include/nebula_common/util/performance_counter.hpp @@ -0,0 +1,43 @@ +#pragma once + +#include +#include +#include + +namespace nebula +{ +namespace util +{ + +class Counter +{ + using clock = std::chrono::high_resolution_clock; + using time_point = clock::time_point; + using duration = std::chrono::nanoseconds; + +public: + Counter() + : buf_record_(), buf_output_() + { + buf_record_.resize(100000); + buf_output_.resize(100000); + } + + std::vector * reset() + { + buf_record_.swap(buf_output_); + buf_record_.clear(); + return &buf_output_; + } + + void update(duration measurement) + { + buf_record_.push_back(measurement); + } + +private: + std::vector buf_record_; + std::vector buf_output_; +}; +} // namespace util +} // namespace nebula \ No newline at end of file diff --git a/nebula_common/include/nebula_common/util/topic_delay.hpp b/nebula_common/include/nebula_common/util/topic_delay.hpp new file mode 100644 index 000000000..a92ce29a2 --- /dev/null +++ b/nebula_common/include/nebula_common/util/topic_delay.hpp @@ -0,0 +1,68 @@ +#pragma once + +#include + +#include "nebula_common/util/performance_counter.hpp" + + +#include +#include +#include + +namespace nebula +{ +namespace util +{ + +using namespace std::chrono_literals; + +class TopicDelay +{ + using clock = std::chrono::high_resolution_clock; + using time_point = clock::time_point; + using duration = std::chrono::nanoseconds; + +public: + TopicDelay(std::string tag) : tag_(std::move(tag)), last_print_(clock::now()), delays_() {} + + void tick(builtin_interfaces::msg::Time & stamp) + { + auto now = clock::now(); + + time_point t_msg = time_point(duration(static_cast(stamp.sec) * 1'000'000'000ULL + stamp.nanosec)); + auto delay = now - t_msg; + + delays_.update(delay); + + if (now - last_print_ >= PRINT_INTERVAL) { + auto del = delays_.reset(); + + last_print_ = now; + + std::stringstream ss; + + ss << "{\"type\": \"topic_delay\", \"tag\": \"" << tag_ + << "\", \"del\": ["; + + for (duration & dt : *del) { + ss << dt.count() << ','; + } + + ss << "null]}" << std::endl; + + std::cout << ss.str(); + } + } + +private: + std::string tag_; + + time_point last_print_{time_point::min()}; + + Counter delays_; + + const duration PRINT_INTERVAL = 1s; +}; + +} // namespace util +} // namespace nebula \ No newline at end of file diff --git a/scripts/plot_instrumentation.py b/scripts/plot_instrumentation.py new file mode 100755 index 000000000..fe9c12fda --- /dev/null +++ b/scripts/plot_instrumentation.py @@ -0,0 +1,77 @@ +#!/usr/bin/python3 + +import argparse +from matplotlib import pyplot as plt +import numpy as np +import pandas as pd +import re +import json +import os +from collections import defaultdict + +def condition_data(log_file: str): + with open(log_file, 'r') as f: + lines = f.readlines() + lines = [re.search(r'(\{"type":.*?"tag":.*?\})', l) for l in lines] + lines = [re.sub(r'([0-9])"', r'\1', l[1]) for l in lines if l] + lines = [json.loads(l) for l in lines if l] + + cols = defaultdict(list) + + for record in lines: + for key in record.keys(): + if key in ["tag", "type"]: + continue + + colname = f"{record['type']}.{key}.{record['tag']}" + cols[colname] += [num for num in record[key] if num is not None] + + def quantile_filter(series, quantile): + q = series.quantile(quantile) + return series[series <= q] + + cols = {k: pd.Series(v, name=k) / 1e3 for k, v in cols.items()} + cols = {k: quantile_filter(v, .999) for k, v in cols.items()} + + for v in cols.values(): + v: pd.Series + v.attrs['file'] = os.path.basename(os.path.splitext(log_file)[0]) + + return cols + +def plot(conditioned_logs): + + conditioned_logs = {k: [dic[k] for dic in conditioned_logs] for k in conditioned_logs[0]} + + fig, axs = plt.subplots(1, len(conditioned_logs), figsize=(15, 8), dpi=120) + + handles, labels = [], [] + + for i, k in enumerate(sorted(conditioned_logs.keys())): + k: str + v = conditioned_logs[k] + ax: plt.Axes = axs[i] + art = ax.boxplot(v) + handles.append(art) + labels.append(k) + ax.set_ylabel("dt [µs]") + ax.set_title(k.replace(".", "\n")) + ax.set_xticks([1 + i for i in range(len(v))], [ser.attrs['file'] for ser in v]) + ax.tick_params(axis='x', labelrotation=90) + + + + fig.tight_layout() + plt.savefig("instrumentation.png") + plt.show() + +def main(args): + conditioned_logs = [condition_data(f) for f in args.log_files] + plot(conditioned_logs) + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("log_files", nargs="+") + args = parser.parse_args() + + main(args) \ No newline at end of file From 185d584f61e8c6231c6dd62a5e3d67cb10d03b4a Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Fri, 5 Apr 2024 17:23:02 +0900 Subject: [PATCH 14/57] fix: update link to transport_drivers fork --- build_depends.repos | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/build_depends.repos b/build_depends.repos index e28379f1a..e9278685a 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -2,5 +2,5 @@ repositories: # TCP version of transport drivers transport_drivers: type: git - url: https://github.com/MapIV/transport_drivers - version: boost + url: https://github.com/mojomex/transport_drivers + version: mutable-buffer-in-udp-callback From 79de4b6f0c17763b76ab04dbc0ab9d5d66045982 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Mon, 8 Apr 2024 15:36:49 +0900 Subject: [PATCH 15/57] update GitHub PR view From 5d64e9b6265700fa88e19ccb0db0b7bca5781628 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Mon, 8 Apr 2024 16:43:51 +0900 Subject: [PATCH 16/57] refactor(mt_queue): make variables more readable --- .../include/nebula_ros/hesai/mt_queue.hpp | 25 +++++++++---------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/nebula_ros/include/nebula_ros/hesai/mt_queue.hpp b/nebula_ros/include/nebula_ros/hesai/mt_queue.hpp index 49c08b957..730b54f7c 100644 --- a/nebula_ros/include/nebula_ros/hesai/mt_queue.hpp +++ b/nebula_ros/include/nebula_ros/hesai/mt_queue.hpp @@ -8,10 +8,9 @@ template class mt_queue { private: - std::mutex d_mutex; - std::condition_variable d_condition; - std::deque d_queue; - + std::mutex mutex_; + std::condition_variable condition_variable_; + std::deque queue_; size_t capacity_; public: @@ -20,22 +19,22 @@ class mt_queue bool push(T && value) { { - std::unique_lock lock(this->d_mutex); - if (d_queue.size() == capacity_) { + std::unique_lock lock(this->mutex_); + if (queue_.size() == capacity_) { return false; } - d_queue.push_front(std::move(value)); + queue_.push_front(std::move(value)); } - this->d_condition.notify_one(); + this->condition_variable_.notify_one(); return true; } T pop() { - std::unique_lock lock(this->d_mutex); - this->d_condition.wait(lock, [=] { return !this->d_queue.empty(); }); - T rc(std::move(this->d_queue.back())); - this->d_queue.pop_back(); - return rc; + std::unique_lock lock(this->mutex_); + this->condition_variable_.wait(lock, [=] { return !this->queue_.empty(); }); + T return_value(std::move(this->queue_.back())); + this->queue_.pop_back(); + return return_value; } }; \ No newline at end of file From 5571d84e1d5d5097d08317c063a90f35b40d1843 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Mon, 8 Apr 2024 16:45:48 +0900 Subject: [PATCH 17/57] perf(hesai_ros_wrapper): update queue capacity to alleviate packet drops on ECU --- nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index b86c05199..f84371a36 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -8,7 +8,7 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) : rclcpp::Node("hesai_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)), hw_interface_(), diagnostics_updater_(this), - packet_queue_(300) + packet_queue_(3000) { setvbuf(stdout, NULL, _IONBF, BUFSIZ); From 3e2b1206dfda450cf919e23a9d38dce68bf2622a Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Mon, 8 Apr 2024 16:46:11 +0900 Subject: [PATCH 18/57] chore(hesai_ros_wrapper): remove unused pub/sub --- .../include/nebula_ros/hesai/hesai_ros_wrapper.hpp | 3 --- nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 11 ----------- 2 files changed, 14 deletions(-) diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp index 4e1a6be7d..b2cbd04dc 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp @@ -195,9 +195,6 @@ class HesaiRosWrapper final : public rclcpp::Node, NebulaRosWrapperBase std::shared_ptr driver_ptr_; Status wrapper_status_; - rclcpp::Publisher::SharedPtr packet_pub_; - rclcpp::Subscription::SharedPtr packet_sub_; - rclcpp::Publisher::SharedPtr nebula_points_pub_; rclcpp::Publisher::SharedPtr aw_points_ex_pub_; rclcpp::Publisher::SharedPtr aw_points_base_pub_; diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index f84371a36..1540df3fa 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -92,14 +92,6 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) auto pointcloud_qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); - // auto packet_qos = - // rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 300), qos_profile); - - // packet_pub_ = create_publisher("hesai_packets", packet_qos); - // packet_sub_ = create_subscription( - // "hesai_packets", packet_qos, - // std::bind(&HesaiRosWrapper::ProcessCloudPacket, this, std::placeholders::_1)); - decoder_thread_ = std::thread([this](){ while(true) { auto pkt = packet_queue_.pop(); @@ -254,9 +246,6 @@ Status HesaiRosWrapper::GetStatus() Status HesaiRosWrapper::GetParameters(drivers::HesaiSensorConfiguration & sensor_configuration) { - /////////////////////////////////////////////// - // Define and get ROS parameters - /////////////////////////////////////////////// { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; From 8b59a58aae463a291a2383193155e1956e5f9950 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Mon, 8 Apr 2024 18:07:03 +0900 Subject: [PATCH 19/57] refactor(hesai_ros_wrapper): clean up control flow, member variables --- .../common/nebula_ros_wrapper_base.hpp | 68 --- .../nebula_ros/hesai/hesai_ros_wrapper.hpp | 110 ++--- nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 436 ++++++++---------- 3 files changed, 230 insertions(+), 384 deletions(-) delete mode 100644 nebula_ros/include/nebula_ros/common/nebula_ros_wrapper_base.hpp diff --git a/nebula_ros/include/nebula_ros/common/nebula_ros_wrapper_base.hpp b/nebula_ros/include/nebula_ros/common/nebula_ros_wrapper_base.hpp deleted file mode 100644 index 5aaeb634b..000000000 --- a/nebula_ros/include/nebula_ros/common/nebula_ros_wrapper_base.hpp +++ /dev/null @@ -1,68 +0,0 @@ -#pragma once - -#include "nebula_common/nebula_common.hpp" -#include "nebula_common/nebula_status.hpp" - -#include -#include -#include - -#include - -#include -#include - -namespace nebula -{ -namespace ros -{ -/// @brief Base class for ros wrapper of each sensor driver -class NebulaRosWrapperBase -{ -public: - NebulaRosWrapperBase() = default; - - NebulaRosWrapperBase(NebulaRosWrapperBase && c) = delete; - NebulaRosWrapperBase & operator=(NebulaRosWrapperBase && c) = delete; - NebulaRosWrapperBase(const NebulaRosWrapperBase & c) = delete; - NebulaRosWrapperBase & operator=(const NebulaRosWrapperBase & c) = delete; - - /// @brief Start point cloud streaming (Call CloudInterfaceStart of HwInterface) - /// @return Resulting status - virtual Status StreamStart() = 0; - - /// @brief Stop point cloud streaming (not used) - /// @return Resulting status - virtual Status StreamStop() = 0; - - /// @brief Shutdown (not used) - /// @return Resulting status - virtual Status Shutdown() = 0; - -private: - /// @brief Virtual function for initializing hardware interface ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @return Resulting status - virtual Status InitializeHwInterface( - const drivers::SensorConfigurationBase & sensor_configuration) = 0; - - /// @brief Virtual function for initializing hardware monitor ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @return Resulting status - virtual Status InitializeHwMonitor( - const drivers::SensorConfigurationBase & sensor_configuration) = 0; - - /// @brief Virtual function for initializing ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @return Resulting status - virtual Status InitializeCloudDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) = 0; - - /// @brief Point cloud publisher - rclcpp::Publisher::SharedPtr cloud_pub_; -}; - -} // namespace ros -} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp index b2cbd04dc..2ba2e2407 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp @@ -8,9 +8,7 @@ // #include "nebula_common/util/topic_delay.hpp" #include "nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" -#include "nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp" -#include "nebula_ros/common/nebula_hw_monitor_ros_wrapper_base.hpp" -#include "nebula_ros/common/nebula_ros_wrapper_base.hpp" +#include "nebula_ros/hesai/mt_queue.hpp" #include #include @@ -28,8 +26,6 @@ #include #include -#include "nebula_ros/hesai/mt_queue.hpp" - namespace nebula { namespace ros @@ -55,7 +51,7 @@ bool get_param(const std::vector & p, const std::string & nam } /// @brief Ros wrapper of hesai driver -class HesaiRosWrapper final : public rclcpp::Node, NebulaRosWrapperBase +class HesaiRosWrapper final : public rclcpp::Node { public: explicit HesaiRosWrapper(const rclcpp::NodeOptions & options); @@ -67,36 +63,17 @@ class HesaiRosWrapper final : public rclcpp::Node, NebulaRosWrapperBase /// @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; + Status StreamStart(); private: - /// @brief Initialize pointcloud decoder - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @param correction_configuration CorrectionConfiguration for this driver (only for AT128, ignored otherwise) - /// @return Resulting status - Status InitializeCloudDriver( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration, - const std::shared_ptr & correction_configuration = nullptr); - - Status InitializeCloudDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) { - return InitializeCloudDriver(sensor_configuration, calibration_configuration, nullptr); - } + Status InitializeHwInterface(); + Status InitializeDecoder(); + Status InitializeHwMonitor(); /// @brief Get configurations from ros parameters /// @param sensor_configuration Output of SensorConfiguration /// @return Resulting status - Status GetParameters( - drivers::HesaiSensorConfiguration & sensor_configuration); + Status GetParameters(drivers::HesaiSensorConfiguration & sensor_configuration); /// @brief Get calibration data from the sensor /// @param calibration_configuration Output of CalibrationConfiguration @@ -124,11 +101,6 @@ class HesaiRosWrapper final : public rclcpp::Node, NebulaRosWrapperBase std::unique_ptr pointcloud, const rclcpp::Publisher::SharedPtr & publisher); - /// @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 a raw UDP packet /// @param scan_buffer Received PandarScan void ReceiveCloudPacketCallback(std::vector & scan_buffer); @@ -146,12 +118,6 @@ class HesaiRosWrapper final : public rclcpp::Node, NebulaRosWrapperBase /// @return SetParametersResult std::vector updateParameters(); - /// @brief Initializing hardware monitor ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @return Resulting status - Status InitializeHwMonitor( - const drivers::SensorConfigurationBase & sensor_configuration) override; - /// @brief Initializing diagnostics void InitializeHesaiDiagnostics(); /// @brief Callback of the timer for getting the current lidar status @@ -193,7 +159,10 @@ class HesaiRosWrapper final : public rclcpp::Node, NebulaRosWrapperBase std::string GetFixedPrecisionString(double val, int pre = 2); std::shared_ptr driver_ptr_; + drivers::HesaiHwInterface hw_interface_; + Status wrapper_status_; + Status interface_status_; rclcpp::Publisher::SharedPtr nebula_points_pub_; rclcpp::Publisher::SharedPtr aw_points_ex_pub_; @@ -203,15 +172,15 @@ class HesaiRosWrapper final : public rclcpp::Node, NebulaRosWrapperBase std::shared_ptr sensor_cfg_ptr_; std::shared_ptr correction_cfg_ptr_; + /// @brief Stores received packets that have not been processed yet by the decoder thread mt_queue> packet_queue_; + /// @brief Thread to isolate decoding from receiving std::thread decoder_thread_; - Status interface_status_; - - //todo: temporary class member during single node refactoring + // todo: temporary class member during single node refactoring bool launch_hw_; - //todo: temporary class member during single node refactoring + // todo: temporary class member during single node refactoring std::string calibration_file_path; /// @brief File path of Correction data (Only required only for AT) std::string correction_file_path; @@ -219,9 +188,6 @@ class HesaiRosWrapper final : public rclcpp::Node, NebulaRosWrapperBase /// @brief Received Hesai message publisher rclcpp::Publisher::SharedPtr pandar_scan_pub_; - drivers::HesaiHwInterface hw_interface_; - - uint16_t delay_hw_ms_; bool retry_hw_; std::mutex mtx_config_; OnSetParametersCallbackHandle::SharedPtr set_param_res_; @@ -229,39 +195,35 @@ class HesaiRosWrapper final : public rclcpp::Node, NebulaRosWrapperBase diagnostic_updater::Updater diagnostics_updater_; rclcpp::TimerBase::SharedPtr diagnostics_update_timer_; - rclcpp::TimerBase::SharedPtr diagnostics_update_monitor_timer_; rclcpp::TimerBase::SharedPtr fetch_diagnostics_timer_; - std::unique_ptr current_status; - std::unique_ptr current_monitor; - std::unique_ptr current_config; - std::unique_ptr current_inventory; - std::unique_ptr current_lidar_monitor_tree; - std::unique_ptr current_status_time; - std::unique_ptr current_config_time; - std::unique_ptr current_inventory_time; - std::unique_ptr current_lidar_monitor_time; - uint8_t current_diag_status; - uint8_t current_monitor_status; - uint16_t diag_span_; - uint16_t delay_monitor_ms_; - std::mutex mtx_diag; - std::mutex mtx_status; - std::mutex mtx_lidar_monitor; + std::unique_ptr current_status_; + std::unique_ptr current_monitor_; + std::unique_ptr current_config_; + std::unique_ptr current_inventory_; + std::unique_ptr current_lidar_monitor_tree_; + + std::unique_ptr current_status_time_; + std::unique_ptr current_config_time_; + std::unique_ptr current_inventory_time_; + std::unique_ptr current_lidar_monitor_time_; - std::string info_model; - std::string info_serial; - rclcpp::CallbackGroup::SharedPtr cbg_r_; - rclcpp::CallbackGroup::SharedPtr cbg_m_; - rclcpp::CallbackGroup::SharedPtr cbg_m2_; + uint8_t current_diag_status_; + uint8_t current_monitor_status_; + + uint16_t diag_span_; + std::mutex mtx_lidar_status_; + std::mutex mtx_lidar_monitor_; - const char * not_supported_message; - const char * error_message; - std::string message_sep; + std::string info_model_; + std::string info_serial_; - std::vector temperature_names; + std::vector temperature_names_; bool setup_sensor; + const std::string MSG_NOT_SUPPORTED = "Not supported"; + const std::string MSG_ERROR = "Error"; + const std::string MSG_SEP = ": "; }; } // namespace ros diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index 1540df3fa..f0e6c121a 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -7,115 +7,164 @@ namespace ros HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) : rclcpp::Node("hesai_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)), hw_interface_(), - diagnostics_updater_(this), - packet_queue_(3000) + packet_queue_(3000), + diagnostics_updater_(this) { setvbuf(stdout, NULL, _IONBF, BUFSIZ); drivers::HesaiSensorConfiguration sensor_configuration; wrapper_status_ = GetParameters(sensor_configuration); sensor_cfg_ptr_ = std::make_shared(sensor_configuration); + // DeclareRosParameters(); + // auto sensor_cfg_ptr = GetRosParameters(); + InitializeHwInterface(); + InitializeDecoder(); + InitializeHwMonitor(); + // StreamStart(); - // hwiface - { - 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())); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr_)); - Status rt = hw_interface_.InitializeTcpDriver(); - if (this->retry_hw_) { - int cnt = 0; - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << " Retry: " << cnt); - while (rt == Status::ERROR_1) { - cnt++; - std::this_thread::sleep_for(std::chrono::milliseconds(8000)); // >5000 - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Retry: " << cnt); - rt = hw_interface_.InitializeTcpDriver(); - } + + RCLCPP_DEBUG(this->get_logger(), "Starting stream"); + StreamStart(); + hw_interface_.RegisterScanCallback( + std::bind(&HesaiRosWrapper::ReceiveCloudPacketCallback, this, std::placeholders::_1)); + + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&HesaiRosWrapper::paramCallback, this, std::placeholders::_1)); +} + +Status HesaiRosWrapper::InitializeHwInterface() +{ + if (Status::OK != interface_status_) { + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); + return interface_status_; + } + hw_interface_.SetLogger(std::make_shared(this->get_logger())); + hw_interface_.SetSensorConfiguration( + std::static_pointer_cast(sensor_cfg_ptr_)); + + Status status; + int retry_count = 0; + + while (true) { + status = hw_interface_.InitializeTcpDriver(); + if (status == Status::OK || !retry_hw_) { + break; } - if (rt != Status::ERROR_1) { - try { - std::vector thread_pool{}; - thread_pool.emplace_back([this] { - auto result = hw_interface_.GetInventory(); - RCLCPP_INFO_STREAM(get_logger(), result); - hw_interface_.SetTargetModel(result.model); - }); - for (std::thread & th : thread_pool) { - th.join(); - } + retry_count++; + std::this_thread::sleep_for(std::chrono::milliseconds(8000)); // >5000 + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Retry: " << retry_count); + }; - } catch (...) { - std::cout << "catch (...) in parent" << std::endl; - RCLCPP_ERROR_STREAM(get_logger(), "Failed to get model from sensor..."); - } - if (this->setup_sensor) { - hw_interface_.CheckAndSetConfig(); - updateParameters(); - } - } else { - RCLCPP_ERROR_STREAM( - get_logger(), - "Failed to get model from sensor... Set from config: " << sensor_cfg_ptr_->sensor_model); - hw_interface_.SetTargetModel(sensor_cfg_ptr_->sensor_model); + if (status == Status::OK) { + try { + auto inventory = hw_interface_.GetInventory(); + RCLCPP_INFO_STREAM(get_logger(), inventory); + hw_interface_.SetTargetModel(inventory.model); + } catch (...) { + RCLCPP_ERROR_STREAM(get_logger(), "Failed to get model from sensor..."); } + if (this->setup_sensor) { + hw_interface_.CheckAndSetConfig(); + updateParameters(); + } + } else { + RCLCPP_ERROR_STREAM( + get_logger(), + "Failed to get model from sensor... Set from config: " << sensor_cfg_ptr_->sensor_model); + hw_interface_.SetTargetModel(sensor_cfg_ptr_->sensor_model); } - // decoder - { - drivers::HesaiCalibrationConfiguration calibration_configuration; - drivers::HesaiCorrection correction_configuration; - wrapper_status_ = GetCalibrationData(calibration_configuration, correction_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); - // Will be left empty for AT128, and ignored by all decoders except AT128 - correction_cfg_ptr_ = std::make_shared(correction_configuration); - - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); - wrapper_status_ = InitializeCloudDriver( - std::static_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_), - std::static_pointer_cast(correction_cfg_ptr_)); - - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Wrapper=" << wrapper_status_); - rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; - auto pointcloud_qos = - rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); - - decoder_thread_ = std::thread([this](){ - while(true) { - auto pkt = packet_queue_.pop(); - this->ProcessCloudPacket(std::move(pkt)); - } - }); + return Status::OK; +} + +Status HesaiRosWrapper::InitializeDecoder() +{ + calibration_cfg_ptr_ = std::make_shared(); + correction_cfg_ptr_ = std::make_shared(); - nebula_points_pub_ = - this->create_publisher("pandar_points", pointcloud_qos); - aw_points_base_pub_ = - this->create_publisher("aw_points", pointcloud_qos); - aw_points_ex_pub_ = - this->create_publisher("aw_points_ex", pointcloud_qos); + wrapper_status_ = GetCalibrationData(*calibration_cfg_ptr_, *correction_cfg_ptr_); + if (Status::OK != wrapper_status_) { + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); + return wrapper_status_; } + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Wrapper=" << wrapper_status_); + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + auto pointcloud_qos = + rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); + + decoder_thread_ = std::thread([this]() { + while (true) { + auto pkt = packet_queue_.pop(); + this->ProcessCloudPacket(std::move(pkt)); + } + }); - RCLCPP_DEBUG(this->get_logger(), "Starting stream"); - StreamStart(); - hw_interface_.RegisterScanCallback( - std::bind(&HesaiRosWrapper::ReceiveCloudPacketCallback, this, std::placeholders::_1)); + nebula_points_pub_ = + this->create_publisher("pandar_points", pointcloud_qos); + aw_points_base_pub_ = + this->create_publisher("aw_points", pointcloud_qos); + aw_points_ex_pub_ = + this->create_publisher("aw_points_ex", pointcloud_qos); - InitializeHwMonitor(*sensor_cfg_ptr_); + driver_ptr_ = std::make_shared( + sensor_cfg_ptr_, calibration_cfg_ptr_, correction_cfg_ptr_); + return driver_ptr_->GetStatus(); +} - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&HesaiRosWrapper::paramCallback, this, std::placeholders::_1)); +Status HesaiRosWrapper::InitializeHwMonitor() +{ + if (Status::OK != interface_status_) { + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); + return Status::ERROR_1; + } + + switch (sensor_cfg_ptr_->sensor_model) { + case nebula::drivers::SensorModel::HESAI_PANDARXT32: + case nebula::drivers::SensorModel::HESAI_PANDARXT32M: + case nebula::drivers::SensorModel::HESAI_PANDARAT128: + temperature_names_.emplace_back("Bottom circuit board T1"); + temperature_names_.emplace_back("Bottom circuit board T2"); + temperature_names_.emplace_back("Laser emitting board RT_L1(Internal)"); + temperature_names_.emplace_back("Laser emitting board RT_L2"); + temperature_names_.emplace_back("Receiving board RT_R"); + temperature_names_.emplace_back("Receiving board RT2"); + temperature_names_.emplace_back("Top circuit RT3"); + temperature_names_.emplace_back("Not used"); + break; + case nebula::drivers::SensorModel::HESAI_PANDAR64: + case nebula::drivers::SensorModel::HESAI_PANDAR40P: + case nebula::drivers::SensorModel::HESAI_PANDAR40M: + case nebula::drivers::SensorModel::HESAI_PANDARQT64: + case nebula::drivers::SensorModel::HESAI_PANDARQT128: + case nebula::drivers::SensorModel::HESAI_PANDAR128_E3X: + case nebula::drivers::SensorModel::HESAI_PANDAR128_E4X: + default: + temperature_names_.emplace_back("Bottom circuit RT1"); + temperature_names_.emplace_back("Bottom circuit RT2"); + temperature_names_.emplace_back("Internal Temperature"); + temperature_names_.emplace_back("Laser emitting board RT1"); + temperature_names_.emplace_back("Laser emitting board RT2"); + temperature_names_.emplace_back("Receiving board RT1"); + temperature_names_.emplace_back("Top circuit RT1"); + temperature_names_.emplace_back("Top circuit RT2"); + break; + } + + auto result = hw_interface_.GetInventory(); + current_inventory_.reset(new HesaiInventory(result)); + current_inventory_time_.reset(new rclcpp::Time(this->get_clock()->now())); + std::cout << "HesaiInventory" << std::endl; + std::cout << result << std::endl; + info_model_ = result.get_str_model(); + info_serial_ = std::string(result.sn.begin(), result.sn.end()); + hw_interface_.SetTargetModel(result.model); + RCLCPP_INFO_STREAM(this->get_logger(), "Model:" << info_model_); + RCLCPP_INFO_STREAM(this->get_logger(), "Serial:" << info_serial_); + InitializeHesaiDiagnostics(); + return Status::OK; } void HesaiRosWrapper::ReceiveCloudPacketCallback(std::vector & packet) @@ -227,18 +276,6 @@ void HesaiRosWrapper::PublishCloud( publisher->publish(std::move(pointcloud)); } -Status HesaiRosWrapper::InitializeCloudDriver( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration, - const std::shared_ptr & correction_configuration) -{ - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration), - std::static_pointer_cast(correction_configuration)); - return driver_ptr_->GetStatus(); -} - Status HesaiRosWrapper::GetStatus() { return wrapper_status_; @@ -766,85 +803,6 @@ Status HesaiRosWrapper::StreamStart() return interface_status_; } -Status HesaiRosWrapper::StreamStop() -{ - return Status::OK; -} -Status HesaiRosWrapper::Shutdown() -{ - return Status::OK; -} - -Status HesaiRosWrapper::InitializeHwInterface( // todo: don't think this is needed - const drivers::SensorConfigurationBase & sensor_configuration) -{ - std::stringstream ss; - ss << sensor_configuration; - RCLCPP_DEBUG_STREAM(this->get_logger(), ss.str()); - return Status::OK; -} - -Status HesaiRosWrapper::InitializeHwMonitor( // todo: don't think this is needed - const drivers::SensorConfigurationBase & /* sensor_configuration */) -{ - cbg_r_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant); - cbg_m_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - cbg_m2_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - if (Status::OK != interface_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); - return Status::ERROR_1; - } - - message_sep = ": "; - not_supported_message = "Not supported"; - error_message = "Error"; - - switch (sensor_cfg_ptr_->sensor_model) { - case nebula::drivers::SensorModel::HESAI_PANDARXT32: - case nebula::drivers::SensorModel::HESAI_PANDARXT32M: - case nebula::drivers::SensorModel::HESAI_PANDARAT128: - temperature_names.emplace_back("Bottom circuit board T1"); - temperature_names.emplace_back("Bottom circuit board T2"); - temperature_names.emplace_back("Laser emitting board RT_L1(Internal)"); - temperature_names.emplace_back("Laser emitting board RT_L2"); - temperature_names.emplace_back("Receiving board RT_R"); - temperature_names.emplace_back("Receiving board RT2"); - temperature_names.emplace_back("Top circuit RT3"); - temperature_names.emplace_back("Not used"); - break; - case nebula::drivers::SensorModel::HESAI_PANDAR64: - case nebula::drivers::SensorModel::HESAI_PANDAR40P: - case nebula::drivers::SensorModel::HESAI_PANDAR40M: - case nebula::drivers::SensorModel::HESAI_PANDARQT64: - case nebula::drivers::SensorModel::HESAI_PANDARQT128: - case nebula::drivers::SensorModel::HESAI_PANDAR128_E3X: - case nebula::drivers::SensorModel::HESAI_PANDAR128_E4X: - default: - temperature_names.emplace_back("Bottom circuit RT1"); - temperature_names.emplace_back("Bottom circuit RT2"); - temperature_names.emplace_back("Internal Temperature"); - temperature_names.emplace_back("Laser emitting board RT1"); - temperature_names.emplace_back("Laser emitting board RT2"); - temperature_names.emplace_back("Receiving board RT1"); - temperature_names.emplace_back("Top circuit RT1"); - temperature_names.emplace_back("Top circuit RT2"); - break; - } - - auto result = hw_interface_.GetInventory(); - current_inventory.reset(new HesaiInventory(result)); - current_inventory_time.reset(new rclcpp::Time(this->get_clock()->now())); - std::cout << "HesaiInventory" << std::endl; - std::cout << result << std::endl; - info_model = result.get_str_model(); - info_serial = std::string(result.sn.begin(), result.sn.end()); - hw_interface_.SetTargetModel(result.model); - RCLCPP_INFO_STREAM(this->get_logger(), "Model:" << info_model); - RCLCPP_INFO_STREAM(this->get_logger(), "Serial:" << info_serial); - InitializeHesaiDiagnostics(); - return Status::OK; -} - rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::paramCallback( const std::vector & p) { @@ -926,7 +884,7 @@ void HesaiRosWrapper::InitializeHesaiDiagnostics() RCLCPP_INFO_STREAM(this->get_logger(), "InitializeHesaiDiagnostics"); using std::chrono_literals::operator""s; std::ostringstream os; - auto hardware_id = info_model + ": " + info_serial; + auto hardware_id = info_model_ + ": " + info_serial_; diagnostics_updater_.setHardwareID(hardware_id); RCLCPP_INFO_STREAM(this->get_logger(), "hardware_id: " + hardware_id); @@ -935,12 +893,12 @@ void HesaiRosWrapper::InitializeHesaiDiagnostics() diagnostics_updater_.add("hesai_temperature", this, &HesaiRosWrapper::HesaiCheckTemperature); diagnostics_updater_.add("hesai_rpm", this, &HesaiRosWrapper::HesaiCheckRpm); - current_status.reset(); - current_monitor.reset(); - current_status_time.reset(new rclcpp::Time(this->get_clock()->now())); - current_lidar_monitor_time.reset(new rclcpp::Time(this->get_clock()->now())); - current_diag_status = diagnostic_msgs::msg::DiagnosticStatus::STALE; - current_monitor_status = diagnostic_msgs::msg::DiagnosticStatus::STALE; + current_status_.reset(); + current_monitor_.reset(); + current_status_time_.reset(new rclcpp::Time(this->get_clock()->now())); + current_lidar_monitor_time_.reset(new rclcpp::Time(this->get_clock()->now())); + current_diag_status_ = diagnostic_msgs::msg::DiagnosticStatus::STALE; + current_monitor_status_ = diagnostic_msgs::msg::DiagnosticStatus::STALE; auto fetch_diag_from_sensor = [this]() { OnHesaiStatusTimer(); @@ -952,10 +910,7 @@ void HesaiRosWrapper::InitializeHesaiDiagnostics() }; fetch_diagnostics_timer_ = - std::make_shared>( - this->get_clock(), std::chrono::milliseconds(diag_span_), std::move(fetch_diag_from_sensor), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(fetch_diagnostics_timer_, cbg_m_); + create_wall_timer(std::chrono::milliseconds(diag_span_), std::move(fetch_diag_from_sensor)); if (hw_interface_.UseHttpGetLidarMonitor()) { diagnostics_updater_.add("hesai_voltage", this, &HesaiRosWrapper::HesaiCheckVoltageHttp); @@ -966,32 +921,30 @@ void HesaiRosWrapper::InitializeHesaiDiagnostics() auto on_timer_update = [this] { RCLCPP_DEBUG_STREAM(get_logger(), "OnUpdateTimer"); auto now = this->get_clock()->now(); - auto dif = (now - *current_status_time).seconds(); + auto dif = (now - *current_status_time_).seconds(); RCLCPP_DEBUG_STREAM(get_logger(), "dif(status): " << dif); if (diag_span_ * 2.0 < dif * 1000) { - current_diag_status = diagnostic_msgs::msg::DiagnosticStatus::STALE; + current_diag_status_ = diagnostic_msgs::msg::DiagnosticStatus::STALE; RCLCPP_DEBUG_STREAM(get_logger(), "STALE"); } else { - current_diag_status = diagnostic_msgs::msg::DiagnosticStatus::OK; + current_diag_status_ = diagnostic_msgs::msg::DiagnosticStatus::OK; RCLCPP_DEBUG_STREAM(get_logger(), "OK"); } - dif = (now - *current_lidar_monitor_time).seconds(); + dif = (now - *current_lidar_monitor_time_).seconds(); RCLCPP_DEBUG_STREAM(get_logger(), "dif(monitor): " << dif); if (diag_span_ * 2.0 < dif * 1000) { - current_monitor_status = diagnostic_msgs::msg::DiagnosticStatus::STALE; + current_monitor_status_ = diagnostic_msgs::msg::DiagnosticStatus::STALE; RCLCPP_DEBUG_STREAM(get_logger(), "STALE"); } else { - current_monitor_status = diagnostic_msgs::msg::DiagnosticStatus::OK; + current_monitor_status_ = diagnostic_msgs::msg::DiagnosticStatus::OK; RCLCPP_DEBUG_STREAM(get_logger(), "OK"); } diagnostics_updater_.force_update(); }; - diagnostics_update_timer_ = std::make_shared>( - this->get_clock(), std::chrono::milliseconds(1000), std::move(on_timer_update), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(diagnostics_update_timer_, cbg_r_); + diagnostics_update_timer_ = + create_wall_timer(std::chrono::milliseconds(1000), std::move(on_timer_update)); RCLCPP_DEBUG_STREAM(get_logger(), "add_timer"); } @@ -1003,7 +956,7 @@ std::string HesaiRosWrapper::GetPtreeValue( if (value) { return value.get(); } else { - return not_supported_message; + return MSG_NOT_SUPPORTED; } } std::string HesaiRosWrapper::GetFixedPrecisionString(double val, int pre) @@ -1018,9 +971,9 @@ void HesaiRosWrapper::OnHesaiStatusTimer() RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiStatusTimer" << std::endl); try { auto result = hw_interface_.GetLidarStatus(); - std::scoped_lock lock(mtx_status); - current_status_time.reset(new rclcpp::Time(this->get_clock()->now())); - current_status.reset(new HesaiLidarStatus(result)); + std::scoped_lock lock(mtx_lidar_status_); + current_status_time_.reset(new rclcpp::Time(this->get_clock()->now())); + current_status_.reset(new HesaiLidarStatus(result)); } catch (const std::system_error & error) { RCLCPP_ERROR_STREAM( rclcpp::get_logger("HesaiRosWrapper::OnHesaiStatusTimer(std::system_error)"), error.what()); @@ -1037,9 +990,9 @@ void HesaiRosWrapper::OnHesaiLidarMonitorTimerHttp() RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiLidarMonitorTimerHttp"); try { hw_interface_.GetLidarMonitorAsyncHttp([this](const std::string & str) { - std::scoped_lock lock(mtx_lidar_monitor); - current_lidar_monitor_time.reset(new rclcpp::Time(this->get_clock()->now())); - current_lidar_monitor_tree = + std::scoped_lock lock(mtx_lidar_monitor_); + current_lidar_monitor_time_.reset(new rclcpp::Time(this->get_clock()->now())); + current_lidar_monitor_tree_ = std::make_unique(hw_interface_.ParseJson(str)); }); } catch (const std::system_error & error) { @@ -1059,11 +1012,10 @@ void HesaiRosWrapper::OnHesaiLidarMonitorTimer() { RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiLidarMonitorTimer"); try { - auto ios = std::make_shared(); auto result = hw_interface_.GetLidarMonitor(); - std::scoped_lock lock(mtx_lidar_monitor); - current_lidar_monitor_time.reset(new rclcpp::Time(this->get_clock()->now())); - current_monitor.reset(new HesaiLidarMonitor(result)); + std::scoped_lock lock(mtx_lidar_monitor_); + current_lidar_monitor_time_.reset(new rclcpp::Time(this->get_clock()->now())); + current_monitor_.reset(new HesaiLidarMonitor(result)); } catch (const std::system_error & error) { RCLCPP_ERROR_STREAM( rclcpp::get_logger("HesaiRosWrapper::OnHesaiLidarMonitorTimer(std::system_error)"), @@ -1078,14 +1030,14 @@ void HesaiRosWrapper::OnHesaiLidarMonitorTimer() void HesaiRosWrapper::HesaiCheckStatus(diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - std::scoped_lock lock(mtx_status); - if (current_status) { + std::scoped_lock lock(mtx_lidar_status_); + if (current_status_) { uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::vector msg; - diagnostics.add("system_uptime", std::to_string(current_status->system_uptime)); - diagnostics.add("startup_times", std::to_string(current_status->startup_times)); - diagnostics.add("total_operation_time", std::to_string(current_status->total_operation_time)); + diagnostics.add("system_uptime", std::to_string(current_status_->system_uptime)); + diagnostics.add("startup_times", std::to_string(current_status_->startup_times)); + diagnostics.add("total_operation_time", std::to_string(current_status_->total_operation_time)); diagnostics.summary(level, boost::algorithm::join(msg, ", ")); } else { @@ -1095,13 +1047,13 @@ void HesaiRosWrapper::HesaiCheckStatus(diagnostic_updater::DiagnosticStatusWrapp void HesaiRosWrapper::HesaiCheckPtp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - std::scoped_lock lock(mtx_status); - if (current_status) { + std::scoped_lock lock(mtx_lidar_status_); + if (current_status_) { uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::vector msg; - auto gps_status = current_status->get_str_gps_pps_lock(); - auto gprmc_status = current_status->get_str_gps_gprmc_status(); - auto ptp_status = current_status->get_str_ptp_clock_status(); + auto gps_status = current_status_->get_str_gps_pps_lock(); + auto gprmc_status = current_status_->get_str_gps_gprmc_status(); + auto ptp_status = current_status_->get_str_ptp_clock_status(); std::transform(gps_status.cbegin(), gps_status.cend(), gps_status.begin(), toupper); std::transform(gprmc_status.cbegin(), gprmc_status.cend(), gprmc_status.begin(), toupper); std::transform(ptp_status.cbegin(), ptp_status.cend(), ptp_status.begin(), toupper); @@ -1129,13 +1081,13 @@ void HesaiRosWrapper::HesaiCheckPtp(diagnostic_updater::DiagnosticStatusWrapper void HesaiRosWrapper::HesaiCheckTemperature( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - std::scoped_lock lock(mtx_status); - if (current_status) { + std::scoped_lock lock(mtx_lidar_status_); + if (current_status_) { uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::vector msg; - for (size_t i = 0; i < current_status->temperature.size(); i++) { + for (size_t i = 0; i < current_status_->temperature.size(); i++) { diagnostics.add( - temperature_names[i], GetFixedPrecisionString(current_status->temperature[i] * 0.01)); + temperature_names_[i], GetFixedPrecisionString(current_status_->temperature[i] * 0.01)); } diagnostics.summary(level, boost::algorithm::join(msg, ", ")); } else { @@ -1145,11 +1097,11 @@ void HesaiRosWrapper::HesaiCheckTemperature( void HesaiRosWrapper::HesaiCheckRpm(diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - std::scoped_lock lock(mtx_status); - if (current_status) { + std::scoped_lock lock(mtx_lidar_status_); + if (current_status_) { uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::vector msg; - diagnostics.add("motor_speed", std::to_string(current_status->motor_speed)); + diagnostics.add("motor_speed", std::to_string(current_status_->motor_speed)); diagnostics.summary(level, boost::algorithm::join(msg, ", ")); } else { @@ -1160,8 +1112,8 @@ void HesaiRosWrapper::HesaiCheckRpm(diagnostic_updater::DiagnosticStatusWrapper void HesaiRosWrapper::HesaiCheckVoltageHttp( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - std::scoped_lock lock(mtx_lidar_monitor); - if (current_lidar_monitor_tree) { + std::scoped_lock lock(mtx_lidar_monitor_); + if (current_lidar_monitor_tree_) { uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::vector msg; std::string key = ""; @@ -1169,18 +1121,18 @@ void HesaiRosWrapper::HesaiCheckVoltageHttp( std::string mes; key = "lidarInCur"; try { - mes = GetPtreeValue(current_lidar_monitor_tree.get(), "Body." + key); + mes = GetPtreeValue(current_lidar_monitor_tree_.get(), "Body." + key); } catch (boost::bad_lexical_cast & ex) { level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message + std::string(ex.what()); + mes = MSG_ERROR + std::string(ex.what()); } diagnostics.add(key, mes); key = "lidarInVol"; try { - mes = GetPtreeValue(current_lidar_monitor_tree.get(), "Body." + key); + mes = GetPtreeValue(current_lidar_monitor_tree_.get(), "Body." + key); } catch (boost::bad_lexical_cast & ex) { level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message + std::string(ex.what()); + mes = MSG_ERROR + std::string(ex.what()); } diagnostics.add(key, mes); @@ -1192,16 +1144,16 @@ void HesaiRosWrapper::HesaiCheckVoltageHttp( void HesaiRosWrapper::HesaiCheckVoltage(diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - std::scoped_lock lock(mtx_lidar_monitor); - if (current_monitor) { + std::scoped_lock lock(mtx_lidar_monitor_); + if (current_monitor_) { uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::vector msg; diagnostics.add( - "input_voltage", GetFixedPrecisionString(current_monitor->input_voltage * 0.01) + " V"); + "input_voltage", GetFixedPrecisionString(current_monitor_->input_voltage * 0.01) + " V"); diagnostics.add( - "input_current", GetFixedPrecisionString(current_monitor->input_current * 0.01) + " mA"); + "input_current", GetFixedPrecisionString(current_monitor_->input_current * 0.01) + " mA"); diagnostics.add( - "input_power", GetFixedPrecisionString(current_monitor->input_power * 0.01) + " W"); + "input_power", GetFixedPrecisionString(current_monitor_->input_power * 0.01) + " W"); diagnostics.summary(level, boost::algorithm::join(msg, ", ")); } else { From 2cebae99f27964b71472b6e188bf64c7069589be Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 28 Mar 2024 20:10:51 +0900 Subject: [PATCH 20/57] fix(hesai_hw_interface): add error handling * check PTC command error codes, throw exception if necessary * perform size checks before parsing responses * emit errors on too-large payload size --- .../nebula_hw_interfaces_hesai/hesai_cmd_response.hpp | 2 +- .../nebula_hw_interfaces_hesai/hesai_hw_interface.hpp | 1 + .../src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp | 1 - 3 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp index a6d78c45e..2691def33 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp @@ -124,7 +124,7 @@ struct HesaiPtpDiagGrandmaster { os << "clockQuality: " << arg.clockQuality; os << ", "; - os << "utc_offset: " << arg.utc_offset; + os << "utc_offset: " << static_cast(arg.utc_offset.value()); os << ", "; os << "time_flags: " << +arg.time_flags; os << ", "; diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp index d71a26998..84750bcb2 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp @@ -102,6 +102,7 @@ const int PTP_LOG_MIN_DELAY_INTERVAL = 0; const int HESAI_LIDAR_GPS_CLOCK_SOURCE = 0; const int HESAI_LIDAR_PTP_CLOCK_SOURCE = 1; + /// @brief Hardware interface of hesai driver class HesaiHwInterface : NebulaHwInterfaceBase { diff --git a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp index e37d42a43..c70bd4400 100644 --- a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp @@ -68,7 +68,6 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( auto recv_buf = std::make_shared>(); auto response_complete = std::make_shared(false); - // Low byte is for PTC error code, the rest is nebula-specific auto error_code = std::make_shared(); std::stringstream ss; From d039f12efe5e1f3c88ac3b24ecc7fa3ac942326d Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Mon, 8 Apr 2024 15:14:44 +0900 Subject: [PATCH 21/57] fix(hesai_hw_monitor_ros_wrapper): fixed wrong range given for S/N copy --- ros2_socketcan | 1 + transport_drivers | 1 + 2 files changed, 2 insertions(+) create mode 160000 ros2_socketcan create mode 160000 transport_drivers diff --git a/ros2_socketcan b/ros2_socketcan new file mode 160000 index 000000000..4ced5284f --- /dev/null +++ b/ros2_socketcan @@ -0,0 +1 @@ +Subproject commit 4ced5284fdd080274b04530406d887e8330a0cdb diff --git a/transport_drivers b/transport_drivers new file mode 160000 index 000000000..abf8aa8e1 --- /dev/null +++ b/transport_drivers @@ -0,0 +1 @@ +Subproject commit abf8aa8e171f33e0acd6d845c3d795192c964e9c From 5a5a6a92df9fcafbc274cb757970479ddc8e55c2 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Mon, 8 Apr 2024 16:26:42 +0900 Subject: [PATCH 22/57] fix(hesai): print uint8, uint16 as numbers --- .../nebula_hw_interfaces_hesai/hesai_cmd_response.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp index 2691def33..a6d78c45e 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp @@ -124,7 +124,7 @@ struct HesaiPtpDiagGrandmaster { os << "clockQuality: " << arg.clockQuality; os << ", "; - os << "utc_offset: " << static_cast(arg.utc_offset.value()); + os << "utc_offset: " << arg.utc_offset; os << ", "; os << "time_flags: " << +arg.time_flags; os << ", "; From 5a11cf92a5cd38dcfc767326e96d1e737975b3ee Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 9 Apr 2024 18:45:41 +0900 Subject: [PATCH 23/57] feat(hesai_ros_wrapper): publish/subscribe to legacy pandar_packets on demand --- .../hesai_hw_interface.hpp | 8 +- .../hesai_hw_interface.cpp | 70 ++---------- .../nebula_ros/hesai/hesai_ros_wrapper.hpp | 52 +++++---- .../include/nebula_ros/hesai/mt_queue.hpp | 13 ++- nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 101 +++++++++++++----- 5 files changed, 124 insertions(+), 120 deletions(-) diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp index 84750bcb2..0bf3a864b 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp @@ -104,7 +104,7 @@ const int HESAI_LIDAR_PTP_CLOCK_SOURCE = 1; /// @brief Hardware interface of hesai driver -class HesaiHwInterface : NebulaHwInterfaceBase +class HesaiHwInterface : public NebulaHwInterfaceBase { private: struct ptc_error_t @@ -122,12 +122,6 @@ class HesaiHwInterface : NebulaHwInterfaceBase std::unique_ptr<::drivers::udp_driver::UdpDriver> cloud_udp_driver_; std::shared_ptr<::drivers::tcp_driver::TcpDriver> tcp_driver_; std::shared_ptr sensor_configuration_; - std::shared_ptr calibration_configuration_; - size_t azimuth_index_{}; - size_t mtu_size_{}; - std::unique_ptr scan_cloud_ptr_; - std::function - is_valid_packet_; /*Lambda Function Array to verify proper packet size*/ std::function & buffer)> cloud_packet_callback_; /**This function pointer is called when the scan is complete*/ diff --git a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp index c70bd4400..451b2a9d3 100644 --- a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp @@ -17,8 +17,7 @@ HesaiHwInterface::HesaiHwInterface() : cloud_io_context_{new ::drivers::common::IoContext(1)}, m_owned_ctx{new boost::asio::io_context(1)}, cloud_udp_driver_{new ::drivers::udp_driver::UdpDriver(*cloud_io_context_)}, - tcp_driver_{new ::drivers::tcp_driver::TcpDriver(m_owned_ctx)}, - scan_cloud_ptr_{std::make_unique()} + tcp_driver_{new ::drivers::tcp_driver::TcpDriver(m_owned_ctx)} { } HesaiHwInterface::~HesaiHwInterface() @@ -145,63 +144,8 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( Status HesaiHwInterface::SetSensorConfiguration( std::shared_ptr sensor_configuration) { - HesaiStatus status = Status::OK; - mtu_size_ = MTU_SIZE; - is_solid_state = false; - try { - sensor_configuration_ = - std::static_pointer_cast(sensor_configuration); - if ( - sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR40P || - sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR40P) { - azimuth_index_ = 2; - is_valid_packet_ = [](size_t packet_size) { - return ( - packet_size == PANDAR40_PACKET_SIZE || packet_size == PANDAR40P_EXTENDED_PACKET_SIZE); - }; - } else if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARQT64) { - azimuth_index_ = 12; // 12 + 258 * [0-3] - is_valid_packet_ = [](size_t packet_size) { return (packet_size == PANDARQT64_PACKET_SIZE); }; - } else if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARQT128) { - azimuth_index_ = 12; // 12 + 514 * [0-1] - is_valid_packet_ = [](size_t packet_size) { - return (packet_size == PANDARQT128_PACKET_SIZE); - }; - } else if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARXT32) { - azimuth_index_ = 12; // 12 + 130 * [0-7] - is_valid_packet_ = [](size_t packet_size) { return (packet_size == PANDARXT32_PACKET_SIZE); }; - } else if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARXT32M) { - azimuth_index_ = 12; // 12 + 130 * [0-7] - is_valid_packet_ = [](size_t packet_size) { - return (packet_size == PANDARXT32M_PACKET_SIZE); - }; - } else if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128) { - azimuth_index_ = 12; // 12 + 4 * 128 * [0-1] - is_solid_state = true; - is_valid_packet_ = [](size_t packet_size) { - return (packet_size == PANDARAT128_PACKET_SIZE); - }; - } else if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR64) { - azimuth_index_ = 8; // 8 + 192 * [0-5] - is_valid_packet_ = [](size_t packet_size) { - return ( - packet_size == PANDAR64_PACKET_SIZE || packet_size == PANDAR64_EXTENDED_PACKET_SIZE); - }; - } else if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR128_E4X) { - azimuth_index_ = 12; // 12 - is_valid_packet_ = [](size_t packet_size) { - return ( - packet_size == PANDAR128_E4X_EXTENDED_PACKET_SIZE || - packet_size == PANDAR128_E4X_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; - } + sensor_configuration_ = + std::static_pointer_cast(sensor_configuration); return Status::OK; } @@ -246,10 +190,6 @@ Status HesaiHwInterface::RegisterScanCallback( void HesaiHwInterface::ReceiveSensorPacketCallback(std::vector & buffer) { - // if (!is_valid_packet_(buffer.size())) { - // PrintDebug("Invalid Packet: " + std::to_string(buffer.size())); - // return; - // } TODO cloud_packet_callback_(buffer); } Status HesaiHwInterface::SensorInterfaceStop() @@ -301,7 +241,9 @@ Status HesaiHwInterface::InitializeTcpDriver() Status HesaiHwInterface::FinalizeTcpDriver() { try { - tcp_driver_->close(); + if (tcp_driver_) { + tcp_driver_->close(); + } } catch (std::exception & e) { PrintError("Error while finalizing the TcpDriver"); return Status::UDP_CONNECTION_ERROR; diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp index 2ba2e2407..9c50e20eb 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp @@ -16,6 +16,8 @@ #include #include "nebula_msgs/msg/nebula_packet.hpp" +#include "pandar_msgs/msg/pandar_packet.hpp" +#include "pandar_msgs/msg/pandar_scan.hpp" #include #include @@ -105,6 +107,10 @@ class HesaiRosWrapper final : public rclcpp::Node /// @param scan_buffer Received PandarScan void ReceiveCloudPacketCallback(std::vector & scan_buffer); + /// @brief Callback for receiving replayed, aggregated packets (= scan messages) + /// @param scan_msg + void ReceiveScanMessageCallback(std::unique_ptr scan_msg); + /// @brief Decodes a nebula packet and, if it completes the scan, publishes the pointcloud. /// @param packet_msg The received packet message void ProcessCloudPacket(std::unique_ptr packet_msg); @@ -158,19 +164,23 @@ class HesaiRosWrapper final : public rclcpp::Node /// @return Created string std::string GetFixedPrecisionString(double val, int pre = 2); - std::shared_ptr driver_ptr_; - drivers::HesaiHwInterface hw_interface_; + std::shared_ptr driver_ptr_{}; + drivers::HesaiHwInterface hw_interface_{}; Status wrapper_status_; Status interface_status_; - rclcpp::Publisher::SharedPtr nebula_points_pub_; - rclcpp::Publisher::SharedPtr aw_points_ex_pub_; - rclcpp::Publisher::SharedPtr aw_points_base_pub_; + rclcpp::Subscription::SharedPtr packets_sub_{}; + rclcpp::Publisher::SharedPtr packets_pub_{}; + pandar_msgs::msg::PandarScan::UniquePtr current_scan_msg_{}; + + 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_; - std::shared_ptr correction_cfg_ptr_; + std::shared_ptr calibration_cfg_ptr_{}; + std::shared_ptr sensor_cfg_ptr_{}; + std::shared_ptr correction_cfg_ptr_{}; /// @brief Stores received packets that have not been processed yet by the decoder thread mt_queue> packet_queue_; @@ -186,27 +196,27 @@ class HesaiRosWrapper final : public rclcpp::Node std::string correction_file_path; /// @brief Received Hesai message publisher - rclcpp::Publisher::SharedPtr pandar_scan_pub_; + rclcpp::Publisher::SharedPtr pandar_scan_pub_{}; bool retry_hw_; std::mutex mtx_config_; - OnSetParametersCallbackHandle::SharedPtr set_param_res_; + OnSetParametersCallbackHandle::SharedPtr set_param_res_{}; diagnostic_updater::Updater diagnostics_updater_; - rclcpp::TimerBase::SharedPtr diagnostics_update_timer_; - rclcpp::TimerBase::SharedPtr fetch_diagnostics_timer_; + rclcpp::TimerBase::SharedPtr diagnostics_update_timer_{}; + rclcpp::TimerBase::SharedPtr fetch_diagnostics_timer_{}; - std::unique_ptr current_status_; - std::unique_ptr current_monitor_; - std::unique_ptr current_config_; - std::unique_ptr current_inventory_; - std::unique_ptr current_lidar_monitor_tree_; + std::unique_ptr current_status_{}; + std::unique_ptr current_monitor_{}; + std::unique_ptr current_config_{}; + std::unique_ptr current_inventory_{}; + std::unique_ptr current_lidar_monitor_tree_{}; - std::unique_ptr current_status_time_; - std::unique_ptr current_config_time_; - std::unique_ptr current_inventory_time_; - std::unique_ptr current_lidar_monitor_time_; + std::unique_ptr current_status_time_{}; + std::unique_ptr current_config_time_{}; + std::unique_ptr current_inventory_time_{}; + std::unique_ptr current_lidar_monitor_time_{}; uint8_t current_diag_status_; uint8_t current_monitor_status_; diff --git a/nebula_ros/include/nebula_ros/hesai/mt_queue.hpp b/nebula_ros/include/nebula_ros/hesai/mt_queue.hpp index 730b54f7c..67b287ddc 100644 --- a/nebula_ros/include/nebula_ros/hesai/mt_queue.hpp +++ b/nebula_ros/include/nebula_ros/hesai/mt_queue.hpp @@ -16,7 +16,7 @@ class mt_queue public: mt_queue(size_t capacity) : capacity_(capacity) {} - bool push(T && value) + bool try_push(T && value) { { std::unique_lock lock(this->mutex_); @@ -29,6 +29,17 @@ class mt_queue this->condition_variable_.notify_one(); return true; } + + void push(T && value) + { + { + std::unique_lock lock(this->mutex_); + this->condition_variable_.wait(lock, [=] { return this->queue_.size() < this->capacity_; }); + queue_.push_front(std::move(value)); + } + this->condition_variable_.notify_one(); + } + T pop() { std::unique_lock lock(this->mutex_); diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index f0e6c121a..0fe4e7bd3 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -7,26 +7,29 @@ namespace ros HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) : rclcpp::Node("hesai_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)), hw_interface_(), + wrapper_status_(Status::NOT_INITIALIZED), + interface_status_(Status::NOT_INITIALIZED), packet_queue_(3000), diagnostics_updater_(this) { setvbuf(stdout, NULL, _IONBF, BUFSIZ); - drivers::HesaiSensorConfiguration sensor_configuration; - wrapper_status_ = GetParameters(sensor_configuration); - sensor_cfg_ptr_ = std::make_shared(sensor_configuration); - // DeclareRosParameters(); - // auto sensor_cfg_ptr = GetRosParameters(); + sensor_cfg_ptr_ = std::make_shared(); + wrapper_status_ = GetParameters(*sensor_cfg_ptr_); + InitializeHwInterface(); InitializeDecoder(); - InitializeHwMonitor(); - // StreamStart(); - RCLCPP_DEBUG(this->get_logger(), "Starting stream"); - StreamStart(); - hw_interface_.RegisterScanCallback( - std::bind(&HesaiRosWrapper::ReceiveCloudPacketCallback, this, std::placeholders::_1)); + + if (launch_hw_) { + InitializeHwMonitor(); + StreamStart(); + hw_interface_.RegisterScanCallback( + std::bind(&HesaiRosWrapper::ReceiveCloudPacketCallback, this, std::placeholders::_1)); + } else { + packets_sub_ = create_subscription("pandar_packets", rclcpp::SensorDataQoS(), std::bind(&HesaiRosWrapper::ReceiveScanMessageCallback, this, std::placeholders::_1)); + } set_param_res_ = this->add_on_set_parameters_callback( std::bind(&HesaiRosWrapper::paramCallback, this, std::placeholders::_1)); @@ -34,29 +37,31 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) Status HesaiRosWrapper::InitializeHwInterface() { - if (Status::OK != interface_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); - return interface_status_; - } + interface_status_ = Status::OK; + hw_interface_.SetLogger(std::make_shared(this->get_logger())); hw_interface_.SetSensorConfiguration( std::static_pointer_cast(sensor_cfg_ptr_)); - - Status status; + hw_interface_.SetTargetModel(sensor_cfg_ptr_->sensor_model); + + if (!launch_hw_) { + return interface_status_; + } + int retry_count = 0; while (true) { - status = hw_interface_.InitializeTcpDriver(); - if (status == Status::OK || !retry_hw_) { + interface_status_ = hw_interface_.InitializeTcpDriver(); + if (interface_status_ == Status::OK || !retry_hw_) { break; } retry_count++; std::this_thread::sleep_for(std::chrono::milliseconds(8000)); // >5000 RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Retry: " << retry_count); - }; + } - if (status == Status::OK) { + if (interface_status_ == Status::OK) { try { auto inventory = hw_interface_.GetInventory(); RCLCPP_INFO_STREAM(get_logger(), inventory); @@ -72,9 +77,12 @@ Status HesaiRosWrapper::InitializeHwInterface() RCLCPP_ERROR_STREAM( get_logger(), "Failed to get model from sensor... Set from config: " << sensor_cfg_ptr_->sensor_model); - hw_interface_.SetTargetModel(sensor_cfg_ptr_->sensor_model); } + pandar_scan_pub_ = + create_publisher("pandar_packets", rclcpp::SensorDataQoS()); + current_scan_msg_ = std::make_unique(); + return Status::OK; } @@ -191,13 +199,32 @@ void HesaiRosWrapper::ReceiveCloudPacketCallback(std::vector & packet) // delay.tick(msg_ptr->stamp); // publish.tick(); - if (!packet_queue_.push(std::move(msg_ptr))) { + if (!packet_queue_.try_push(std::move(msg_ptr))) { RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packets dropped"); } // packet_pub_->publish(std::move(msg_ptr)); // publish.tock(); } +void HesaiRosWrapper::ReceiveScanMessageCallback( + std::unique_ptr scan_msg) +{ + if (launch_hw_) { + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 1000, + "Ignoring received PandarScan. Launch with launch_hw:=false to enable PandarScan replay."); + return; + } + + for (auto & pkt : scan_msg->packets) { + auto nebula_pkt_ptr = std::make_unique(); + nebula_pkt_ptr->stamp = pkt.stamp; + std::copy(pkt.data.begin(), pkt.data.end(), std::back_inserter(nebula_pkt_ptr->data)); + + packet_queue_.push(std::move(nebula_pkt_ptr)); + } +} + void HesaiRosWrapper::ProcessCloudPacket(std::unique_ptr packet_msg) { // static auto parse = nebula::util::Instrumentation("ProcessCloudPacket.parse"); @@ -207,6 +234,22 @@ void HesaiRosWrapper::ProcessCloudPacket(std::unique_ptrstamp); + // Accumulate packets for recording only if someone is subscribed to the topic (for performance) + if ( + pandar_scan_pub_ && + (pandar_scan_pub_->get_subscription_count() > 0 || + pandar_scan_pub_->get_intra_process_subscription_count() > 0)) { + if (current_scan_msg_->packets.size() == 0) { + current_scan_msg_->header.stamp = packet_msg->stamp; + } + + pandar_msgs::msg::PandarPacket pandar_packet_msg{}; + pandar_packet_msg.stamp = packet_msg->stamp; + pandar_packet_msg.size = packet_msg->data.size(); + std::copy(packet_msg->data.begin(), packet_msg->data.end(), pandar_packet_msg.data.begin()); + current_scan_msg_->packets.emplace_back(std::move(pandar_packet_msg)); + } + // parse.tick(); std::tuple pointcloud_ts = driver_ptr_->ParseCloudPacket(packet_msg->data); @@ -218,6 +261,13 @@ void HesaiRosWrapper::ProcessCloudPacket(std::unique_ptrpackets.empty()) { + pandar_scan_pub_->publish(std::move(current_scan_msg_)); + current_scan_msg_ = std::make_unique(); + } + if ( nebula_points_pub_->get_subscription_count() > 0 || nebula_points_pub_->get_intra_process_subscription_count() > 0) { @@ -577,11 +627,8 @@ Status HesaiRosWrapper::GetParameters(drivers::HesaiSensorConfiguration & sensor return Status::SENSOR_CONFIG_ERROR; } - std::shared_ptr sensor_cfg_ptr = - std::make_shared(sensor_configuration); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); + std::static_pointer_cast(sensor_cfg_ptr_)); RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); return Status::OK; From dc50d12e92b05f44af4df20ea9ea93b9ed6ad386 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 9 Apr 2024 18:46:05 +0900 Subject: [PATCH 24/57] change launch file back to single-threaded container --- nebula_ros/launch/hesai_launch_all_hw.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_ros/launch/hesai_launch_all_hw.xml b/nebula_ros/launch/hesai_launch_all_hw.xml index b7c1459ec..1bd285766 100644 --- a/nebula_ros/launch/hesai_launch_all_hw.xml +++ b/nebula_ros/launch/hesai_launch_all_hw.xml @@ -33,7 +33,7 @@ - From 9aaedbcbc8af7def23829b5d9c55cfb2c7ea28fc Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 10 Apr 2024 12:23:36 +0900 Subject: [PATCH 25/57] attempt to make queue contention less bad --- .../include/nebula_ros/hesai/mt_queue.hpp | 37 +++++++++++-------- nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 12 ++++-- 2 files changed, 30 insertions(+), 19 deletions(-) diff --git a/nebula_ros/include/nebula_ros/hesai/mt_queue.hpp b/nebula_ros/include/nebula_ros/hesai/mt_queue.hpp index 67b287ddc..a259a80ba 100644 --- a/nebula_ros/include/nebula_ros/hesai/mt_queue.hpp +++ b/nebula_ros/include/nebula_ros/hesai/mt_queue.hpp @@ -9,7 +9,7 @@ class mt_queue { private: std::mutex mutex_; - std::condition_variable condition_variable_; + std::condition_variable cv_not_empty_, cv_not_full_; std::deque queue_; size_t capacity_; @@ -18,34 +18,39 @@ class mt_queue bool try_push(T && value) { - { - std::unique_lock lock(this->mutex_); - if (queue_.size() == capacity_) { - return false; - } - + std::unique_lock lock(this->mutex_); + bool can_push = queue_.size() < capacity_; + if (can_push) { queue_.push_front(std::move(value)); } - this->condition_variable_.notify_one(); - return true; + this->cv_not_empty_.notify_all(); + return can_push; } void push(T && value) { - { - std::unique_lock lock(this->mutex_); - this->condition_variable_.wait(lock, [=] { return this->queue_.size() < this->capacity_; }); - queue_.push_front(std::move(value)); - } - this->condition_variable_.notify_one(); + std::unique_lock lock(this->mutex_); + this->cv_not_full_.wait(lock, [=] { return this->queue_.size() < this->capacity_; }); + queue_.push_front(std::move(value)); + this->cv_not_empty_.notify_all(); } T pop() { std::unique_lock lock(this->mutex_); - this->condition_variable_.wait(lock, [=] { return !this->queue_.empty(); }); + this->cv_not_empty_.wait(lock, [=] { return !this->queue_.empty(); }); T return_value(std::move(this->queue_.back())); this->queue_.pop_back(); + this->cv_not_full_.notify_all(); + return return_value; } + + void pop_all(std::deque & dest) { + std::unique_lock lock(this->mutex_); + this->cv_not_empty_.wait(lock, [=] { return !this->queue_.empty(); }); + dest.insert(dest.end(), std::make_move_iterator(queue_.begin()), std::make_move_iterator(queue_.end())); + this->queue_.clear(); + this->cv_not_full_.notify_all(); + } }; \ No newline at end of file diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index 0fe4e7bd3..6fdc46a1a 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -104,9 +104,15 @@ Status HesaiRosWrapper::InitializeDecoder() rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); decoder_thread_ = std::thread([this]() { + std::deque> received{}; while (true) { - auto pkt = packet_queue_.pop(); - this->ProcessCloudPacket(std::move(pkt)); + packet_queue_.pop_all(received); + + while (!received.empty()) { + auto pkt = std::move(received.back()); + received.pop_back(); + this->ProcessCloudPacket(std::move(pkt)); + } } }); @@ -200,7 +206,7 @@ void HesaiRosWrapper::ReceiveCloudPacketCallback(std::vector & packet) // publish.tick(); if (!packet_queue_.try_push(std::move(msg_ptr))) { - RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packets dropped"); + RCLCPP_ERROR(get_logger(), "Packet dropped"); } // packet_pub_->publish(std::move(msg_ptr)); // publish.tock(); From a0d24b9f668387f0fab2428f77a30231e4e29b8c Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 11 Apr 2024 19:00:25 +0900 Subject: [PATCH 26/57] chore(hesai_ros_wrapper): reduce logging output --- nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index 6fdc46a1a..7a0bf34f0 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -206,7 +206,7 @@ void HesaiRosWrapper::ReceiveCloudPacketCallback(std::vector & packet) // publish.tick(); if (!packet_queue_.try_push(std::move(msg_ptr))) { - RCLCPP_ERROR(get_logger(), "Packet dropped"); + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped"); } // packet_pub_->publish(std::move(msg_ptr)); // publish.tock(); From dab6795f1acff06da5209301db8a2d163346bd91 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 11 Apr 2024 19:18:35 +0900 Subject: [PATCH 27/57] feat(hesai_ros_wrapper): print warning when connected to HW and receiving /pandar_packets --- nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 16 +++++----------- 1 file changed, 5 insertions(+), 11 deletions(-) diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index 7a0bf34f0..23205ea64 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -27,10 +27,11 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) StreamStart(); hw_interface_.RegisterScanCallback( std::bind(&HesaiRosWrapper::ReceiveCloudPacketCallback, this, std::placeholders::_1)); - } else { - packets_sub_ = create_subscription("pandar_packets", rclcpp::SensorDataQoS(), std::bind(&HesaiRosWrapper::ReceiveScanMessageCallback, this, std::placeholders::_1)); } + packets_sub_ = create_subscription("pandar_packets", rclcpp::SensorDataQoS(), + std::bind(&HesaiRosWrapper::ReceiveScanMessageCallback, this, std::placeholders::_1)); + set_param_res_ = this->add_on_set_parameters_callback( std::bind(&HesaiRosWrapper::paramCallback, this, std::placeholders::_1)); } @@ -104,15 +105,8 @@ Status HesaiRosWrapper::InitializeDecoder() rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); decoder_thread_ = std::thread([this]() { - std::deque> received{}; while (true) { - packet_queue_.pop_all(received); - - while (!received.empty()) { - auto pkt = std::move(received.back()); - received.pop_back(); - this->ProcessCloudPacket(std::move(pkt)); - } + this->ProcessCloudPacket(std::move(packet_queue_.pop())); } }); @@ -242,7 +236,7 @@ void HesaiRosWrapper::ProcessCloudPacket(std::unique_ptrget_subscription_count() > 0 || pandar_scan_pub_->get_intra_process_subscription_count() > 0)) { if (current_scan_msg_->packets.size() == 0) { From d26473ccef056bf5bb684292350348e271cf229a Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Fri, 12 Apr 2024 14:43:27 +0900 Subject: [PATCH 28/57] feat(nebula_launch.py): throw error when trying to launch Hesai sensor and refer to hesai_launch_all_hw.xml --- nebula_ros/launch/nebula_launch.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/nebula_ros/launch/nebula_launch.py b/nebula_ros/launch/nebula_launch.py index b394498fb..74d3eb475 100644 --- a/nebula_ros/launch/nebula_launch.py +++ b/nebula_ros/launch/nebula_launch.py @@ -41,8 +41,11 @@ def launch_setup(context, *args, **kwargs): # Model and make sensor_model = LaunchConfiguration("sensor_model").perform(context) calibration_file = LaunchConfiguration("calibration_file").perform(context) - correction_file = LaunchConfiguration("correction_file").perform(context) sensor_make, sensor_extension = get_sensor_make(sensor_model) + + if sensor_make.lower() == "hesai": + raise ValueError("\n `nebula_launch.py` is deprecated. For Hesai sensors, use `hesai_launch_all_hw.xml` instead.") + nebula_decoders_share_dir = get_package_share_directory("nebula_decoders") nebula_ros_share_dir = get_package_share_directory("nebula_ros") @@ -84,7 +87,6 @@ def launch_setup(context, *args, **kwargs): "sensor_ip": LaunchConfiguration("sensor_ip"), "return_mode": LaunchConfiguration("return_mode"), "calibration_file": calibration_file or sensor_calib_fp, - "correction_file": correction_file or sensor_corr_fp, "setup_sensor": LaunchConfiguration("setup_sensor"), "ptp_profile": LaunchConfiguration("ptp_profile"), "ptp_domain": LaunchConfiguration("ptp_domain"), @@ -110,7 +112,6 @@ def launch_setup(context, *args, **kwargs): "sensor_ip": LaunchConfiguration("sensor_ip"), "return_mode": LaunchConfiguration("return_mode"), "calibration_file": calibration_file or sensor_calib_fp, - "correction_file": correction_file or sensor_corr_fp, }, ], ) @@ -127,7 +128,6 @@ def launch_setup(context, *args, **kwargs): "sensor_ip": LaunchConfiguration("sensor_ip"), "return_mode": LaunchConfiguration("return_mode"), "calibration_file": calibration_file or sensor_calib_fp, - "correction_file": correction_file or sensor_corr_fp, "launch_hw": LaunchConfiguration("launch_hw"), "ptp_profile": LaunchConfiguration("ptp_profile"), "ptp_domain": LaunchConfiguration("ptp_domain"), From 91359d98c327f58bdec4e0e1b5e9f4654f8f4b38 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 18 Apr 2024 16:38:39 +0900 Subject: [PATCH 29/57] refactor(nebula_ros): split single wrapper file into 3 sub-wrappers --- .cspell.json | 27 +- .../nebula_common/hesai/hesai_common.hpp | 35 +- .../include/nebula_common/util/expected.hpp | 5 +- .../decoders/hesai_packet.hpp | 2 +- .../nebula_decoders_hesai/hesai_driver.hpp | 15 +- .../nebula_decoders_hesai/hesai_driver.cpp | 55 +- .../hesai_hw_interface.cpp | 24 +- nebula_ros/CMakeLists.txt | 4 + .../nebula_ros/hesai/decoder_wrapper.hpp | 66 + .../nebula_ros/hesai/hesai_ros_wrapper.hpp | 181 +-- .../nebula_ros/hesai/hw_interface_wrapper.hpp | 32 + .../nebula_ros/hesai/hw_monitor_wrapper.hpp | 95 ++ .../include/nebula_ros/hesai/mt_queue.hpp | 9 +- nebula_ros/package.xml | 1 + nebula_ros/src/hesai/decoder_wrapper.cpp | 255 ++++ nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 1159 +++-------------- nebula_ros/src/hesai/hw_interface_wrapper.cpp | 94 ++ nebula_ros/src/hesai/hw_monitor_wrapper.cpp | 412 ++++++ 18 files changed, 1268 insertions(+), 1203 deletions(-) create mode 100644 nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp create mode 100644 nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp create mode 100644 nebula_ros/include/nebula_ros/hesai/hw_monitor_wrapper.hpp create mode 100644 nebula_ros/src/hesai/decoder_wrapper.cpp create mode 100644 nebula_ros/src/hesai/hw_interface_wrapper.cpp create mode 100644 nebula_ros/src/hesai/hw_monitor_wrapper.cpp diff --git a/.cspell.json b/.cspell.json index 75b26f454..5265d4bf1 100644 --- a/.cspell.json +++ b/.cspell.json @@ -6,14 +6,26 @@ "adctp", "Adctp", "AT", + "autosar", "block_id", + "Bpearl", + "calib", + "DHAVE", + "Difop", "extrinsics", "gprmc", + "gptp", + "Helios", "Hesai", + "Idat", + "ipaddr", + "manc", "memcpy", "mkdoxy", + "Msop", "nohup", "nproc", + "ntoa", "pandar", "PANDAR", "PANDARAT", @@ -23,6 +35,7 @@ "QT", "rclcpp", "schedutil", + "srvs", "STD_COUT", "stds", "struct", @@ -30,21 +43,9 @@ "UDP_SEQ", "vccint", "Vccint", + "Vdat", "XT", "XTM", - "DHAVE", - "Bpearl", - "Helios", - "Msop", - "Difop", - "gptp", - "Idat", - "Vdat", - "autosar", - "srvs", - "manc", - "ipaddr", - "ntoa", "piyushk", "Piyush", "fout" diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index 9c6e5d555..0c7ae1c1b 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_common.hpp +++ b/nebula_common/include/nebula_common/hesai/hesai_common.hpp @@ -14,7 +14,7 @@ namespace nebula namespace drivers { /// @brief struct for Hesai sensor configuration -struct HesaiSensorConfiguration : LidarConfigurationBase +struct HesaiSensorConfiguration : public LidarConfigurationBase { uint16_t gnss_port{}; double scan_phase{}; @@ -43,13 +43,20 @@ inline std::ostream & operator<<(std::ostream & os, HesaiSensorConfiguration con return os; } +struct HesaiCalibrationConfigurationBase : public CalibrationConfigurationBase +{ + virtual nebula::Status LoadFromBytes(const std::vector & buf) = 0; + virtual nebula::Status LoadFromFile(const std::string & calibration_file) = 0; + virtual nebula::Status SaveToFileFromBytes(const std::string & calibration_file, const std::vector & buf) = 0; +}; + /// @brief struct for Hesai calibration configuration -struct HesaiCalibrationConfiguration : CalibrationConfigurationBase +struct HesaiCalibrationConfiguration : public HesaiCalibrationConfigurationBase { std::map elev_angle_map; std::map azimuth_offset_map; - inline nebula::Status LoadFromFile(const std::string & calibration_file) + inline nebula::Status LoadFromFile(const std::string & calibration_file) override { std::ifstream ifs(calibration_file); if (!ifs) { @@ -61,6 +68,11 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase return LoadFromString(ss.str()); } + nebula::Status LoadFromBytes(const std::vector & buf) { + std::string calibration_string = std::string(buf.begin(), buf.end()); + return LoadFromString(calibration_string); + } + /// @brief Loading calibration data /// @param calibration_content /// @return Resulting status @@ -99,7 +111,7 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase /// @brief Saving calibration data (not used) /// @param calibration_file /// @return Resulting status - inline nebula::Status SaveFile(const std::string & calibration_file) + inline nebula::Status SaveToFile(const std::string & calibration_file) { std::ofstream ofs(calibration_file); if (!ofs) { @@ -117,6 +129,11 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase return Status::OK; } + nebula::Status SaveToFileFromBytes(const std::string & calibration_file, const std::vector & buf) override { + std::string calibration_string = std::string(buf.begin(), buf.end()); + return SaveFileFromString(calibration_file, calibration_string); + } + /// @brief Saving calibration data from string /// @param calibration_file path /// @param calibration_string calibration string @@ -134,7 +151,7 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase }; /// @brief struct for Hesai correction configuration (for AT) -struct HesaiCorrection +struct HesaiCorrection : public HesaiCalibrationConfigurationBase { uint16_t delimiter; uint8_t versionMajor; @@ -156,7 +173,7 @@ struct HesaiCorrection /// @brief Load correction data from file /// @param buf Binary buffer /// @return Resulting status - inline nebula::Status LoadFromBinary(const std::vector & buf) + inline nebula::Status LoadFromBytes(const std::vector & buf) override { size_t index; for (index = 0; index < buf.size()-1; index++) { @@ -258,7 +275,7 @@ struct HesaiCorrection /// @brief Load correction data from file /// @param correction_file path /// @return Resulting status - inline nebula::Status LoadFromFile(const std::string & correction_file) + inline nebula::Status LoadFromFile(const std::string & correction_file) override { std::ifstream ifs(correction_file, std::ios::in | std::ios::binary); if (!ifs) { @@ -271,7 +288,7 @@ struct HesaiCorrection ifs.read((char *)&c, sizeof(unsigned char)); buf.emplace_back(c); } - LoadFromBinary(buf); + LoadFromBytes(buf); ifs.close(); return Status::OK; @@ -281,7 +298,7 @@ struct HesaiCorrection /// @param correction_file path /// @param buf correction binary /// @return Resulting status - inline nebula::Status SaveFileFromBinary(const std::string & correction_file, const std::vector & buf) + inline nebula::Status SaveToFileFromBytes(const std::string & correction_file, const std::vector & buf) override { std::cerr << "Saving in:" << correction_file << "\n"; std::ofstream ofs(correction_file, std::ios::trunc | std::ios::binary); diff --git a/nebula_common/include/nebula_common/util/expected.hpp b/nebula_common/include/nebula_common/util/expected.hpp index d3643cfb4..0f47e9de1 100644 --- a/nebula_common/include/nebula_common/util/expected.hpp +++ b/nebula_common/include/nebula_common/util/expected.hpp @@ -54,8 +54,9 @@ struct expected return default_; } - /// @brief Return the contained value, or, if an error is contained, throw `runtime_error(error_msg)`. - /// @return The contained value if no error is thrown + /// @brief If the instance has a value, return the value, else throw std::runtime_error(error_msg) + /// @param error_msg The message to be included in the thrown exception + /// @return The value T value_or_throw(const std::string & error_msg) { if (has_value()) return value(); throw std::runtime_error(error_msg); diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp index 730888c91..0280f1ff7 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp @@ -190,7 +190,7 @@ struct PacketBase /// @brief Get the number of returns for a given return mode /// @param return_mode The return mode /// @return The number of returns -int get_n_returns(uint8_t return_mode) +inline int get_n_returns(uint8_t return_mode) { switch (return_mode) { case return_mode::SINGLE_FIRST: diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp index a3f37cd32..5ee9892d4 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp @@ -34,12 +34,11 @@ class HesaiDriver : NebulaDriverBase HesaiDriver() = delete; /// @brief Constructor /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @param correction_configuration CorrectionConfiguration for this driver (for AT) + /// @param calibration_configuration CalibrationConfiguration for this driver (either HesaiCalibrationConfiguration + /// for sensors other than AT128 or HesaiCorrection for AT128) explicit HesaiDriver( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration, - const std::shared_ptr & correction_configuration = nullptr); + const std::shared_ptr& sensor_configuration, + const std::shared_ptr& calibration_configuration = nullptr); /// @brief Get current status of this driver /// @return Current status @@ -48,14 +47,12 @@ class HesaiDriver : NebulaDriverBase /// @brief Setting CalibrationConfiguration (not used) /// @param calibration_configuration /// @return Resulting status - Status SetCalibrationConfiguration( - const CalibrationConfigurationBase & calibration_configuration) override; + Status SetCalibrationConfiguration(const CalibrationConfigurationBase& calibration_configuration) override; /// @brief Convert PandarScan message to point cloud /// @param pandar_scan Message /// @return tuple of Point cloud and timestamp - std::tuple ParseCloudPacket( - const std::vector & packet); + std::tuple ParseCloudPacket(const std::vector& packet); }; } // namespace drivers diff --git a/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp b/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp index 61b969ed7..27624a469 100644 --- a/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp +++ b/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp @@ -17,53 +17,56 @@ namespace nebula { namespace drivers { -HesaiDriver::HesaiDriver( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration, - const std::shared_ptr & correction_configuration) +HesaiDriver::HesaiDriver(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) { + + std::shared_ptr calibration = nullptr; + std::shared_ptr correction = nullptr; + + if (sensor_configuration->sensor_model == SensorModel::HESAI_PANDARAT128) + { + correction = std::static_pointer_cast(calibration_configuration); + } + else + { + calibration = std::static_pointer_cast(calibration_configuration); + } + + switch (sensor_configuration->sensor_model) + { case SensorModel::UNKNOWN: driver_status_ = nebula::Status::INVALID_SENSOR_MODEL; break; case SensorModel::HESAI_PANDAR64: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_.reset(new HesaiDecoder(sensor_configuration, calibration, correction)); break; case SensorModel::HESAI_PANDAR40P: case SensorModel::HESAI_PANDAR40M: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_.reset(new HesaiDecoder(sensor_configuration, calibration, correction)); break; case SensorModel::HESAI_PANDARQT64: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_.reset(new HesaiDecoder(sensor_configuration, calibration, correction)); break; case SensorModel::HESAI_PANDARQT128: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_.reset(new HesaiDecoder(sensor_configuration, calibration, correction)); break; case SensorModel::HESAI_PANDARXT32: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_.reset(new HesaiDecoder(sensor_configuration, calibration, correction)); break; case SensorModel::HESAI_PANDARXT32M: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_.reset(new HesaiDecoder(sensor_configuration, calibration, correction)); break; case SensorModel::HESAI_PANDARAT128: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_.reset(new HesaiDecoder(sensor_configuration, calibration, correction)); break; case SensorModel::HESAI_PANDAR128_E3X: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_.reset(new HesaiDecoder(sensor_configuration, calibration, correction)); break; case SensorModel::HESAI_PANDAR128_E4X: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_.reset(new HesaiDecoder(sensor_configuration, calibration, correction)); break; default: driver_status_ = nebula::Status::NOT_INITIALIZED; @@ -78,13 +81,15 @@ std::tuple HesaiDriver::ParseCloudPacket( std::tuple pointcloud; auto logger = rclcpp::get_logger("HesaiDriver"); - if (driver_status_ != nebula::Status::OK) { + if (driver_status_ != nebula::Status::OK) + { RCLCPP_ERROR(logger, "Driver not OK."); return pointcloud; } scan_decoder_->unpack(packet); - if (scan_decoder_->hasScanned()) { + if (scan_decoder_->hasScanned()) + { pointcloud = scan_decoder_->getPointcloud(); } diff --git a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp index 451b2a9d3..5a40b6b60 100644 --- a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp @@ -22,29 +22,7 @@ HesaiHwInterface::HesaiHwInterface() } HesaiHwInterface::~HesaiHwInterface() { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << ".......................st: HesaiHwInterface::~HesaiHwInterface()" << std::endl; -#endif - if (tcp_driver_) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << ".......................tcp_driver_ is available" << std::endl; -#endif - if (tcp_driver_ && tcp_driver_->isOpen()) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << ".......................st: tcp_driver_->close();" << std::endl; -#endif - tcp_driver_->close(); -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << ".......................ed: tcp_driver_->close();" << std::endl; -#endif - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << ".......................ed: if(tcp_driver_)" << std::endl; -#endif - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << ".......................ed: HesaiHwInterface::~HesaiHwInterface()" << std::endl; -#endif + FinalizeTcpDriver(); } HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( diff --git a/nebula_ros/CMakeLists.txt b/nebula_ros/CMakeLists.txt index 1bd697624..b11339765 100644 --- a/nebula_ros/CMakeLists.txt +++ b/nebula_ros/CMakeLists.txt @@ -24,6 +24,7 @@ find_package(nebula_hw_interfaces REQUIRED) find_package(yaml-cpp REQUIRED) find_package(robosense_msgs REQUIRED) find_package(nebula_msgs REQUIRED) +find_package(pandar_msgs REQUIRED) include_directories( include @@ -36,6 +37,9 @@ link_libraries(${YAML_CPP_LIBRARIES} ${PCL_LIBRARIES}) ## Hesai ament_auto_add_library(hesai_ros_wrapper SHARED src/hesai/hesai_ros_wrapper.cpp + src/hesai/decoder_wrapper.cpp + src/hesai/hw_interface_wrapper.cpp + src/hesai/hw_monitor_wrapper.cpp ) rclcpp_components_register_node(hesai_ros_wrapper diff --git a/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp new file mode 100644 index 000000000..262c16839 --- /dev/null +++ b/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp @@ -0,0 +1,66 @@ +#pragma once + +#include "nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" + +#include +#include +#include + +#include "nebula_msgs/msg/nebula_packet.hpp" +#include "pandar_msgs/msg/pandar_packet.hpp" +#include "pandar_msgs/msg/pandar_scan.hpp" + +#include +#include + +namespace nebula +{ +namespace ros +{ +class HesaiDecoderWrapper +{ +public: + HesaiDecoderWrapper(rclcpp::Node* const parent_node, + const std::shared_ptr& hw_interface, + std::shared_ptr& config); + + nebula::util::expected, nebula::Status> + GetCalibrationData(); + + void ProcessCloudPacket(std::unique_ptr packet_msg); + + void PublishCloud(std::unique_ptr pointcloud, + const rclcpp::Publisher::SharedPtr& publisher); + + nebula::Status Status(); + +private: + /// @brief Convert seconds to chrono::nanoseconds + /// @param seconds + /// @return chrono::nanoseconds + static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) + { + return std::chrono::duration_cast(std::chrono::duration(seconds)); + } + + nebula::Status status_; + rclcpp::Logger logger_; + + const std::shared_ptr hw_interface_; + std::shared_ptr sensor_cfg_; + + std::string calibration_file_path_{}; + std::shared_ptr calibration_cfg_ptr_{}; + + std::shared_ptr driver_ptr_; + + rclcpp::Publisher::SharedPtr packets_pub_{}; + pandar_msgs::msg::PandarScan::UniquePtr current_scan_msg_{}; + + rclcpp::Publisher::SharedPtr nebula_points_pub_{}; + rclcpp::Publisher::SharedPtr aw_points_ex_pub_{}; + rclcpp::Publisher::SharedPtr aw_points_base_pub_{}; +}; +} // namespace ros +} // namespace nebula \ No newline at end of file diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp index 9c50e20eb..e037bce40 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp @@ -4,29 +4,27 @@ #include "nebula_common/hesai/hesai_common.hpp" #include "nebula_common/nebula_common.hpp" #include "nebula_common/nebula_status.hpp" -// #include "nebula_common/util/instrumentation.hpp" -// #include "nebula_common/util/topic_delay.hpp" -#include "nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" +#include "nebula_ros/hesai/decoder_wrapper.hpp" +#include "nebula_ros/hesai/hw_interface_wrapper.hpp" +#include "nebula_ros/hesai/hw_monitor_wrapper.hpp" #include "nebula_ros/hesai/mt_queue.hpp" #include -#include #include #include #include "nebula_msgs/msg/nebula_packet.hpp" -#include "pandar_msgs/msg/pandar_packet.hpp" -#include "pandar_msgs/msg/pandar_scan.hpp" #include #include #include +#include #include #include +#include #include -#include namespace nebula { @@ -40,12 +38,12 @@ namespace ros /// @param value Corresponding value /// @return Whether the target name existed template -bool get_param(const std::vector & p, const std::string & name, T & value) +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()) { + 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; } @@ -56,8 +54,8 @@ bool get_param(const std::vector & p, const std::string & nam class HesaiRosWrapper final : public rclcpp::Node { public: - explicit HesaiRosWrapper(const rclcpp::NodeOptions & options); - ~HesaiRosWrapper(); + explicit HesaiRosWrapper(const rclcpp::NodeOptions& options); + ~HesaiRosWrapper() noexcept; /// @brief Get current status of this driver /// @return Current status @@ -68,172 +66,45 @@ class HesaiRosWrapper final : public rclcpp::Node Status StreamStart(); private: - Status InitializeHwInterface(); - Status InitializeDecoder(); - Status InitializeHwMonitor(); + void ReceiveCloudPacketCallback(std::vector& packet); + + void ReceiveScanMessageCallback(std::unique_ptr scan_msg); + + Status DeclareAndGetSensorConfigParams(); /// @brief Get configurations from ros parameters /// @param sensor_configuration Output of SensorConfiguration /// @return Resulting status - Status GetParameters(drivers::HesaiSensorConfiguration & sensor_configuration); - - /// @brief Get calibration data from the sensor - /// @param calibration_configuration Output of CalibrationConfiguration - /// @param correction_configuration Output of CorrectionConfiguration (for AT) - /// @return Resulting status - Status GetCalibrationData( - drivers::HesaiCalibrationConfiguration & calibration_configuration, - drivers::HesaiCorrection & correction_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); - - /// @brief Callback for receiving a raw UDP packet - /// @param scan_buffer Received PandarScan - void ReceiveCloudPacketCallback(std::vector & scan_buffer); - - /// @brief Callback for receiving replayed, aggregated packets (= scan messages) - /// @param scan_msg - void ReceiveScanMessageCallback(std::unique_ptr scan_msg); - - /// @brief Decodes a nebula packet and, if it completes the scan, publishes the pointcloud. - /// @param packet_msg The received packet message - void ProcessCloudPacket(std::unique_ptr packet_msg); + Status DeclareAndGetWrapperParams(); /// @brief rclcpp parameter callback /// @param parameters Received parameters /// @return SetParametersResult - rcl_interfaces::msg::SetParametersResult paramCallback( - const std::vector & parameters); + rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector& parameters); + /// @brief Updating rclcpp parameter /// @return SetParametersResult std::vector updateParameters(); - /// @brief Initializing diagnostics - void InitializeHesaiDiagnostics(); - /// @brief Callback of the timer for getting the current lidar status - void OnHesaiStatusTimer(); - /// @brief Callback of the timer for getting the current lidar monitor via http - void OnHesaiLidarMonitorTimerHttp(); - /// @brief Callback of the timer for getting the current lidar monitor via tcp - void OnHesaiLidarMonitorTimer(); - // void OnHesaiDiagnosticsTimer(); - // void OnHesaiStatusTimer(); - - /// @brief Check status information from HesaiLidarStatus for diagnostic_updater - /// @param diagnostics DiagnosticStatusWrapper - void HesaiCheckStatus(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - /// @brief Check ptp information from HesaiLidarStatus for diagnostic_updater - /// @param diagnostics DiagnosticStatusWrapper - void HesaiCheckPtp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - /// @brief Check temperature information from HesaiLidarStatus for diagnostic_updater - /// @param diagnostics DiagnosticStatusWrapper - void HesaiCheckTemperature(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - /// @brief Check rpm information from HesaiLidarStatus for diagnostic_updater - /// @param diagnostics DiagnosticStatusWrapper - void HesaiCheckRpm(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - /// @brief Check voltage information from HesaiLidarStatus for diagnostic_updater via http - /// @param diagnostics DiagnosticStatusWrapper - void HesaiCheckVoltageHttp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - /// @brief Check voltage information from HesaiLidarStatus for diagnostic_updater via tcp - /// @param diagnostics DiagnosticStatusWrapper - void HesaiCheckVoltage(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - /// @brief Get value from property_tree - /// @param pt property_tree - /// @param key Pey string - /// @return Value - std::string GetPtreeValue(boost::property_tree::ptree * pt, const std::string & key); - /// @brief Making fixed precision string - /// @param val Target value - /// @param pre Precision - /// @return Created string - std::string GetFixedPrecisionString(double val, int pre = 2); - - std::shared_ptr driver_ptr_{}; - drivers::HesaiHwInterface hw_interface_{}; - Status wrapper_status_; - Status interface_status_; - rclcpp::Subscription::SharedPtr packets_sub_{}; - rclcpp::Publisher::SharedPtr packets_pub_{}; - pandar_msgs::msg::PandarScan::UniquePtr current_scan_msg_{}; - - 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_{}; - std::shared_ptr correction_cfg_ptr_{}; + std::shared_ptr sensor_cfg_ptr_{}; /// @brief Stores received packets that have not been processed yet by the decoder thread mt_queue> packet_queue_; /// @brief Thread to isolate decoding from receiving std::thread decoder_thread_; - // todo: temporary class member during single node refactoring - bool launch_hw_; + rclcpp::Subscription::SharedPtr packets_sub_{}; - // todo: temporary class member during single node refactoring - std::string calibration_file_path; - /// @brief File path of Correction data (Only required only for AT) - std::string correction_file_path; + bool launch_hw_; - /// @brief Received Hesai message publisher - rclcpp::Publisher::SharedPtr pandar_scan_pub_{}; + std::optional hw_interface_wrapper_; + std::optional hw_monitor_wrapper_; + std::optional decoder_wrapper_; - bool retry_hw_; std::mutex mtx_config_; OnSetParametersCallbackHandle::SharedPtr set_param_res_{}; - - diagnostic_updater::Updater diagnostics_updater_; - - rclcpp::TimerBase::SharedPtr diagnostics_update_timer_{}; - rclcpp::TimerBase::SharedPtr fetch_diagnostics_timer_{}; - - std::unique_ptr current_status_{}; - std::unique_ptr current_monitor_{}; - std::unique_ptr current_config_{}; - std::unique_ptr current_inventory_{}; - std::unique_ptr current_lidar_monitor_tree_{}; - - std::unique_ptr current_status_time_{}; - std::unique_ptr current_config_time_{}; - std::unique_ptr current_inventory_time_{}; - std::unique_ptr current_lidar_monitor_time_{}; - - uint8_t current_diag_status_; - uint8_t current_monitor_status_; - - uint16_t diag_span_; - std::mutex mtx_lidar_status_; - std::mutex mtx_lidar_monitor_; - - std::string info_model_; - std::string info_serial_; - - std::vector temperature_names_; - - bool setup_sensor; - const std::string MSG_NOT_SUPPORTED = "Not supported"; - const std::string MSG_ERROR = "Error"; - const std::string MSG_SEP = ": "; }; } // namespace ros diff --git a/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp new file mode 100644 index 000000000..30f069c99 --- /dev/null +++ b/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp @@ -0,0 +1,32 @@ +#pragma once + +#include +#include + +#include + +#include + +namespace nebula +{ +namespace ros +{ +class HesaiHwInterfaceWrapper +{ +public: + HesaiHwInterfaceWrapper(rclcpp::Node* const parent_node, + std::shared_ptr& config); + + nebula::Status InitializeHwInterface(); + + nebula::Status Status(); + + std::shared_ptr HwInterface() const; + +private: + std::shared_ptr hw_interface_; + rclcpp::Logger logger_; + nebula::Status status_; +}; +} // namespace ros +} // namespace nebula \ No newline at end of file diff --git a/nebula_ros/include/nebula_ros/hesai/hw_monitor_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hw_monitor_wrapper.hpp new file mode 100644 index 000000000..5ddfc9e79 --- /dev/null +++ b/nebula_ros/include/nebula_ros/hesai/hw_monitor_wrapper.hpp @@ -0,0 +1,95 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +namespace nebula +{ +namespace ros +{ +class HesaiHwMonitorWrapper +{ +public: + HesaiHwMonitorWrapper(rclcpp::Node* const parent_node, + const std::shared_ptr& hw_interface, + std::shared_ptr& config); + + nebula::Status InitializeHwMonitor(); + + void InitializeHesaiDiagnostics(); + + std::string GetPtreeValue(boost::property_tree::ptree* pt, const std::string& key); + + std::string GetFixedPrecisionString(double val, int pre); + + void OnHesaiStatusTimer(); + + void OnHesaiLidarMonitorTimerHttp(); + + void OnHesaiLidarMonitorTimer(); + + void HesaiCheckStatus(diagnostic_updater::DiagnosticStatusWrapper& diagnostics); + + void HesaiCheckPtp(diagnostic_updater::DiagnosticStatusWrapper& diagnostics); + + void HesaiCheckTemperature(diagnostic_updater::DiagnosticStatusWrapper& diagnostics); + + void HesaiCheckRpm(diagnostic_updater::DiagnosticStatusWrapper& diagnostics); + + void HesaiCheckVoltageHttp(diagnostic_updater::DiagnosticStatusWrapper& diagnostics); + + void HesaiCheckVoltage(diagnostic_updater::DiagnosticStatusWrapper& diagnostics); + + nebula::Status Status(); + +private: + rclcpp::Logger logger_; + diagnostic_updater::Updater diagnostics_updater_; + nebula::Status status_; + + const std::shared_ptr hw_interface_; + rclcpp::Node* const parent_node_; + + uint16_t diag_span_; + rclcpp::TimerBase::SharedPtr diagnostics_update_timer_{}; + rclcpp::TimerBase::SharedPtr fetch_diagnostics_timer_{}; + + std::unique_ptr current_status_{}; + std::unique_ptr current_monitor_{}; + std::unique_ptr current_config_{}; + std::unique_ptr current_inventory_{}; + std::unique_ptr current_lidar_monitor_tree_{}; + + std::unique_ptr current_status_time_{}; + std::unique_ptr current_config_time_{}; + std::unique_ptr current_inventory_time_{}; + std::unique_ptr current_lidar_monitor_time_{}; + + uint8_t current_diag_status_; + uint8_t current_monitor_status_; + + std::mutex mtx_lidar_status_; + std::mutex mtx_lidar_monitor_; + + std::string info_model_; + std::string info_serial_; + + std::vector temperature_names_; + + bool setup_sensor; + const std::string MSG_NOT_SUPPORTED = "Not supported"; + const std::string MSG_ERROR = "Error"; + const std::string MSG_SEP = ": "; +}; +} // namespace ros +} // namespace nebula \ No newline at end of file diff --git a/nebula_ros/include/nebula_ros/hesai/mt_queue.hpp b/nebula_ros/include/nebula_ros/hesai/mt_queue.hpp index a259a80ba..d60b7b4db 100644 --- a/nebula_ros/include/nebula_ros/hesai/mt_queue.hpp +++ b/nebula_ros/include/nebula_ros/hesai/mt_queue.hpp @@ -3,6 +3,7 @@ #include #include #include +#include template class mt_queue @@ -45,12 +46,4 @@ class mt_queue return return_value; } - - void pop_all(std::deque & dest) { - std::unique_lock lock(this->mutex_); - this->cv_not_empty_.wait(lock, [=] { return !this->queue_.empty(); }); - dest.insert(dest.end(), std::make_move_iterator(queue_.begin()), std::make_move_iterator(queue_.end())); - this->queue_.clear(); - this->cv_not_full_.notify_all(); - } }; \ No newline at end of file diff --git a/nebula_ros/package.xml b/nebula_ros/package.xml index 54df70ea9..e918afdc7 100644 --- a/nebula_ros/package.xml +++ b/nebula_ros/package.xml @@ -32,6 +32,7 @@ visualization_msgs yaml-cpp nebula_msgs + pandar_msgs ament_cmake_gtest ament_lint_auto diff --git a/nebula_ros/src/hesai/decoder_wrapper.cpp b/nebula_ros/src/hesai/decoder_wrapper.cpp new file mode 100644 index 000000000..1ae6d2837 --- /dev/null +++ b/nebula_ros/src/hesai/decoder_wrapper.cpp @@ -0,0 +1,255 @@ +#include "nebula_ros/hesai/decoder_wrapper.hpp" + +namespace nebula +{ +namespace ros +{ + +HesaiDecoderWrapper::HesaiDecoderWrapper(rclcpp::Node* const parent_node, + const std::shared_ptr& hw_interface, + std::shared_ptr& config) + : status_(nebula::Status::NOT_INITIALIZED) + , logger_(parent_node->get_logger().get_child("HesaiDecoder")) + , hw_interface_(hw_interface) + , sensor_cfg_(config) +{ + if (!config) + { + throw std::runtime_error("HesaiDecoderWrapper cannot be instantiated without a valid config!"); + } + + if (config->sensor_model == drivers::SensorModel::HESAI_PANDARAT128) + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + parent_node->declare_parameter("correction_file", "", descriptor); + calibration_file_path_ = parent_node->get_parameter("correction_file").as_string(); + } + else + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + parent_node->declare_parameter("calibration_file", "", descriptor); + calibration_file_path_ = parent_node->get_parameter("calibration_file").as_string(); + } + + auto calibration_result = GetCalibrationData(); + + if (!calibration_result.has_value()) + { + throw std::runtime_error( + (std::stringstream() << "No valid calibration found: " << calibration_result.error()).str()); + } + + calibration_cfg_ptr_ = calibration_result.value(); + RCLCPP_INFO_STREAM(logger_, "Using calibration data from " << calibration_cfg_ptr_->calibration_file); + + RCLCPP_INFO(logger_, "Starting Decoder"); + + driver_ptr_ = std::make_shared(config, calibration_cfg_ptr_); + status_ = driver_ptr_->GetStatus(); + + if (Status::OK != status_) + { + throw std::runtime_error((std::stringstream() << "Error instantiating decoder: " << status_).str()); + } + + // Publish packets only if HW interface is connected + if (hw_interface_) + { + current_scan_msg_ = std::make_unique(); + packets_pub_ = + parent_node->create_publisher("pandar_packets", rclcpp::SensorDataQoS()); + } + + auto qos_profile = rmw_qos_profile_sensor_data; + auto pointcloud_qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); + + nebula_points_pub_ = parent_node->create_publisher("pandar_points", pointcloud_qos); + aw_points_base_pub_ = parent_node->create_publisher("aw_points", pointcloud_qos); + aw_points_ex_pub_ = parent_node->create_publisher("aw_points_ex", pointcloud_qos); + + RCLCPP_INFO_STREAM(logger_, ". Wrapper=" << status_); +} + +nebula::util::expected, nebula::Status> +HesaiDecoderWrapper::GetCalibrationData() +{ + std::shared_ptr calib; + + if (sensor_cfg_->sensor_model == drivers::SensorModel::HESAI_PANDARAT128) + { + calib = std::make_shared(); + } + else + { + calib = std::make_shared(); + } + + bool hw_connected = hw_interface_ != nullptr; + std::string calibration_file_path_from_sensor; + + { + int ext_pos = calibration_file_path_.find_last_of('.'); + calibration_file_path_from_sensor = calibration_file_path_.substr(0, ext_pos); + // TODO: if multiple different sensors of the same type are used, this will mix up their calibration data + calibration_file_path_from_sensor += "_from_sensor"; + calibration_file_path_from_sensor += + calibration_file_path_.substr(ext_pos, calibration_file_path_.size() - ext_pos); + } + + // If a sensor is connected, try to download and save its calibration data + if (hw_connected) + { + try + { + auto raw_data = hw_interface_->GetLidarCalibrationBytes(); + RCLCPP_INFO(logger_, "Downloaded calibration data from sensor."); + auto status = calib->SaveToFileFromBytes(calibration_file_path_from_sensor, raw_data); + if (status != Status::OK) + { + RCLCPP_ERROR_STREAM(logger_, "Could not save calibration data: " << status); + } + else + { + RCLCPP_INFO_STREAM(logger_, "Saved downloaded data to " << calibration_file_path_from_sensor); + } + } + catch (std::runtime_error& e) + { + RCLCPP_ERROR_STREAM(logger_, "Could not download calibration data: " << e.what()); + } + } + + // If saved calibration data from a sensor exists (either just downloaded above, or previously), try to load it + if (std::filesystem::exists(calibration_file_path_from_sensor)) + { + auto status = calib->LoadFromFile(calibration_file_path_from_sensor); + if (status == Status::OK) + { + calib->calibration_file = calibration_file_path_from_sensor; + return calib; + } + + RCLCPP_ERROR_STREAM(logger_, "Could not load downloaded calibration data: " << status); + } + else + { + RCLCPP_ERROR(logger_, "No downloaded calibration data found."); + } + + RCLCPP_WARN(logger_, "Falling back to generic calibration file."); + + // If downloaded data did not exist or could not be loaded, fall back to a generic file. + // If that file does not exist either, return an error code + if (!std::filesystem::exists(calibration_file_path_)) + { + RCLCPP_ERROR(logger_, "No calibration data found."); + return nebula::Status(Status::INVALID_CALIBRATION_FILE); + } + + // Try to load the existing fallback calibration file. Return an error if this fails + auto status = calib->LoadFromFile(calibration_file_path_); + if (status != Status::OK) + { + RCLCPP_ERROR(logger_, "Could not load fallback calibration file."); + return status; + } + + // Return the fallback calibration file + calib->calibration_file = calibration_file_path_; + return calib; +} + +void HesaiDecoderWrapper::ProcessCloudPacket(std::unique_ptr packet_msg) +{ + // Accumulate packets for recording only if someone is subscribed to the topic (for performance) + if (hw_interface_ && + (packets_pub_->get_subscription_count() > 0 || packets_pub_->get_intra_process_subscription_count() > 0)) + { + if (current_scan_msg_->packets.size() == 0) + { + current_scan_msg_->header.stamp = packet_msg->stamp; + } + + pandar_msgs::msg::PandarPacket pandar_packet_msg{}; + pandar_packet_msg.stamp = packet_msg->stamp; + pandar_packet_msg.size = packet_msg->data.size(); + std::copy(packet_msg->data.begin(), packet_msg->data.end(), pandar_packet_msg.data.begin()); + current_scan_msg_->packets.emplace_back(std::move(pandar_packet_msg)); + } + + std::tuple pointcloud_ts = + driver_ptr_->ParseCloudPacket(packet_msg->data); + nebula::drivers::NebulaPointCloudPtr pointcloud = std::get<0>(pointcloud_ts); + + if (pointcloud == nullptr) + { + // todo + // RCLCPP_WARN_STREAM(logger_, "Empty cloud parsed."); + return; + }; + + // Publish scan message only if it has been written to + if (current_scan_msg_ && !current_scan_msg_->packets.empty()) + { + packets_pub_->publish(std::move(current_scan_msg_)); + current_scan_msg_ = std::make_unique(); + } + + 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_); + } +} + +void HesaiDecoderWrapper::PublishCloud(std::unique_ptr pointcloud, + const rclcpp::Publisher::SharedPtr& publisher) +{ + if (pointcloud->header.stamp.sec < 0) + { + RCLCPP_WARN_STREAM(logger_, "Timestamp error, verify clock source."); + } + pointcloud->header.frame_id = sensor_cfg_->frame_id; + publisher->publish(std::move(pointcloud)); +} + +nebula::Status HesaiDecoderWrapper::Status() +{ + if (!driver_ptr_) + { + return nebula::Status::NOT_INITIALIZED; + } + + return driver_ptr_->GetStatus(); +} +} // namespace ros +} // namespace nebula \ No newline at end of file diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index 23205ea64..e99e5c6d9 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -4,344 +4,70 @@ namespace nebula { namespace ros { -HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) -: rclcpp::Node("hesai_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)), - hw_interface_(), - wrapper_status_(Status::NOT_INITIALIZED), - interface_status_(Status::NOT_INITIALIZED), - packet_queue_(3000), - diagnostics_updater_(this) +HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions& options) + : rclcpp::Node("hesai_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)) + , wrapper_status_(Status::NOT_INITIALIZED) + , sensor_cfg_ptr_(nullptr) + , packet_queue_(3000) + , hw_interface_wrapper_() + , hw_monitor_wrapper_() + , decoder_wrapper_() { setvbuf(stdout, NULL, _IONBF, BUFSIZ); - sensor_cfg_ptr_ = std::make_shared(); - wrapper_status_ = GetParameters(*sensor_cfg_ptr_); + wrapper_status_ = DeclareAndGetSensorConfigParams(); + wrapper_status_ = DeclareAndGetWrapperParams(); - InitializeHwInterface(); - InitializeDecoder(); - - RCLCPP_DEBUG(this->get_logger(), "Starting stream"); - - if (launch_hw_) { - InitializeHwMonitor(); - StreamStart(); - hw_interface_.RegisterScanCallback( - std::bind(&HesaiRosWrapper::ReceiveCloudPacketCallback, this, std::placeholders::_1)); - } - - packets_sub_ = create_subscription("pandar_packets", rclcpp::SensorDataQoS(), - std::bind(&HesaiRosWrapper::ReceiveScanMessageCallback, this, std::placeholders::_1)); - - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&HesaiRosWrapper::paramCallback, this, std::placeholders::_1)); -} - -Status HesaiRosWrapper::InitializeHwInterface() -{ - interface_status_ = Status::OK; - - hw_interface_.SetLogger(std::make_shared(this->get_logger())); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr_)); - hw_interface_.SetTargetModel(sensor_cfg_ptr_->sensor_model); - - if (!launch_hw_) { - return interface_status_; - } - - int retry_count = 0; - - while (true) { - interface_status_ = hw_interface_.InitializeTcpDriver(); - if (interface_status_ == Status::OK || !retry_hw_) { - break; - } - - retry_count++; - std::this_thread::sleep_for(std::chrono::milliseconds(8000)); // >5000 - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Retry: " << retry_count); - } - - if (interface_status_ == Status::OK) { - try { - auto inventory = hw_interface_.GetInventory(); - RCLCPP_INFO_STREAM(get_logger(), inventory); - hw_interface_.SetTargetModel(inventory.model); - } catch (...) { - RCLCPP_ERROR_STREAM(get_logger(), "Failed to get model from sensor..."); - } - if (this->setup_sensor) { - hw_interface_.CheckAndSetConfig(); - updateParameters(); - } - } else { - RCLCPP_ERROR_STREAM( - get_logger(), - "Failed to get model from sensor... Set from config: " << sensor_cfg_ptr_->sensor_model); + if (launch_hw_) + { + hw_interface_wrapper_.emplace(this, sensor_cfg_ptr_); + hw_monitor_wrapper_.emplace(this, hw_interface_wrapper_->HwInterface(), sensor_cfg_ptr_); } - pandar_scan_pub_ = - create_publisher("pandar_packets", rclcpp::SensorDataQoS()); - current_scan_msg_ = std::make_unique(); - - return Status::OK; -} + decoder_wrapper_.emplace(this, hw_interface_wrapper_ ? hw_interface_wrapper_->HwInterface() : nullptr, + sensor_cfg_ptr_); -Status HesaiRosWrapper::InitializeDecoder() -{ - calibration_cfg_ptr_ = std::make_shared(); - correction_cfg_ptr_ = std::make_shared(); + RCLCPP_INFO_STREAM(get_logger(), "SensorConfig:" << *sensor_cfg_ptr_); + set_param_res_ = + add_on_set_parameters_callback(std::bind(&HesaiRosWrapper::paramCallback, this, std::placeholders::_1)); - wrapper_status_ = GetCalibrationData(*calibration_cfg_ptr_, *correction_cfg_ptr_); - if (Status::OK != wrapper_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); - return wrapper_status_; - } - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Wrapper=" << wrapper_status_); - rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; - auto pointcloud_qos = - rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); + RCLCPP_DEBUG(get_logger(), "Starting stream"); decoder_thread_ = std::thread([this]() { - while (true) { - this->ProcessCloudPacket(std::move(packet_queue_.pop())); + while (true) + { + decoder_wrapper_->ProcessCloudPacket(std::move(packet_queue_.pop())); } }); - nebula_points_pub_ = - this->create_publisher("pandar_points", pointcloud_qos); - aw_points_base_pub_ = - this->create_publisher("aw_points", pointcloud_qos); - aw_points_ex_pub_ = - this->create_publisher("aw_points_ex", pointcloud_qos); - - driver_ptr_ = std::make_shared( - sensor_cfg_ptr_, calibration_cfg_ptr_, correction_cfg_ptr_); - return driver_ptr_->GetStatus(); -} - -Status HesaiRosWrapper::InitializeHwMonitor() -{ - if (Status::OK != interface_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); - return Status::ERROR_1; - } - - switch (sensor_cfg_ptr_->sensor_model) { - case nebula::drivers::SensorModel::HESAI_PANDARXT32: - case nebula::drivers::SensorModel::HESAI_PANDARXT32M: - case nebula::drivers::SensorModel::HESAI_PANDARAT128: - temperature_names_.emplace_back("Bottom circuit board T1"); - temperature_names_.emplace_back("Bottom circuit board T2"); - temperature_names_.emplace_back("Laser emitting board RT_L1(Internal)"); - temperature_names_.emplace_back("Laser emitting board RT_L2"); - temperature_names_.emplace_back("Receiving board RT_R"); - temperature_names_.emplace_back("Receiving board RT2"); - temperature_names_.emplace_back("Top circuit RT3"); - temperature_names_.emplace_back("Not used"); - break; - case nebula::drivers::SensorModel::HESAI_PANDAR64: - case nebula::drivers::SensorModel::HESAI_PANDAR40P: - case nebula::drivers::SensorModel::HESAI_PANDAR40M: - case nebula::drivers::SensorModel::HESAI_PANDARQT64: - case nebula::drivers::SensorModel::HESAI_PANDARQT128: - case nebula::drivers::SensorModel::HESAI_PANDAR128_E3X: - case nebula::drivers::SensorModel::HESAI_PANDAR128_E4X: - default: - temperature_names_.emplace_back("Bottom circuit RT1"); - temperature_names_.emplace_back("Bottom circuit RT2"); - temperature_names_.emplace_back("Internal Temperature"); - temperature_names_.emplace_back("Laser emitting board RT1"); - temperature_names_.emplace_back("Laser emitting board RT2"); - temperature_names_.emplace_back("Receiving board RT1"); - temperature_names_.emplace_back("Top circuit RT1"); - temperature_names_.emplace_back("Top circuit RT2"); - break; - } - - auto result = hw_interface_.GetInventory(); - current_inventory_.reset(new HesaiInventory(result)); - current_inventory_time_.reset(new rclcpp::Time(this->get_clock()->now())); - std::cout << "HesaiInventory" << std::endl; - std::cout << result << std::endl; - info_model_ = result.get_str_model(); - info_serial_ = std::string(result.sn.begin(), result.sn.end()); - hw_interface_.SetTargetModel(result.model); - RCLCPP_INFO_STREAM(this->get_logger(), "Model:" << info_model_); - RCLCPP_INFO_STREAM(this->get_logger(), "Serial:" << info_serial_); - InitializeHesaiDiagnostics(); - return Status::OK; -} - -void HesaiRosWrapper::ReceiveCloudPacketCallback(std::vector & packet) -{ - // static auto convert = nebula::util::Instrumentation("ReceiveCloudPacketCallback.convert"); - // static auto publish = nebula::util::Instrumentation("ReceiveCloudPacketCallback.publish"); - // static auto delay = nebula::util::TopicDelay("ReceiveCloudPacketCallback"); - // Driver is not initialized yet - if (!driver_ptr_) { - return; - } - - // convert.tick(); - const auto now = std::chrono::system_clock::now(); - const auto timestamp_ns = - std::chrono::duration_cast(now.time_since_epoch()).count(); - - auto msg_ptr = std::make_unique(); - msg_ptr->stamp.sec = static_cast(timestamp_ns / 1'000'000'000); - msg_ptr->stamp.nanosec = static_cast(timestamp_ns % 1'000'000'000); - msg_ptr->data.swap(packet); - // convert.tock(); - - // delay.tick(msg_ptr->stamp); - - // publish.tick(); - if (!packet_queue_.try_push(std::move(msg_ptr))) { - RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped"); - } - // packet_pub_->publish(std::move(msg_ptr)); - // publish.tock(); -} - -void HesaiRosWrapper::ReceiveScanMessageCallback( - std::unique_ptr scan_msg) -{ - if (launch_hw_) { - RCLCPP_ERROR_THROTTLE( - get_logger(), *get_clock(), 1000, - "Ignoring received PandarScan. Launch with launch_hw:=false to enable PandarScan replay."); - return; - } - - for (auto & pkt : scan_msg->packets) { - auto nebula_pkt_ptr = std::make_unique(); - nebula_pkt_ptr->stamp = pkt.stamp; - std::copy(pkt.data.begin(), pkt.data.end(), std::back_inserter(nebula_pkt_ptr->data)); - - packet_queue_.push(std::move(nebula_pkt_ptr)); - } -} - -void HesaiRosWrapper::ProcessCloudPacket(std::unique_ptr packet_msg) -{ - // static auto parse = nebula::util::Instrumentation("ProcessCloudPacket.parse"); - // static auto convert = nebula::util::Instrumentation("ProcessCloudPacket.convert"); - // static auto publish = nebula::util::Instrumentation("ProcessCloudPacket.publish"); - // static auto delay = nebula::util::TopicDelay("ProcessCloudPacket"); - - // delay.tick(packet_msg->stamp); - - // Accumulate packets for recording only if someone is subscribed to the topic (for performance) - if ( - launch_hw_ && - (pandar_scan_pub_->get_subscription_count() > 0 || - pandar_scan_pub_->get_intra_process_subscription_count() > 0)) { - if (current_scan_msg_->packets.size() == 0) { - current_scan_msg_->header.stamp = packet_msg->stamp; - } - - pandar_msgs::msg::PandarPacket pandar_packet_msg{}; - pandar_packet_msg.stamp = packet_msg->stamp; - pandar_packet_msg.size = packet_msg->data.size(); - std::copy(packet_msg->data.begin(), packet_msg->data.end(), pandar_packet_msg.data.begin()); - current_scan_msg_->packets.emplace_back(std::move(pandar_packet_msg)); - } - - // parse.tick(); - std::tuple pointcloud_ts = - driver_ptr_->ParseCloudPacket(packet_msg->data); - nebula::drivers::NebulaPointCloudPtr pointcloud = std::get<0>(pointcloud_ts); - // parse.tock(); - - if (pointcloud == nullptr) { - // todo - // RCLCPP_WARN_STREAM(get_logger(), "Empty cloud parsed."); - return; - }; - - // Publish scan message only if it has been written to - if (current_scan_msg_ && !current_scan_msg_->packets.empty()) { - pandar_scan_pub_->publish(std::move(current_scan_msg_)); - current_scan_msg_ = std::make_unique(); - } - - if ( - nebula_points_pub_->get_subscription_count() > 0 || - nebula_points_pub_->get_intra_process_subscription_count() > 0) { - // convert.tick(); - 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()); - // convert.tock(); - // publish.tick(); - PublishCloud(std::move(ros_pc_msg_ptr), nebula_points_pub_); - // publish.tock(); - } - if ( - aw_points_base_pub_->get_subscription_count() > 0 || - aw_points_base_pub_->get_intra_process_subscription_count() > 0) { - // convert.tick(); - - 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()); - // convert.tock(); - // publish.tick(); - PublishCloud(std::move(ros_pc_msg_ptr), aw_points_base_pub_); - // publish.tock(); + if (launch_hw_) + { + StreamStart(); + hw_interface_wrapper_->HwInterface()->RegisterScanCallback( + std::bind(&HesaiRosWrapper::ReceiveCloudPacketCallback, this, std::placeholders::_1)); } - if ( - aw_points_ex_pub_->get_subscription_count() > 0 || - aw_points_ex_pub_->get_intra_process_subscription_count() > 0) { - // convert.tick(); - - 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()); - // convert.tock(); - // publish.tick(); - PublishCloud(std::move(ros_pc_msg_ptr), aw_points_ex_pub_); - // publish.tock(); + else + { + packets_sub_ = create_subscription( + "pandar_packets", rclcpp::SensorDataQoS(), + std::bind(&HesaiRosWrapper::ReceiveScanMessageCallback, this, std::placeholders::_1)); + RCLCPP_INFO_STREAM(get_logger(), "Hardware connection disabled, listening for packets on " << packets_sub_->get_topic_name()); } } -void HesaiRosWrapper::PublishCloud( - std::unique_ptr pointcloud, - const rclcpp::Publisher::SharedPtr & publisher) +nebula::Status HesaiRosWrapper::DeclareAndGetSensorConfigParams() { - if (pointcloud->header.stamp.sec < 0) { - RCLCPP_WARN_STREAM(this->get_logger(), "Timestamp error, verify clock source."); - } - pointcloud->header.frame_id = sensor_cfg_ptr_->frame_id; - publisher->publish(std::move(pointcloud)); -} + nebula::drivers::HesaiSensorConfiguration sensor_configuration; -Status HesaiRosWrapper::GetStatus() -{ - return wrapper_status_; -} - -Status HesaiRosWrapper::GetParameters(drivers::HesaiSensorConfiguration & 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", ""); + declare_parameter("sensor_model", ""); sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); + nebula::drivers::SensorModelFromString(get_parameter("sensor_model").as_string()); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -349,9 +75,9 @@ Status HesaiRosWrapper::GetParameters(drivers::HesaiSensorConfiguration & sensor descriptor.read_only = false; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("return_mode", "", descriptor); + declare_parameter("return_mode", "", descriptor); sensor_configuration.return_mode = nebula::drivers::ReturnModeFromStringHesai( - this->get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); + get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -359,8 +85,8 @@ Status HesaiRosWrapper::GetParameters(drivers::HesaiSensorConfiguration & sensor 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(); + declare_parameter("host_ip", "255.255.255.255", descriptor); + sensor_configuration.host_ip = get_parameter("host_ip").as_string(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -368,8 +94,8 @@ Status HesaiRosWrapper::GetParameters(drivers::HesaiSensorConfiguration & sensor 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(); + declare_parameter("sensor_ip", "192.168.1.201", descriptor); + sensor_configuration.sensor_ip = get_parameter("sensor_ip").as_string(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -377,8 +103,8 @@ Status HesaiRosWrapper::GetParameters(drivers::HesaiSensorConfiguration & sensor 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(); + declare_parameter("data_port", 2368, descriptor); + sensor_configuration.data_port = get_parameter("data_port").as_int(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -386,8 +112,8 @@ Status HesaiRosWrapper::GetParameters(drivers::HesaiSensorConfiguration & sensor descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("gnss_port", 2369, descriptor); - sensor_configuration.gnss_port = this->get_parameter("gnss_port").as_int(); + declare_parameter("gnss_port", 2369, descriptor); + sensor_configuration.gnss_port = get_parameter("gnss_port").as_int(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -395,8 +121,8 @@ Status HesaiRosWrapper::GetParameters(drivers::HesaiSensorConfiguration & sensor descriptor.read_only = false; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "pandar", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); + declare_parameter("frame_id", "pandar", descriptor); + sensor_configuration.frame_id = get_parameter("frame_id").as_string(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -406,18 +132,9 @@ Status HesaiRosWrapper::GetParameters(drivers::HesaiSensorConfiguration & sensor 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(); - } - { - 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("calibration_file", "", descriptor); - calibration_file_path = this->get_parameter("calibration_file").as_string(); + descriptor.floating_point_range = { range }; + declare_parameter("scan_phase", 0., descriptor); + sensor_configuration.scan_phase = get_parameter("scan_phase").as_double(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -425,8 +142,8 @@ Status HesaiRosWrapper::GetParameters(drivers::HesaiSensorConfiguration & sensor descriptor.read_only = false; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("min_range", 0.3, descriptor); - sensor_configuration.min_range = this->get_parameter("min_range").as_double(); + declare_parameter("min_range", 0.3, descriptor); + sensor_configuration.min_range = get_parameter("min_range").as_double(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -434,8 +151,8 @@ Status HesaiRosWrapper::GetParameters(drivers::HesaiSensorConfiguration & sensor descriptor.read_only = false; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("max_range", 300., descriptor); - sensor_configuration.max_range = this->get_parameter("max_range").as_double(); + declare_parameter("max_range", 300., descriptor); + sensor_configuration.max_range = get_parameter("max_range").as_double(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -443,10 +160,9 @@ Status HesaiRosWrapper::GetParameters(drivers::HesaiSensorConfiguration & sensor descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("packet_mtu_size", 1500, descriptor); - sensor_configuration.packet_mtu_size = this->get_parameter("packet_mtu_size").as_int(); + declare_parameter("packet_mtu_size", 1500, descriptor); + sensor_configuration.packet_mtu_size = get_parameter("packet_mtu_size").as_int(); } - { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; @@ -454,18 +170,21 @@ Status HesaiRosWrapper::GetParameters(drivers::HesaiSensorConfiguration & sensor descriptor.dynamic_typing = false; rcl_interfaces::msg::IntegerRange range; RCLCPP_DEBUG_STREAM(get_logger(), sensor_configuration.sensor_model); - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::HESAI_PANDARAT128) { + if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::HESAI_PANDARAT128) + { descriptor.additional_constraints = "200, 300, 400, 500"; // range.set__from_value(200).set__to_value(500).set__step(100); // descriptor.integer_range = {range}; //todo - this->declare_parameter("rotation_speed", 200, descriptor); - } else { + declare_parameter("rotation_speed", 200, descriptor); + } + else + { descriptor.additional_constraints = "300, 600, 1200"; // range.set__from_value(300).set__to_value(1200).set__step(300); // descriptor.integer_range = {range}; //todo - this->declare_parameter("rotation_speed", 600, descriptor); + declare_parameter("rotation_speed", 600, descriptor); } - sensor_configuration.rotation_speed = this->get_parameter("rotation_speed").as_int(); + sensor_configuration.rotation_speed = get_parameter("rotation_speed").as_int(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -475,9 +194,9 @@ Status HesaiRosWrapper::GetParameters(drivers::HesaiSensorConfiguration & sensor descriptor.additional_constraints = ""; rcl_interfaces::msg::IntegerRange range; range.set__from_value(0).set__to_value(360).set__step(1); - descriptor.integer_range = {range}; - this->declare_parameter("cloud_min_angle", 0, descriptor); - sensor_configuration.cloud_min_angle = this->get_parameter("cloud_min_angle").as_int(); + descriptor.integer_range = { range }; + declare_parameter("cloud_min_angle", 0, descriptor); + sensor_configuration.cloud_min_angle = get_parameter("cloud_min_angle").as_int(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -487,18 +206,9 @@ Status HesaiRosWrapper::GetParameters(drivers::HesaiSensorConfiguration & sensor descriptor.additional_constraints = ""; rcl_interfaces::msg::IntegerRange range; range.set__from_value(0).set__to_value(360).set__step(1); - descriptor.integer_range = {range}; - this->declare_parameter("cloud_max_angle", 360, descriptor); - sensor_configuration.cloud_max_angle = this->get_parameter("cloud_max_angle").as_int(); - } - if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - 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("correction_file", "", descriptor); - correction_file_path = this->get_parameter("correction_file").as_string(); + descriptor.integer_range = { range }; + declare_parameter("cloud_max_angle", 360, descriptor); + sensor_configuration.cloud_max_angle = get_parameter("cloud_max_angle").as_int(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -508,37 +218,9 @@ Status HesaiRosWrapper::GetParameters(drivers::HesaiSensorConfiguration & sensor 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_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("launch_hw", "", descriptor); - launch_hw_ = this->get_parameter("launch_hw").as_bool(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("setup_sensor", true, descriptor); - this->setup_sensor = this->get_parameter("setup_sensor").as_bool(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("retry_hw", true, descriptor); - this->retry_hw_ = this->get_parameter("retry_hw").as_bool(); + descriptor.floating_point_range = { range }; + declare_parameter("dual_return_distance_threshold", 0.1, descriptor); + sensor_configuration.dual_return_distance_threshold = get_parameter("dual_return_distance_threshold").as_double(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -546,9 +228,8 @@ Status HesaiRosWrapper::GetParameters(drivers::HesaiSensorConfiguration & sensor descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("ptp_profile", ""); - sensor_configuration.ptp_profile = - nebula::drivers::PtpProfileFromString(this->get_parameter("ptp_profile").as_string()); + declare_parameter("ptp_profile", ""); + sensor_configuration.ptp_profile = nebula::drivers::PtpProfileFromString(get_parameter("ptp_profile").as_string()); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -556,10 +237,11 @@ Status HesaiRosWrapper::GetParameters(drivers::HesaiSensorConfiguration & sensor descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("ptp_transport_type", ""); - sensor_configuration.ptp_transport_type = nebula::drivers::PtpTransportTypeFromString( - this->get_parameter("ptp_transport_type").as_string()); - if (static_cast(sensor_configuration.ptp_profile) > 0) { + declare_parameter("ptp_transport_type", ""); + sensor_configuration.ptp_transport_type = + nebula::drivers::PtpTransportTypeFromString(get_parameter("ptp_transport_type").as_string()); + if (static_cast(sensor_configuration.ptp_profile) > 0) + { sensor_configuration.ptp_transport_type = nebula::drivers::PtpTransportType::L2; } } @@ -569,9 +251,9 @@ Status HesaiRosWrapper::GetParameters(drivers::HesaiSensorConfiguration & sensor descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("ptp_switch_type", ""); + declare_parameter("ptp_switch_type", ""); sensor_configuration.ptp_switch_type = - nebula::drivers::PtpSwitchTypeFromString(this->get_parameter("ptp_switch_type").as_string()); + nebula::drivers::PtpSwitchTypeFromString(get_parameter("ptp_switch_type").as_string()); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -580,320 +262,144 @@ Status HesaiRosWrapper::GetParameters(drivers::HesaiSensorConfiguration & sensor descriptor.dynamic_typing = false; rcl_interfaces::msg::IntegerRange range; range.set__from_value(0).set__to_value(127).set__step(1); - descriptor.integer_range = {range}; - this->declare_parameter("ptp_domain", 0, descriptor); - sensor_configuration.ptp_domain = this->get_parameter("ptp_domain").as_int(); - } - { - 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); - this->diag_span_ = this->get_parameter("diag_span").as_int(); + descriptor.integer_range = { range }; + declare_parameter("ptp_domain", 0, descriptor); + sensor_configuration.ptp_domain = get_parameter("ptp_domain").as_int(); } - /////////////////////////////////////////////// - // Validate ROS parameters - /////////////////////////////////////////////// - - if (sensor_configuration.ptp_profile == nebula::drivers::PtpProfile::PROFILE_UNKNOWN) { - RCLCPP_ERROR_STREAM( - get_logger(), "Invalid PTP Profile Provided. Please use '1588v2', '802.1as' or 'automotive'"); + if (sensor_configuration.ptp_profile == nebula::drivers::PtpProfile::PROFILE_UNKNOWN) + { + RCLCPP_ERROR_STREAM(get_logger(), "Invalid PTP Profile Provided. Please use '1588v2', '802.1as' or 'automotive'"); return Status::SENSOR_CONFIG_ERROR; } - if ( - sensor_configuration.ptp_transport_type == - nebula::drivers::PtpTransportType::UNKNOWN_TRANSPORT) { - RCLCPP_ERROR_STREAM( - get_logger(), - "Invalid PTP Transport Provided. Please use 'udp' or 'l2', 'udp' is only available when " - "using the '1588v2' PTP Profile"); + if (sensor_configuration.ptp_transport_type == nebula::drivers::PtpTransportType::UNKNOWN_TRANSPORT) + { + RCLCPP_ERROR_STREAM(get_logger(), + "Invalid PTP Transport Provided. Please use 'udp' or 'l2', 'udp' is only available when " + "using the '1588v2' PTP Profile"); return Status::SENSOR_CONFIG_ERROR; } - if (sensor_configuration.ptp_switch_type == nebula::drivers::PtpSwitchType::UNKNOWN_SWITCH) { - RCLCPP_ERROR_STREAM( - get_logger(), "Invalid PTP Switch Type Provided. Please use 'tsn' or 'non_tsn'"); + if (sensor_configuration.ptp_switch_type == nebula::drivers::PtpSwitchType::UNKNOWN_SWITCH) + { + RCLCPP_ERROR_STREAM(get_logger(), "Invalid PTP Switch Type Provided. Please use 'tsn' or 'non_tsn'"); return Status::SENSOR_CONFIG_ERROR; } - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) + { return Status::INVALID_SENSOR_MODEL; } - if (sensor_configuration.return_mode == nebula::drivers::ReturnMode::UNKNOWN) { + if (sensor_configuration.return_mode == nebula::drivers::ReturnMode::UNKNOWN) + { return Status::INVALID_ECHO_MODE; } - if (sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { + if (sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) + { return Status::SENSOR_CONFIG_ERROR; } - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr_)); - - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); + auto new_cfg_ptr_ = std::make_shared(sensor_configuration); + sensor_cfg_ptr_.swap(new_cfg_ptr_); return Status::OK; } -Status HesaiRosWrapper::GetCalibrationData( - drivers::HesaiCalibrationConfiguration & calibration_configuration, - drivers::HesaiCorrection & correction_configuration) +void HesaiRosWrapper::ReceiveScanMessageCallback(std::unique_ptr scan_msg) { - calibration_configuration.calibration_file = calibration_file_path; // todo - - bool run_local = !launch_hw_; - if (sensor_cfg_ptr_->sensor_model != drivers::SensorModel::HESAI_PANDARAT128) { - std::string calibration_file_path_from_sensor; - if (launch_hw_ && !calibration_configuration.calibration_file.empty()) { - int ext_pos = calibration_configuration.calibration_file.find_last_of('.'); - calibration_file_path_from_sensor += - calibration_configuration.calibration_file.substr(0, ext_pos); - calibration_file_path_from_sensor += "_from_sensor"; - calibration_file_path_from_sensor += calibration_configuration.calibration_file.substr( - ext_pos, calibration_configuration.calibration_file.size() - ext_pos); - } - if (launch_hw_) { - run_local = false; - RCLCPP_INFO_STREAM( - this->get_logger(), - "Trying to acquire calibration data from sensor: '" << sensor_cfg_ptr_->sensor_ip << "'"); - std::future future = std::async( - std::launch::async, - [this, &calibration_configuration, &calibration_file_path_from_sensor, &run_local]() { - auto str = hw_interface_.GetLidarCalibrationString(); - auto rt = - calibration_configuration.SaveFileFromString(calibration_file_path_from_sensor, str); - RCLCPP_ERROR_STREAM(get_logger(), str); - if (rt == Status::OK) { - RCLCPP_INFO_STREAM( - get_logger(), - "SaveFileFromString success:" << calibration_file_path_from_sensor << "\n"); - } else { - RCLCPP_ERROR_STREAM( - get_logger(), - "SaveFileFromString failed:" << calibration_file_path_from_sensor << "\n"); - } - rt = calibration_configuration.LoadFromString(str); - if (rt == Status::OK) { - RCLCPP_INFO_STREAM(get_logger(), "LoadFromString success:" << str << "\n"); - } else { - RCLCPP_ERROR_STREAM(get_logger(), "LoadFromString failed:" << str << "\n"); - } - }); - std::future_status status; - status = future.wait_for(std::chrono::milliseconds(5000)); - if (status == std::future_status::timeout) { - std::cerr << "# std::future_status::timeout\n"; - RCLCPP_ERROR_STREAM(get_logger(), "GetCalibration Timeout"); - run_local = true; - } else if (status == std::future_status::ready && !run_local) { - RCLCPP_INFO_STREAM( - this->get_logger(), - "Acquired calibration data from sensor: '" << sensor_cfg_ptr_->sensor_ip << "'"); - RCLCPP_INFO_STREAM( - this->get_logger(), - "The calibration has been saved in '" << calibration_file_path_from_sensor << "'"); - } - } - if (run_local) { - RCLCPP_WARN_STREAM(get_logger(), "Running locally"); - bool run_org = false; - if (calibration_file_path_from_sensor.empty()) { - run_org = true; - } else { - RCLCPP_INFO_STREAM( - get_logger(), "Trying to load file: " << calibration_file_path_from_sensor); - auto cal_status = calibration_configuration.LoadFromFile(calibration_file_path_from_sensor); + if (hw_interface_wrapper_) + { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 1000, + "Ignoring received PandarScan. Launch with launch_hw:=false to enable PandarScan replay."); + return; + } - if (cal_status != Status::OK) { - run_org = true; - } else { - RCLCPP_INFO_STREAM( - this->get_logger(), - "Load calibration data from: '" << calibration_file_path_from_sensor << "'"); - } - } - if (run_org) { - RCLCPP_INFO_STREAM( - get_logger(), "Trying to load file: " << calibration_configuration.calibration_file); - if (calibration_configuration.calibration_file.empty()) { - RCLCPP_ERROR_STREAM( - this->get_logger(), - "Empty Calibration_file File: '" << calibration_configuration.calibration_file << "'"); - return Status::INVALID_CALIBRATION_FILE; - } else { - auto cal_status = - calibration_configuration.LoadFromFile(calibration_configuration.calibration_file); + for (auto& pkt : scan_msg->packets) + { + auto nebula_pkt_ptr = std::make_unique(); + nebula_pkt_ptr->stamp = pkt.stamp; + std::copy(pkt.data.begin(), pkt.data.end(), std::back_inserter(nebula_pkt_ptr->data)); - if (cal_status != Status::OK) { - RCLCPP_ERROR_STREAM( - this->get_logger(), - "Given Calibration File: '" << calibration_configuration.calibration_file << "'"); - return cal_status; - } else { - RCLCPP_INFO_STREAM( - this->get_logger(), - "Load calibration data from: '" << calibration_configuration.calibration_file << "'"); - } - } - } - } - } else { // sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128 - std::string correction_file_path_from_sensor; - if (launch_hw_ && !correction_file_path.empty()) { - int ext_pos = correction_file_path.find_last_of('.'); - correction_file_path_from_sensor += correction_file_path.substr(0, ext_pos); - correction_file_path_from_sensor += "_from_sensor"; - correction_file_path_from_sensor += - correction_file_path.substr(ext_pos, correction_file_path.size() - ext_pos); - } - std::future future = std::async( - std::launch::async, - [this, &correction_configuration, &correction_file_path_from_sensor, &run_local]() { - if (launch_hw_) { - RCLCPP_INFO_STREAM(this->get_logger(), "Trying to acquire calibration data from sensor"); - auto received_bytes = hw_interface_.GetLidarCalibrationBytes(); - RCLCPP_INFO_STREAM( - get_logger(), "AT128 calibration size:" << received_bytes.size() << "\n"); - auto rt = correction_configuration.SaveFileFromBinary( - correction_file_path_from_sensor, received_bytes); - if (rt == Status::OK) { - RCLCPP_INFO_STREAM( - get_logger(), - "SaveFileFromBinary success:" << correction_file_path_from_sensor << "\n"); - } else { - RCLCPP_ERROR_STREAM( - get_logger(), - "SaveFileFromBinary failed:" << correction_file_path_from_sensor - << ". Falling back to offline calibration file."); - run_local = true; - } - rt = correction_configuration.LoadFromBinary(received_bytes); - if (rt == Status::OK) { - RCLCPP_INFO_STREAM(get_logger(), "LoadFromBinary success" << "\n"); - run_local = false; - } else { - RCLCPP_ERROR_STREAM( - get_logger(), - "LoadFromBinary failed" << ". Falling back to offline calibration file."); - run_local = true; - } - } else { - RCLCPP_ERROR_STREAM(get_logger(), "Falling back to offline calibration file."); - run_local = true; - } - }); - if (!run_local) { - std::future_status status; - status = future.wait_for(std::chrono::milliseconds(8000)); - if (status == std::future_status::timeout) { - std::cerr << "# std::future_status::timeout\n"; - run_local = true; - } else if (status == std::future_status::ready && !run_local) { - RCLCPP_INFO_STREAM( - this->get_logger(), - "Acquired correction data from sensor: '" << sensor_cfg_ptr_->sensor_ip << "'"); - RCLCPP_INFO_STREAM( - this->get_logger(), - "The correction has been saved in '" << correction_file_path_from_sensor << "'"); - } - } - if (run_local) { - bool run_org = false; - if (correction_file_path_from_sensor.empty()) { - run_org = true; - } else { - auto cal_status = correction_configuration.LoadFromFile(correction_file_path_from_sensor); + packet_queue_.push(std::move(nebula_pkt_ptr)); + } +} - if (cal_status != Status::OK) { - run_org = true; - } else { - RCLCPP_INFO_STREAM( - this->get_logger(), - "Load correction data from: '" << correction_file_path_from_sensor << "'"); - } - } - if (run_org) { - if (correction_file_path.empty()) { - RCLCPP_ERROR_STREAM( - this->get_logger(), "Empty Correction File: '" << correction_file_path << "'"); - return Status::INVALID_CALIBRATION_FILE; - } else { - auto cal_status = correction_configuration.LoadFromFile(correction_file_path); +Status HesaiRosWrapper::GetStatus() +{ + return wrapper_status_; +} - if (cal_status != Status::OK) { - RCLCPP_ERROR_STREAM( - this->get_logger(), "Given Correction File: '" << correction_file_path << "'"); - return cal_status; - } else { - RCLCPP_INFO_STREAM( - this->get_logger(), "Load correction data from: '" << correction_file_path << "'"); - } - } - } - } - } // end AT128 +Status HesaiRosWrapper::DeclareAndGetWrapperParams() +{ + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + declare_parameter("launch_hw", "", descriptor); + launch_hw_ = get_parameter("launch_hw").as_bool(); + } return Status::OK; } HesaiRosWrapper::~HesaiRosWrapper() { - RCLCPP_INFO_STREAM(get_logger(), "Closing TcpDriver"); - hw_interface_.FinalizeTcpDriver(); } Status HesaiRosWrapper::StreamStart() { - if (Status::OK == interface_status_) { - interface_status_ = hw_interface_.SensorInterfaceStart(); + if (!hw_interface_wrapper_) + { + return Status::UDP_CONNECTION_ERROR; } - return interface_status_; + + if (hw_interface_wrapper_->Status() != Status::OK) + { + return hw_interface_wrapper_->Status(); + } + + return hw_interface_wrapper_->HwInterface()->SensorInterfaceStart(); } -rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::paramCallback( - const std::vector & p) +rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::paramCallback(const std::vector& p) { std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback"); - RCLCPP_DEBUG_STREAM(this->get_logger(), p); - RCLCPP_DEBUG_STREAM(this->get_logger(), *sensor_cfg_ptr_); - RCLCPP_INFO_STREAM(this->get_logger(), p); + RCLCPP_DEBUG_STREAM(get_logger(), "add_on_set_parameters_callback"); + RCLCPP_DEBUG_STREAM(get_logger(), p); + RCLCPP_DEBUG_STREAM(get_logger(), *sensor_cfg_ptr_); + + drivers::HesaiSensorConfiguration new_cfg(*sensor_cfg_ptr_); - std::shared_ptr new_param = - std::make_shared(*sensor_cfg_ptr_); - RCLCPP_INFO_STREAM(this->get_logger(), new_param); std::string sensor_model_str; std::string return_mode_str; - if ( - get_param(p, "sensor_model", sensor_model_str) | get_param(p, "return_mode", return_mode_str) | - get_param(p, "host_ip", new_param->host_ip) | get_param(p, "sensor_ip", new_param->sensor_ip) | - get_param(p, "frame_id", new_param->frame_id) | - get_param(p, "data_port", new_param->data_port) | - get_param(p, "gnss_port", new_param->gnss_port) | - get_param(p, "scan_phase", new_param->scan_phase) | - get_param(p, "packet_mtu_size", new_param->packet_mtu_size) | - get_param(p, "rotation_speed", new_param->rotation_speed) | - get_param(p, "cloud_min_angle", new_param->cloud_min_angle) | - get_param(p, "cloud_max_angle", new_param->cloud_max_angle) | - get_param(p, "dual_return_distance_threshold", new_param->dual_return_distance_threshold)) { + if (get_param(p, "sensor_model", sensor_model_str) | get_param(p, "return_mode", return_mode_str) | + get_param(p, "host_ip", new_cfg.host_ip) | get_param(p, "sensor_ip", new_cfg.sensor_ip) | + get_param(p, "frame_id", new_cfg.frame_id) | get_param(p, "data_port", new_cfg.data_port) | + get_param(p, "gnss_port", new_cfg.gnss_port) | get_param(p, "scan_phase", new_cfg.scan_phase) | + get_param(p, "packet_mtu_size", new_cfg.packet_mtu_size) | + get_param(p, "rotation_speed", new_cfg.rotation_speed) | + get_param(p, "cloud_min_angle", new_cfg.cloud_min_angle) | + get_param(p, "cloud_max_angle", new_cfg.cloud_max_angle) | + get_param(p, "dual_return_distance_threshold", new_cfg.dual_return_distance_threshold)) + { if (0 < sensor_model_str.length()) - new_param->sensor_model = nebula::drivers::SensorModelFromString(sensor_model_str); + new_cfg.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_str); if (0 < return_mode_str.length()) - new_param->return_mode = nebula::drivers::ReturnModeFromString(return_mode_str); + new_cfg.return_mode = nebula::drivers::ReturnModeFromString(return_mode_str); - sensor_cfg_ptr_.swap(new_param); - RCLCPP_INFO_STREAM(this->get_logger(), "Update sensor_configuration"); - RCLCPP_DEBUG_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr_)); - hw_interface_.CheckAndSetConfig(); + auto new_cfg_ptr = std::make_shared(new_cfg); + sensor_cfg_ptr_.swap(new_cfg_ptr); + RCLCPP_INFO_STREAM(get_logger(), "Update sensor_configuration"); + RCLCPP_DEBUG_STREAM(get_logger(), "hw_interface_.SetSensorConfiguration"); + hw_interface_wrapper_->HwInterface()->SetSensorConfiguration( + std::static_pointer_cast(sensor_cfg_ptr_)); + hw_interface_wrapper_->HwInterface()->CheckAndSetConfig(); } auto result = std::make_shared(); result->successful = true; result->reason = "success"; - RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback success"); + RCLCPP_DEBUG_STREAM(get_logger(), "add_on_set_parameters_callback success"); return *result; } @@ -901,310 +407,47 @@ rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::paramCallback( std::vector HesaiRosWrapper::updateParameters() { std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters start"); + RCLCPP_DEBUG_STREAM(get_logger(), "updateParameters start"); std::ostringstream os_sensor_model; os_sensor_model << sensor_cfg_ptr_->sensor_model; std::ostringstream os_return_mode; os_return_mode << sensor_cfg_ptr_->return_mode; - RCLCPP_INFO_STREAM(this->get_logger(), "set_parameters"); + RCLCPP_INFO_STREAM(get_logger(), "set_parameters"); auto results = set_parameters( - {rclcpp::Parameter("sensor_model", os_sensor_model.str()), - rclcpp::Parameter("return_mode", os_return_mode.str()), - rclcpp::Parameter("host_ip", sensor_cfg_ptr_->host_ip), - rclcpp::Parameter("sensor_ip", sensor_cfg_ptr_->sensor_ip), - rclcpp::Parameter("frame_id", sensor_cfg_ptr_->frame_id), - rclcpp::Parameter("data_port", sensor_cfg_ptr_->data_port), - rclcpp::Parameter("gnss_port", sensor_cfg_ptr_->gnss_port), - rclcpp::Parameter("scan_phase", sensor_cfg_ptr_->scan_phase), - rclcpp::Parameter("packet_mtu_size", sensor_cfg_ptr_->packet_mtu_size), - rclcpp::Parameter("rotation_speed", sensor_cfg_ptr_->rotation_speed), - rclcpp::Parameter("cloud_min_angle", sensor_cfg_ptr_->cloud_min_angle), - rclcpp::Parameter("cloud_max_angle", sensor_cfg_ptr_->cloud_max_angle), - rclcpp::Parameter( - "dual_return_distance_threshold", sensor_cfg_ptr_->dual_return_distance_threshold)}); - RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters end"); + { rclcpp::Parameter("sensor_model", os_sensor_model.str()), + rclcpp::Parameter("return_mode", os_return_mode.str()), rclcpp::Parameter("host_ip", sensor_cfg_ptr_->host_ip), + rclcpp::Parameter("sensor_ip", sensor_cfg_ptr_->sensor_ip), + rclcpp::Parameter("frame_id", sensor_cfg_ptr_->frame_id), + rclcpp::Parameter("data_port", sensor_cfg_ptr_->data_port), + rclcpp::Parameter("gnss_port", sensor_cfg_ptr_->gnss_port), + rclcpp::Parameter("scan_phase", sensor_cfg_ptr_->scan_phase), + rclcpp::Parameter("packet_mtu_size", sensor_cfg_ptr_->packet_mtu_size), + rclcpp::Parameter("rotation_speed", sensor_cfg_ptr_->rotation_speed), + rclcpp::Parameter("cloud_min_angle", sensor_cfg_ptr_->cloud_min_angle), + rclcpp::Parameter("cloud_max_angle", sensor_cfg_ptr_->cloud_max_angle), + rclcpp::Parameter("dual_return_distance_threshold", sensor_cfg_ptr_->dual_return_distance_threshold) }); + RCLCPP_DEBUG_STREAM(get_logger(), "updateParameters end"); return results; } -void HesaiRosWrapper::InitializeHesaiDiagnostics() -{ - RCLCPP_INFO_STREAM(this->get_logger(), "InitializeHesaiDiagnostics"); - using std::chrono_literals::operator""s; - std::ostringstream os; - auto hardware_id = info_model_ + ": " + info_serial_; - diagnostics_updater_.setHardwareID(hardware_id); - RCLCPP_INFO_STREAM(this->get_logger(), "hardware_id: " + hardware_id); - - diagnostics_updater_.add("hesai_status", this, &HesaiRosWrapper::HesaiCheckStatus); - diagnostics_updater_.add("hesai_ptp", this, &HesaiRosWrapper::HesaiCheckPtp); - diagnostics_updater_.add("hesai_temperature", this, &HesaiRosWrapper::HesaiCheckTemperature); - diagnostics_updater_.add("hesai_rpm", this, &HesaiRosWrapper::HesaiCheckRpm); - - current_status_.reset(); - current_monitor_.reset(); - current_status_time_.reset(new rclcpp::Time(this->get_clock()->now())); - current_lidar_monitor_time_.reset(new rclcpp::Time(this->get_clock()->now())); - current_diag_status_ = diagnostic_msgs::msg::DiagnosticStatus::STALE; - current_monitor_status_ = diagnostic_msgs::msg::DiagnosticStatus::STALE; - - auto fetch_diag_from_sensor = [this]() { - OnHesaiStatusTimer(); - if (hw_interface_.UseHttpGetLidarMonitor()) { - OnHesaiLidarMonitorTimerHttp(); - } else { - OnHesaiLidarMonitorTimer(); - } - }; - - fetch_diagnostics_timer_ = - create_wall_timer(std::chrono::milliseconds(diag_span_), std::move(fetch_diag_from_sensor)); - - if (hw_interface_.UseHttpGetLidarMonitor()) { - diagnostics_updater_.add("hesai_voltage", this, &HesaiRosWrapper::HesaiCheckVoltageHttp); - } else { - diagnostics_updater_.add("hesai_voltage", this, &HesaiRosWrapper::HesaiCheckVoltage); - } - - auto on_timer_update = [this] { - RCLCPP_DEBUG_STREAM(get_logger(), "OnUpdateTimer"); - auto now = this->get_clock()->now(); - auto dif = (now - *current_status_time_).seconds(); - - RCLCPP_DEBUG_STREAM(get_logger(), "dif(status): " << dif); - - if (diag_span_ * 2.0 < dif * 1000) { - current_diag_status_ = diagnostic_msgs::msg::DiagnosticStatus::STALE; - RCLCPP_DEBUG_STREAM(get_logger(), "STALE"); - } else { - current_diag_status_ = diagnostic_msgs::msg::DiagnosticStatus::OK; - RCLCPP_DEBUG_STREAM(get_logger(), "OK"); - } - dif = (now - *current_lidar_monitor_time_).seconds(); - RCLCPP_DEBUG_STREAM(get_logger(), "dif(monitor): " << dif); - if (diag_span_ * 2.0 < dif * 1000) { - current_monitor_status_ = diagnostic_msgs::msg::DiagnosticStatus::STALE; - RCLCPP_DEBUG_STREAM(get_logger(), "STALE"); - } else { - current_monitor_status_ = diagnostic_msgs::msg::DiagnosticStatus::OK; - RCLCPP_DEBUG_STREAM(get_logger(), "OK"); - } - diagnostics_updater_.force_update(); - }; - diagnostics_update_timer_ = - create_wall_timer(std::chrono::milliseconds(1000), std::move(on_timer_update)); - - RCLCPP_DEBUG_STREAM(get_logger(), "add_timer"); -} - -std::string HesaiRosWrapper::GetPtreeValue( - boost::property_tree::ptree * pt, const std::string & key) -{ - boost::optional value = pt->get_optional(key); - if (value) { - return value.get(); - } else { - return MSG_NOT_SUPPORTED; - } -} -std::string HesaiRosWrapper::GetFixedPrecisionString(double val, int pre) -{ - std::stringstream ss; - ss << std::fixed << std::setprecision(pre) << val; - return ss.str(); -} - -void HesaiRosWrapper::OnHesaiStatusTimer() -{ - RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiStatusTimer" << std::endl); - try { - auto result = hw_interface_.GetLidarStatus(); - std::scoped_lock lock(mtx_lidar_status_); - current_status_time_.reset(new rclcpp::Time(this->get_clock()->now())); - current_status_.reset(new HesaiLidarStatus(result)); - } catch (const std::system_error & error) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("HesaiRosWrapper::OnHesaiStatusTimer(std::system_error)"), error.what()); - } catch (const boost::system::system_error & error) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("HesaiRosWrapper::OnHesaiStatusTimer(boost::system::system_error)"), - error.what()); - } - RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiStatusTimer END" << std::endl); -} - -void HesaiRosWrapper::OnHesaiLidarMonitorTimerHttp() -{ - RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiLidarMonitorTimerHttp"); - try { - hw_interface_.GetLidarMonitorAsyncHttp([this](const std::string & str) { - std::scoped_lock lock(mtx_lidar_monitor_); - current_lidar_monitor_time_.reset(new rclcpp::Time(this->get_clock()->now())); - current_lidar_monitor_tree_ = - std::make_unique(hw_interface_.ParseJson(str)); - }); - } catch (const std::system_error & error) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("HesaiRosWrapper::OnHesaiLidarMonitorTimerHttp(std::system_error)"), - error.what()); - } catch (const boost::system::system_error & error) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger( - "HesaiRosWrapper::OnHesaiLidarMonitorTimerHttp(boost::system::system_error)"), - error.what()); - } - RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiLidarMonitorTimerHttp END"); -} - -void HesaiRosWrapper::OnHesaiLidarMonitorTimer() -{ - RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiLidarMonitorTimer"); - try { - auto result = hw_interface_.GetLidarMonitor(); - std::scoped_lock lock(mtx_lidar_monitor_); - current_lidar_monitor_time_.reset(new rclcpp::Time(this->get_clock()->now())); - current_monitor_.reset(new HesaiLidarMonitor(result)); - } catch (const std::system_error & error) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("HesaiRosWrapper::OnHesaiLidarMonitorTimer(std::system_error)"), - error.what()); - } catch (const boost::system::system_error & error) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("HesaiRosWrapper::OnHesaiLidarMonitorTimer(boost::system::system_error)"), - error.what()); - } - RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiLidarMonitorTimer END"); -} - -void HesaiRosWrapper::HesaiCheckStatus(diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - std::scoped_lock lock(mtx_lidar_status_); - if (current_status_) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::vector msg; - - diagnostics.add("system_uptime", std::to_string(current_status_->system_uptime)); - diagnostics.add("startup_times", std::to_string(current_status_->startup_times)); - diagnostics.add("total_operation_time", std::to_string(current_status_->total_operation_time)); - - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } else { - diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); - } -} - -void HesaiRosWrapper::HesaiCheckPtp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - std::scoped_lock lock(mtx_lidar_status_); - if (current_status_) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::vector msg; - auto gps_status = current_status_->get_str_gps_pps_lock(); - auto gprmc_status = current_status_->get_str_gps_gprmc_status(); - auto ptp_status = current_status_->get_str_ptp_clock_status(); - std::transform(gps_status.cbegin(), gps_status.cend(), gps_status.begin(), toupper); - std::transform(gprmc_status.cbegin(), gprmc_status.cend(), gprmc_status.begin(), toupper); - std::transform(ptp_status.cbegin(), ptp_status.cend(), ptp_status.begin(), toupper); - diagnostics.add("gps_pps_lock", gps_status); - diagnostics.add("gps_gprmc_status", gprmc_status); - diagnostics.add("ptp_clock_status", ptp_status); - if (gps_status != "UNKNOWN") { - msg.emplace_back("gps_pps_lock: " + gps_status); - } - if (gprmc_status != "UNKNOWN") { - msg.emplace_back("gprmc_status: " + gprmc_status); - } - if (ptp_status != "UNKNOWN") { - msg.emplace_back("ptp_status: " + ptp_status); - } - if (ptp_status == "FREE RUN" && gps_status == "UNKNOWN") { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - } - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } else { - diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); - } -} - -void HesaiRosWrapper::HesaiCheckTemperature( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - std::scoped_lock lock(mtx_lidar_status_); - if (current_status_) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::vector msg; - for (size_t i = 0; i < current_status_->temperature.size(); i++) { - diagnostics.add( - temperature_names_[i], GetFixedPrecisionString(current_status_->temperature[i] * 0.01)); - } - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } else { - diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); - } -} - -void HesaiRosWrapper::HesaiCheckRpm(diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +void HesaiRosWrapper::ReceiveCloudPacketCallback(std::vector& packet) { - std::scoped_lock lock(mtx_lidar_status_); - if (current_status_) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::vector msg; - diagnostics.add("motor_speed", std::to_string(current_status_->motor_speed)); - - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } else { - diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); + if (!decoder_wrapper_ || decoder_wrapper_->Status() != Status::OK) + { + return; } -} -void HesaiRosWrapper::HesaiCheckVoltageHttp( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - std::scoped_lock lock(mtx_lidar_monitor_); - if (current_lidar_monitor_tree_) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::vector msg; - std::string key = ""; - - std::string mes; - key = "lidarInCur"; - try { - mes = GetPtreeValue(current_lidar_monitor_tree_.get(), "Body." + key); - } catch (boost::bad_lexical_cast & ex) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = MSG_ERROR + std::string(ex.what()); - } - diagnostics.add(key, mes); - key = "lidarInVol"; - try { - mes = GetPtreeValue(current_lidar_monitor_tree_.get(), "Body." + key); - } catch (boost::bad_lexical_cast & ex) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = MSG_ERROR + std::string(ex.what()); - } - diagnostics.add(key, mes); - - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } else { - diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); - } -} + const auto now = std::chrono::system_clock::now(); + const auto timestamp_ns = std::chrono::duration_cast(now.time_since_epoch()).count(); -void HesaiRosWrapper::HesaiCheckVoltage(diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - std::scoped_lock lock(mtx_lidar_monitor_); - if (current_monitor_) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::vector msg; - diagnostics.add( - "input_voltage", GetFixedPrecisionString(current_monitor_->input_voltage * 0.01) + " V"); - diagnostics.add( - "input_current", GetFixedPrecisionString(current_monitor_->input_current * 0.01) + " mA"); - diagnostics.add( - "input_power", GetFixedPrecisionString(current_monitor_->input_power * 0.01) + " W"); + auto msg_ptr = std::make_unique(); + msg_ptr->stamp.sec = static_cast(timestamp_ns / 1'000'000'000); + msg_ptr->stamp.nanosec = static_cast(timestamp_ns % 1'000'000'000); + msg_ptr->data.swap(packet); - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } else { - diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); + if (!packet_queue_.try_push(std::move(msg_ptr))) + { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped"); } } diff --git a/nebula_ros/src/hesai/hw_interface_wrapper.cpp b/nebula_ros/src/hesai/hw_interface_wrapper.cpp new file mode 100644 index 000000000..e38f6b181 --- /dev/null +++ b/nebula_ros/src/hesai/hw_interface_wrapper.cpp @@ -0,0 +1,94 @@ +#include "nebula_ros/hesai/hw_interface_wrapper.hpp" + +namespace nebula +{ +namespace ros +{ + +HesaiHwInterfaceWrapper::HesaiHwInterfaceWrapper(rclcpp::Node* const parent_node, + std::shared_ptr& config) + : hw_interface_(new nebula::drivers::HesaiHwInterface()) + , logger_(parent_node->get_logger().get_child("HwInterface")) + , status_(Status::NOT_INITIALIZED) +{ + bool setup_sensor, retry_connect; + + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + parent_node->declare_parameter("setup_sensor", true, descriptor); + setup_sensor = parent_node->get_parameter("setup_sensor").as_bool(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + parent_node->declare_parameter("retry_hw", true, descriptor); + retry_connect = parent_node->get_parameter("retry_hw").as_bool(); + } + + hw_interface_->SetSensorConfiguration(std::static_pointer_cast(config)); + + status_ = Status::OK; + + hw_interface_->SetLogger(std::make_shared(parent_node->get_logger())); + hw_interface_->SetTargetModel(config->sensor_model); + + int retry_count = 0; + + while (true) + { + status_ = hw_interface_->InitializeTcpDriver(); + if (status_ == Status::OK || !retry_connect) + { + break; + } + + retry_count++; + std::this_thread::sleep_for(std::chrono::milliseconds(8000)); // >5000 + RCLCPP_WARN_STREAM(logger_, "Retry: " << retry_count); + } + + if (status_ == Status::OK) + { + try + { + auto inventory = hw_interface_->GetInventory(); + RCLCPP_INFO_STREAM(logger_, inventory); + hw_interface_->SetTargetModel(inventory.model); + } + catch (...) + { + RCLCPP_ERROR_STREAM(logger_, "Failed to get model from sensor..."); + } + if (setup_sensor) + { + hw_interface_->CheckAndSetConfig(); + // updateParameters(); TODO + } + } + else + { + RCLCPP_ERROR_STREAM(logger_, "Failed to get model from sensor... Set from config: " << config->sensor_model); + } + + status_ = Status::OK; +} + +Status HesaiHwInterfaceWrapper::Status() +{ + return status_; +} + +std::shared_ptr HesaiHwInterfaceWrapper::HwInterface() const +{ + return hw_interface_; +} + +} // namespace ros +} // namespace nebula \ No newline at end of file diff --git a/nebula_ros/src/hesai/hw_monitor_wrapper.cpp b/nebula_ros/src/hesai/hw_monitor_wrapper.cpp new file mode 100644 index 000000000..579e32a4e --- /dev/null +++ b/nebula_ros/src/hesai/hw_monitor_wrapper.cpp @@ -0,0 +1,412 @@ +#include "nebula_ros/hesai/hw_monitor_wrapper.hpp" + +namespace nebula +{ +namespace ros +{ +HesaiHwMonitorWrapper::HesaiHwMonitorWrapper(rclcpp::Node* const parent_node, + const std::shared_ptr& hw_interface, + std::shared_ptr& config) + : logger_(parent_node->get_logger().get_child("HwMonitor")) + , diagnostics_updater_(parent_node) + , status_(Status::OK) + , hw_interface_(hw_interface) + , parent_node_(parent_node) +{ + { + 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"; + parent_node->declare_parameter("diag_span", 1000, descriptor); + diag_span_ = parent_node->get_parameter("diag_span").as_int(); + } + + switch (config->sensor_model) + { + case nebula::drivers::SensorModel::HESAI_PANDARXT32: + case nebula::drivers::SensorModel::HESAI_PANDARXT32M: + case nebula::drivers::SensorModel::HESAI_PANDARAT128: + temperature_names_.emplace_back("Bottom circuit board T1"); + temperature_names_.emplace_back("Bottom circuit board T2"); + temperature_names_.emplace_back("Laser emitting board RT_L1(Internal)"); + temperature_names_.emplace_back("Laser emitting board RT_L2"); + temperature_names_.emplace_back("Receiving board RT_R"); + temperature_names_.emplace_back("Receiving board RT2"); + temperature_names_.emplace_back("Top circuit RT3"); + temperature_names_.emplace_back("Not used"); + break; + case nebula::drivers::SensorModel::HESAI_PANDAR64: + case nebula::drivers::SensorModel::HESAI_PANDAR40P: + case nebula::drivers::SensorModel::HESAI_PANDAR40M: + case nebula::drivers::SensorModel::HESAI_PANDARQT64: + case nebula::drivers::SensorModel::HESAI_PANDARQT128: + case nebula::drivers::SensorModel::HESAI_PANDAR128_E3X: + case nebula::drivers::SensorModel::HESAI_PANDAR128_E4X: + default: + temperature_names_.emplace_back("Bottom circuit RT1"); + temperature_names_.emplace_back("Bottom circuit RT2"); + temperature_names_.emplace_back("Internal Temperature"); + temperature_names_.emplace_back("Laser emitting board RT1"); + temperature_names_.emplace_back("Laser emitting board RT2"); + temperature_names_.emplace_back("Receiving board RT1"); + temperature_names_.emplace_back("Top circuit RT1"); + temperature_names_.emplace_back("Top circuit RT2"); + break; + } + + auto result = hw_interface->GetInventory(); + current_inventory_.reset(new HesaiInventory(result)); + current_inventory_time_.reset(new rclcpp::Time(parent_node->get_clock()->now())); + std::cout << "HesaiInventory" << std::endl; + std::cout << result << std::endl; + info_model_ = result.get_str_model(); + info_serial_ = std::string(std::begin(result.sn), std::end(result.sn)); + RCLCPP_INFO_STREAM(logger_, "Model:" << info_model_); + RCLCPP_INFO_STREAM(logger_, "Serial:" << info_serial_); + InitializeHesaiDiagnostics(); + + if (Status::OK != status_) + { + throw std::runtime_error((std::stringstream() << "Error getting calibration data: " << status_).str()); + } +} + +void HesaiHwMonitorWrapper::InitializeHesaiDiagnostics() +{ + RCLCPP_INFO_STREAM(logger_, "InitializeHesaiDiagnostics"); + using std::chrono_literals::operator""s; + std::ostringstream os; + auto hardware_id = info_model_ + ": " + info_serial_; + diagnostics_updater_.setHardwareID(hardware_id); + RCLCPP_INFO_STREAM(logger_, "hardware_id: " + hardware_id); + + diagnostics_updater_.add("hesai_status", this, &HesaiHwMonitorWrapper::HesaiCheckStatus); + diagnostics_updater_.add("hesai_ptp", this, &HesaiHwMonitorWrapper::HesaiCheckPtp); + diagnostics_updater_.add("hesai_temperature", this, &HesaiHwMonitorWrapper::HesaiCheckTemperature); + diagnostics_updater_.add("hesai_rpm", this, &HesaiHwMonitorWrapper::HesaiCheckRpm); + + current_status_.reset(); + current_monitor_.reset(); + current_status_time_.reset(new rclcpp::Time(parent_node_->get_clock()->now())); + current_lidar_monitor_time_.reset(new rclcpp::Time(parent_node_->get_clock()->now())); + current_diag_status_ = diagnostic_msgs::msg::DiagnosticStatus::STALE; + current_monitor_status_ = diagnostic_msgs::msg::DiagnosticStatus::STALE; + + auto fetch_diag_from_sensor = [this]() { + OnHesaiStatusTimer(); + if (hw_interface_->UseHttpGetLidarMonitor()) + { + OnHesaiLidarMonitorTimerHttp(); + } + else + { + OnHesaiLidarMonitorTimer(); + } + }; + + fetch_diagnostics_timer_ = + parent_node_->create_wall_timer(std::chrono::milliseconds(diag_span_), std::move(fetch_diag_from_sensor)); + + if (hw_interface_->UseHttpGetLidarMonitor()) + { + diagnostics_updater_.add("hesai_voltage", this, &HesaiHwMonitorWrapper::HesaiCheckVoltageHttp); + } + else + { + diagnostics_updater_.add("hesai_voltage", this, &HesaiHwMonitorWrapper::HesaiCheckVoltage); + } + + auto on_timer_update = [this] { + RCLCPP_DEBUG_STREAM(logger_, "OnUpdateTimer"); + auto now = parent_node_->get_clock()->now(); + auto dif = (now - *current_status_time_).seconds(); + + RCLCPP_DEBUG_STREAM(logger_, "dif(status): " << dif); + + if (diag_span_ * 2.0 < dif * 1000) + { + current_diag_status_ = diagnostic_msgs::msg::DiagnosticStatus::STALE; + RCLCPP_DEBUG_STREAM(logger_, "STALE"); + } + else + { + current_diag_status_ = diagnostic_msgs::msg::DiagnosticStatus::OK; + RCLCPP_DEBUG_STREAM(logger_, "OK"); + } + dif = (now - *current_lidar_monitor_time_).seconds(); + RCLCPP_DEBUG_STREAM(logger_, "dif(monitor): " << dif); + if (diag_span_ * 2.0 < dif * 1000) + { + current_monitor_status_ = diagnostic_msgs::msg::DiagnosticStatus::STALE; + RCLCPP_DEBUG_STREAM(logger_, "STALE"); + } + else + { + current_monitor_status_ = diagnostic_msgs::msg::DiagnosticStatus::OK; + RCLCPP_DEBUG_STREAM(logger_, "OK"); + } + diagnostics_updater_.force_update(); + }; + diagnostics_update_timer_ = + parent_node_->create_wall_timer(std::chrono::milliseconds(1000), std::move(on_timer_update)); + + RCLCPP_DEBUG_STREAM(logger_, "add_timer"); +} + +std::string HesaiHwMonitorWrapper::GetPtreeValue(boost::property_tree::ptree* pt, const std::string& key) +{ + boost::optional value = pt->get_optional(key); + if (value) + { + return value.get(); + } + else + { + return MSG_NOT_SUPPORTED; + } +} +std::string HesaiHwMonitorWrapper::GetFixedPrecisionString(double val, int pre) +{ + std::stringstream ss; + ss << std::fixed << std::setprecision(pre) << val; + return ss.str(); +} + +void HesaiHwMonitorWrapper::OnHesaiStatusTimer() +{ + RCLCPP_DEBUG_STREAM(logger_, "OnHesaiStatusTimer" << std::endl); + try + { + auto result = hw_interface_->GetLidarStatus(); + std::scoped_lock lock(mtx_lidar_status_); + current_status_time_.reset(new rclcpp::Time(parent_node_->get_clock()->now())); + current_status_.reset(new HesaiLidarStatus(result)); + } + catch (const std::system_error& error) + { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiStatusTimer(std::system_error)"), + error.what()); + } + catch (const boost::system::system_error& error) + { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiStatusTimer(boost::system::system_error)"), + error.what()); + } + RCLCPP_DEBUG_STREAM(logger_, "OnHesaiStatusTimer END" << std::endl); +} + +void HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimerHttp() +{ + RCLCPP_DEBUG_STREAM(logger_, "OnHesaiLidarMonitorTimerHttp"); + try + { + hw_interface_->GetLidarMonitorAsyncHttp([this](const std::string& str) { + std::scoped_lock lock(mtx_lidar_monitor_); + current_lidar_monitor_time_.reset(new rclcpp::Time(parent_node_->get_clock()->now())); + current_lidar_monitor_tree_ = std::make_unique(hw_interface_->ParseJson(str)); + }); + } + catch (const std::system_error& error) + { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimerHttp(std::system_error)"), + error.what()); + } + catch (const boost::system::system_error& error) + { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimerHttp(boost::system::system_" + "error)"), + error.what()); + } + RCLCPP_DEBUG_STREAM(logger_, "OnHesaiLidarMonitorTimerHttp END"); +} + +void HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimer() +{ + RCLCPP_DEBUG_STREAM(logger_, "OnHesaiLidarMonitorTimer"); + try + { + auto result = hw_interface_->GetLidarMonitor(); + std::scoped_lock lock(mtx_lidar_monitor_); + current_lidar_monitor_time_.reset(new rclcpp::Time(parent_node_->get_clock()->now())); + current_monitor_.reset(new HesaiLidarMonitor(result)); + } + catch (const std::system_error& error) + { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimer(std::system_error)"), + error.what()); + } + catch (const boost::system::system_error& error) + { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimer(boost::system::system_" + "error)"), + error.what()); + } + RCLCPP_DEBUG_STREAM(logger_, "OnHesaiLidarMonitorTimer END"); +} + +void HesaiHwMonitorWrapper::HesaiCheckStatus(diagnostic_updater::DiagnosticStatusWrapper& diagnostics) +{ + std::scoped_lock lock(mtx_lidar_status_); + if (current_status_) + { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; + + diagnostics.add("system_uptime", std::to_string(current_status_->system_uptime.value())); + diagnostics.add("startup_times", std::to_string(current_status_->startup_times.value())); + diagnostics.add("total_operation_time", std::to_string(current_status_->total_operation_time.value())); + + diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + } + else + { + diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); + } +} + +void HesaiHwMonitorWrapper::HesaiCheckPtp(diagnostic_updater::DiagnosticStatusWrapper& diagnostics) +{ + std::scoped_lock lock(mtx_lidar_status_); + if (current_status_) + { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; + auto gps_status = current_status_->get_str_gps_pps_lock(); + auto gprmc_status = current_status_->get_str_gps_gprmc_status(); + auto ptp_status = current_status_->get_str_ptp_clock_status(); + std::transform(gps_status.cbegin(), gps_status.cend(), gps_status.begin(), toupper); + std::transform(gprmc_status.cbegin(), gprmc_status.cend(), gprmc_status.begin(), toupper); + std::transform(ptp_status.cbegin(), ptp_status.cend(), ptp_status.begin(), toupper); + diagnostics.add("gps_pps_lock", gps_status); + diagnostics.add("gps_gprmc_status", gprmc_status); + diagnostics.add("ptp_clock_status", ptp_status); + if (gps_status != "UNKNOWN") + { + msg.emplace_back("gps_pps_lock: " + gps_status); + } + if (gprmc_status != "UNKNOWN") + { + msg.emplace_back("gprmc_status: " + gprmc_status); + } + if (ptp_status != "UNKNOWN") + { + msg.emplace_back("ptp_status: " + ptp_status); + } + if (ptp_status == "FREE RUN" && gps_status == "UNKNOWN") + { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + } + diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + } + else + { + diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); + } +} + +void HesaiHwMonitorWrapper::HesaiCheckTemperature(diagnostic_updater::DiagnosticStatusWrapper& diagnostics) +{ + std::scoped_lock lock(mtx_lidar_status_); + if (current_status_) + { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; + for (size_t i = 0; i < std::size(current_status_->temperature); i++) + { + diagnostics.add(temperature_names_[i], + GetFixedPrecisionString(current_status_->temperature[i].value() * 0.01, 3)); + } + diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + } + else + { + diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); + } +} + +void HesaiHwMonitorWrapper::HesaiCheckRpm(diagnostic_updater::DiagnosticStatusWrapper& diagnostics) +{ + std::scoped_lock lock(mtx_lidar_status_); + if (current_status_) + { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; + diagnostics.add("motor_speed", std::to_string(current_status_->motor_speed.value())); + + diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + } + else + { + diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); + } +} + +void HesaiHwMonitorWrapper::HesaiCheckVoltageHttp(diagnostic_updater::DiagnosticStatusWrapper& diagnostics) +{ + std::scoped_lock lock(mtx_lidar_monitor_); + if (current_lidar_monitor_tree_) + { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; + std::string key = ""; + + std::string mes; + key = "lidarInCur"; + try + { + mes = GetPtreeValue(current_lidar_monitor_tree_.get(), "Body." + key); + } + catch (boost::bad_lexical_cast& ex) + { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = MSG_ERROR + std::string(ex.what()); + } + diagnostics.add(key, mes); + key = "lidarInVol"; + try + { + mes = GetPtreeValue(current_lidar_monitor_tree_.get(), "Body." + key); + } + catch (boost::bad_lexical_cast& ex) + { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = MSG_ERROR + std::string(ex.what()); + } + diagnostics.add(key, mes); + + diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + } + else + { + diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); + } +} + +void HesaiHwMonitorWrapper::HesaiCheckVoltage(diagnostic_updater::DiagnosticStatusWrapper& diagnostics) +{ + std::scoped_lock lock(mtx_lidar_monitor_); + if (current_monitor_) + { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; + diagnostics.add("input_voltage", GetFixedPrecisionString(current_monitor_->input_voltage.value() * 0.01, 3) + " V"); + diagnostics.add("input_current", GetFixedPrecisionString(current_monitor_->input_current.value() * 0.01, 3) + " m" + "A"); + diagnostics.add("input_power", GetFixedPrecisionString(current_monitor_->input_power.value() * 0.01, 3) + " W"); + + diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + } + else + { + diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); + } +} + +Status HesaiHwMonitorWrapper::Status() +{ + return Status::NOT_IMPLEMENTED; // TODO +} +} // namespace ros +} // namespace nebula \ No newline at end of file From 9145a6daf49a7cf598a94d8db69c529be99c7b4a Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 16 Apr 2024 17:57:28 +0900 Subject: [PATCH 30/57] chore: update cspell ignore --- nebula_common/src/velodyne/velodyne_calibration_decoder.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nebula_common/src/velodyne/velodyne_calibration_decoder.cpp b/nebula_common/src/velodyne/velodyne_calibration_decoder.cpp index 0c03c3c9e..954da496b 100644 --- a/nebula_common/src/velodyne/velodyne_calibration_decoder.cpp +++ b/nebula_common/src/velodyne/velodyne_calibration_decoder.cpp @@ -1,3 +1,4 @@ +// cspell:ignore piyush, fout /** * \author Piyush Khandelwal (piyushk@cs.utexas.edu) * Copyright (C) 2012, Austin Robot Technology, From de21908834dec1c78c47557549b84d3e8a533923 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 18 Apr 2024 16:40:54 +0900 Subject: [PATCH 31/57] chore(velodyne_calibration_decoder): fix spelling --- nebula_common/src/velodyne/velodyne_calibration_decoder.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nebula_common/src/velodyne/velodyne_calibration_decoder.cpp b/nebula_common/src/velodyne/velodyne_calibration_decoder.cpp index 954da496b..0016e8208 100644 --- a/nebula_common/src/velodyne/velodyne_calibration_decoder.cpp +++ b/nebula_common/src/velodyne/velodyne_calibration_decoder.cpp @@ -1,4 +1,3 @@ -// cspell:ignore piyush, fout /** * \author Piyush Khandelwal (piyushk@cs.utexas.edu) * Copyright (C) 2012, Austin Robot Technology, @@ -7,6 +6,8 @@ * License: Modified BSD License */ +// cspell:ignore piyush, piyushk, fout + #include #ifdef HAVE_NEW_YAMLCPP From 21cdffcf443a7ade04fde24e63ec84dedc29ec75 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 18 Apr 2024 16:43:32 +0900 Subject: [PATCH 32/57] chore(nebula_common): remove debug code --- .../nebula_common/util/instrumentation.hpp | 86 ------------------- .../util/performance_counter.hpp | 43 ---------- .../nebula_common/util/topic_delay.hpp | 68 --------------- 3 files changed, 197 deletions(-) delete mode 100644 nebula_common/include/nebula_common/util/instrumentation.hpp delete mode 100644 nebula_common/include/nebula_common/util/performance_counter.hpp delete mode 100644 nebula_common/include/nebula_common/util/topic_delay.hpp diff --git a/nebula_common/include/nebula_common/util/instrumentation.hpp b/nebula_common/include/nebula_common/util/instrumentation.hpp deleted file mode 100644 index ccce705d8..000000000 --- a/nebula_common/include/nebula_common/util/instrumentation.hpp +++ /dev/null @@ -1,86 +0,0 @@ -#pragma once - -#include "nebula_common/util/performance_counter.hpp" - -#include -#include -#include - -namespace nebula -{ -namespace util -{ - -using namespace std::chrono_literals; - -class Instrumentation -{ - using clock = std::chrono::high_resolution_clock; - using time_point = clock::time_point; - using duration = std::chrono::nanoseconds; - -public: - Instrumentation(std::string tag) - : tag_(std::move(tag)), last_print_(clock::now()), delays_(), rtts_() - { - } - - void tick() - { - auto now = clock::now(); - - if (last_tick_ != time_point::min()) { - auto rtt = now - last_tick_; - rtts_.update(rtt); - } - - last_tick_ = now; - } - - void tock() - { - auto now = clock::now(); - auto delay = now - last_tick_; - delays_.update(delay); - - if (now - last_print_ >= PRINT_INTERVAL) { - auto del = delays_.reset(); - auto rtt = rtts_.reset(); - - last_print_ = now; - - std::stringstream ss; - - ss << "{\"type\": \"instrumentation\", \"tag\": \"" << tag_ - << "\", \"del\": ["; - - for (duration & dt : *del) { - ss << dt.count() << ','; - } - - ss <<"null], \"rtt\": ["; - - for (duration & dt : *rtt) { - ss << dt.count() << ','; - } - - ss << "null]}" << std::endl; - - std::cout << ss.str(); - } - } - -private: - std::string tag_; - - time_point last_tick_{time_point::min()}; - time_point last_print_{time_point::min()}; - - Counter delays_; - Counter rtts_; - - const duration PRINT_INTERVAL = 1s; -}; - -} // namespace util -} // namespace nebula \ No newline at end of file diff --git a/nebula_common/include/nebula_common/util/performance_counter.hpp b/nebula_common/include/nebula_common/util/performance_counter.hpp deleted file mode 100644 index cd6b618f1..000000000 --- a/nebula_common/include/nebula_common/util/performance_counter.hpp +++ /dev/null @@ -1,43 +0,0 @@ -#pragma once - -#include -#include -#include - -namespace nebula -{ -namespace util -{ - -class Counter -{ - using clock = std::chrono::high_resolution_clock; - using time_point = clock::time_point; - using duration = std::chrono::nanoseconds; - -public: - Counter() - : buf_record_(), buf_output_() - { - buf_record_.resize(100000); - buf_output_.resize(100000); - } - - std::vector * reset() - { - buf_record_.swap(buf_output_); - buf_record_.clear(); - return &buf_output_; - } - - void update(duration measurement) - { - buf_record_.push_back(measurement); - } - -private: - std::vector buf_record_; - std::vector buf_output_; -}; -} // namespace util -} // namespace nebula \ No newline at end of file diff --git a/nebula_common/include/nebula_common/util/topic_delay.hpp b/nebula_common/include/nebula_common/util/topic_delay.hpp deleted file mode 100644 index a92ce29a2..000000000 --- a/nebula_common/include/nebula_common/util/topic_delay.hpp +++ /dev/null @@ -1,68 +0,0 @@ -#pragma once - -#include - -#include "nebula_common/util/performance_counter.hpp" - - -#include -#include -#include - -namespace nebula -{ -namespace util -{ - -using namespace std::chrono_literals; - -class TopicDelay -{ - using clock = std::chrono::high_resolution_clock; - using time_point = clock::time_point; - using duration = std::chrono::nanoseconds; - -public: - TopicDelay(std::string tag) : tag_(std::move(tag)), last_print_(clock::now()), delays_() {} - - void tick(builtin_interfaces::msg::Time & stamp) - { - auto now = clock::now(); - - time_point t_msg = time_point(duration(static_cast(stamp.sec) * 1'000'000'000ULL + stamp.nanosec)); - auto delay = now - t_msg; - - delays_.update(delay); - - if (now - last_print_ >= PRINT_INTERVAL) { - auto del = delays_.reset(); - - last_print_ = now; - - std::stringstream ss; - - ss << "{\"type\": \"topic_delay\", \"tag\": \"" << tag_ - << "\", \"del\": ["; - - for (duration & dt : *del) { - ss << dt.count() << ','; - } - - ss << "null]}" << std::endl; - - std::cout << ss.str(); - } - } - -private: - std::string tag_; - - time_point last_print_{time_point::min()}; - - Counter delays_; - - const duration PRINT_INTERVAL = 1s; -}; - -} // namespace util -} // namespace nebula \ No newline at end of file From 5dddb1760d67d40a5c7401e90f73288cb703e062 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 18 Apr 2024 16:40:54 +0900 Subject: [PATCH 33/57] chore(velodyne_calibration_decoder): fix spelling --- nebula_common/src/velodyne/velodyne_calibration_decoder.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nebula_common/src/velodyne/velodyne_calibration_decoder.cpp b/nebula_common/src/velodyne/velodyne_calibration_decoder.cpp index 0016e8208..bfea254b7 100644 --- a/nebula_common/src/velodyne/velodyne_calibration_decoder.cpp +++ b/nebula_common/src/velodyne/velodyne_calibration_decoder.cpp @@ -1,4 +1,5 @@ /** + * cspell:ignore piyush, piyushk, fout * \author Piyush Khandelwal (piyushk@cs.utexas.edu) * Copyright (C) 2012, Austin Robot Technology, * The University of Texas at Austin From d8ab51a7c3dab13ac0df873062bd5bd01293f1a1 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 18 Apr 2024 19:25:11 +0900 Subject: [PATCH 34/57] fix(hesai_decoder): initialize last_phase to prevent empty pointcloud published on startup --- .../nebula_decoders_hesai/decoders/hesai_decoder.hpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp index 8f28856e8..daf9406cc 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp @@ -174,6 +174,10 @@ class HesaiDecoder : public HesaiScanDecoder /// @return Whether the scan has completed bool checkScanCompleted(uint32_t current_phase, uint32_t sync_phase) { + if (last_phase_ == -1) { + return false; + } + return angle_corrector_.hasScanned(current_phase, last_phase_, sync_phase); } @@ -220,6 +224,8 @@ class HesaiDecoder : public HesaiScanDecoder decode_pc_->reserve(SensorT::MAX_SCAN_BUFFER_POINTS); output_pc_->reserve(SensorT::MAX_SCAN_BUFFER_POINTS); + + last_phase_ = -1; // Dummy value to signal last_phase_ has not been set yet } int unpack(const std::vector & packet) override @@ -247,7 +253,7 @@ class HesaiDecoder : public HesaiScanDecoder sensor_configuration_->scan_phase * SensorT::packet_t::DEGREE_SUBDIVISIONS); if (scan_completed) { - std::swap(decode_pc_, output_pc_); + std::swap(decode_pc_, output_pc_); decode_pc_->clear(); has_scanned_ = true; output_scan_timestamp_ns_ = decode_scan_timestamp_ns_; From efa730a33c1ae61a6eb4f371dc4f3e45243cf906 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 18 Apr 2024 19:25:36 +0900 Subject: [PATCH 35/57] fix(nebula_tests): make Hesai tests compile again --- nebula_tests/hesai/hesai_ros_decoder_test.cpp | 42 ++++++++----------- nebula_tests/hesai/hesai_ros_decoder_test.hpp | 30 +++++++------ .../hesai/hesai_ros_decoder_test_main.cpp | 2 + 3 files changed, 37 insertions(+), 37 deletions(-) diff --git a/nebula_tests/hesai/hesai_ros_decoder_test.cpp b/nebula_tests/hesai/hesai_ros_decoder_test.cpp index a07a4dabd..00f33fa1a 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test.cpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test.cpp @@ -46,12 +46,11 @@ HesaiRosDecoderTest::HesaiRosDecoderTest( correction_cfg_ptr_ = std::make_shared(correction_configuration); wrapper_status_ = InitializeDriver( std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_), - std::static_pointer_cast(correction_cfg_ptr_)); + correction_cfg_ptr_); } else { wrapper_status_ = InitializeDriver( std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_)); + calibration_cfg_ptr_); } RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); @@ -59,26 +58,12 @@ HesaiRosDecoderTest::HesaiRosDecoderTest( Status HesaiRosDecoderTest::InitializeDriver( std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) + std::shared_ptr calibration_configuration) { // driver should be initialized here with proper decoder driver_ptr_ = std::make_shared( std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration)); - return driver_ptr_->GetStatus(); -} - -Status HesaiRosDecoderTest::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration, - std::shared_ptr correction_configuration) -{ - // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast( - calibration_configuration), //); - std::static_pointer_cast(correction_configuration)); + calibration_configuration); return driver_ptr_->GetStatus(); } @@ -291,7 +276,6 @@ void HesaiRosDecoderTest::ReadBag( storage_options.storage_id = params_.storage_id; converter_options.output_serialization_format = params_.format; //"cdr"; rclcpp::Serialization serialization; - nebula::drivers::NebulaPointCloudPtr pointcloud(new nebula::drivers::NebulaPointCloud); rosbag2_cpp::Reader bag_reader(std::make_unique()); bag_reader.open(storage_options, converter_options); @@ -310,12 +294,20 @@ void HesaiRosDecoderTest::ReadBag( "Found data in topic " << bag_message->topic_name << ": " << bag_message->time_stamp); auto extracted_msg_ptr = std::make_shared(extracted_msg); - auto pointcloud_ts = driver_ptr_->ConvertScanToPointcloud(extracted_msg_ptr); - auto scan_timestamp = std::get<1>(pointcloud_ts); - pointcloud = std::get<0>(pointcloud_ts); - scan_callback(bag_message->time_stamp, scan_timestamp, pointcloud); - pointcloud.reset(new nebula::drivers::NebulaPointCloud); + for (auto & pkt : extracted_msg_ptr->packets) { + auto pointcloud_ts = driver_ptr_->ParseCloudPacket(std::vector(pkt.data.begin(), std::next(pkt.data.begin(), pkt.size))); + auto pointcloud = std::get<0>(pointcloud_ts); + auto scan_timestamp = std::get<1>(pointcloud_ts); + + if (!pointcloud) { + continue; + } + + std::cerr << "PC size: " << pointcloud->size() << std::endl; + + scan_callback(bag_message->time_stamp, scan_timestamp, pointcloud); + } } } } diff --git a/nebula_tests/hesai/hesai_ros_decoder_test.hpp b/nebula_tests/hesai/hesai_ros_decoder_test.hpp index a0477a366..43996b9ac 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test.hpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test.hpp @@ -48,9 +48,25 @@ struct HesaiRosDecoderTestParams double dual_return_distance_threshold = 0.1; }; +inline std::ostream & operator<<(std::ostream & os, nebula::ros::HesaiRosDecoderTestParams const & arg) { + return os << "sensor_model=" << arg.sensor_model << ", " + << "return_mode=" << arg.return_mode << ", " + << "calibration_file=" << arg.calibration_file << ", " + << "bag_path=" << arg.bag_path << ", " + << "correction_file=" << arg.correction_file << ", " + << "frame_id=" << arg.frame_id << ", " + << "scan_phase=" << arg.scan_phase << ", " + << "min_range=" << arg.min_range << ", " + << "max_range=" << arg.max_range << ", " + << "storage_id=" << arg.storage_id << ", " + << "format=" << arg.format << ", " + << "target_topic=" << arg.target_topic << ", " + << "dual_return_distance_threshold=" << arg.dual_return_distance_threshold << ", "; +} + /// @brief Testing decoder of pandar 40p (Keeps HesaiDriverRosWrapper structure as much as /// possible) -class HesaiRosDecoderTest final : public rclcpp::Node, NebulaDriverRosWrapperBase //, testing::Test +class HesaiRosDecoderTest final : public rclcpp::Node { std::shared_ptr driver_ptr_; Status wrapper_status_; @@ -65,17 +81,7 @@ class HesaiRosDecoderTest final : public rclcpp::Node, NebulaDriverRosWrapperBas /// @return Resulting status Status InitializeDriver( std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) override; - - /// @brief Initializing ros wrapper for AT - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @param correction_configuration CorrectionConfiguration for this driver - /// @return Resulting status - Status InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration, - std::shared_ptr correction_configuration); + std::shared_ptr calibration_configuration); /// @brief Get configurations (Magic numbers for Pandar40P is described, each function can be /// integrated if the ros parameter can be passed to Google Test) diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp b/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp index 96ff00c3c..d6574ccd4 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp @@ -142,6 +142,8 @@ TEST_P(DecoderTest, TestTimezone) auto gmt = timezone; hesai_driver_->ReadBag(scan_callback); + ASSERT_GT(decoded_timestamps.size(), 0U); + // Then, reset driver and timestamps vector for the next decode run TearDown(); SetUp(); From 784db44e1ed98c7608ee7884aa18fea3f5689d7d Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 18 Apr 2024 19:25:52 +0900 Subject: [PATCH 36/57] fix(nebula_examples): make Hesai examples compile again --- .../hesai_ros_offline_extract_bag_pcd.hpp | 9 +- .../hesai/hesai_ros_offline_extract_pcd.hpp | 14 +-- .../hesai_ros_offline_extract_bag_pcd.cpp | 91 +++++++++---------- .../hesai/hesai_ros_offline_extract_pcd.cpp | 41 ++++----- 4 files changed, 61 insertions(+), 94 deletions(-) diff --git a/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp b/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp index a7681323c..5136cbb12 100644 --- a/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp +++ b/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp @@ -35,7 +35,7 @@ namespace nebula { namespace ros { -class HesaiRosOfflineExtractBag final : public rclcpp::Node, NebulaDriverRosWrapperBase +class HesaiRosOfflineExtractBag final : public rclcpp::Node { std::shared_ptr driver_ptr_; Status wrapper_status_; @@ -46,12 +46,7 @@ class HesaiRosOfflineExtractBag final : public rclcpp::Node, NebulaDriverRosWrap Status InitializeDriver( std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) override; - - Status InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration, - std::shared_ptr correction_configuration); + std::shared_ptr calibration_configuration); Status GetParameters( drivers::HesaiSensorConfiguration & sensor_configuration, diff --git a/nebula_examples/include/hesai/hesai_ros_offline_extract_pcd.hpp b/nebula_examples/include/hesai/hesai_ros_offline_extract_pcd.hpp index 711502e6a..8e26515bf 100644 --- a/nebula_examples/include/hesai/hesai_ros_offline_extract_pcd.hpp +++ b/nebula_examples/include/hesai/hesai_ros_offline_extract_pcd.hpp @@ -36,7 +36,7 @@ namespace nebula namespace ros { /// @brief Offline hesai driver usage example (Output PCD data) -class HesaiRosOfflineExtractSample final : public rclcpp::Node, NebulaDriverRosWrapperBase +class HesaiRosOfflineExtractSample final : public rclcpp::Node { std::shared_ptr driver_ptr_; Status wrapper_status_; @@ -51,17 +51,7 @@ class HesaiRosOfflineExtractSample final : public rclcpp::Node, NebulaDriverRosW /// @return Resulting status Status InitializeDriver( std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) override; - - /// @brief Initializing ros wrapper for AT - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @param correction_configuration CorrectionConfiguration for this driver - /// @return Resulting status - Status InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration, - std::shared_ptr correction_configuration); + std::shared_ptr calibration_configuration); /// @brief Get configurations from ros parameters /// @param sensor_configuration Output of SensorConfiguration diff --git a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp index 9c1266347..02cc84c72 100644 --- a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp +++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp @@ -56,12 +56,11 @@ HesaiRosOfflineExtractBag::HesaiRosOfflineExtractBag( correction_cfg_ptr_ = std::make_shared(correction_configuration); wrapper_status_ = InitializeDriver( std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_), - std::static_pointer_cast(correction_cfg_ptr_)); + correction_cfg_ptr_); } else { wrapper_status_ = InitializeDriver( std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_)); + calibration_cfg_ptr_); } RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); @@ -69,26 +68,12 @@ HesaiRosOfflineExtractBag::HesaiRosOfflineExtractBag( Status HesaiRosOfflineExtractBag::InitializeDriver( std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) + std::shared_ptr calibration_configuration) { // driver should be initialized here with proper decoder driver_ptr_ = std::make_shared( std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration)); - return driver_ptr_->GetStatus(); -} - -Status HesaiRosOfflineExtractBag::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration, - std::shared_ptr correction_configuration) -{ - // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast( - calibration_configuration), //); - std::static_pointer_cast(correction_configuration)); + calibration_configuration); return driver_ptr_->GetStatus(); } @@ -328,39 +313,45 @@ Status HesaiRosOfflineExtractBag::ReadBag() std::cout << "Found data in topic " << bag_message->topic_name << ": " << bag_message->time_stamp << std::endl; - auto pointcloud_ts = driver_ptr_->ConvertScanToPointcloud( - std::make_shared(extracted_msg)); - auto pointcloud = std::get<0>(pointcloud_ts); - auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; - // pcl::io::savePCDFileBinary((o_dir / fn).string(), *pointcloud); + for (auto & pkt : extracted_msg.packets) { + auto pointcloud_ts = driver_ptr_->ParseCloudPacket(std::vector(pkt.data.begin(), std::next(pkt.data.begin(), pkt.size))); + auto pointcloud = std::get<0>(pointcloud_ts); - if (needs_open) { - const rosbag2_storage::StorageOptions storage_options_w( - {(o_dir / std::to_string(bag_message->time_stamp)).string(), "sqlite3"}); - const rosbag2_cpp::ConverterOptions converter_options_w( - {rmw_get_serialization_format(), rmw_get_serialization_format()}); - writer_ = std::make_unique(); - writer_->open(storage_options_w, converter_options_w); - writer_->create_topic( - {bag_message->topic_name, "pandar_msgs/msg/PandarScan", rmw_get_serialization_format(), - ""}); - needs_open = false; - } - writer_->write(bag_message); - cnt++; - if (skip_num < cnt) { - out_cnt++; - if (only_xyz) { - pcl::PointCloud cloud_xyz; - pcl::copyPointCloud(*pointcloud, cloud_xyz); - writer.writeBinary((o_dir / fn).string(), cloud_xyz); - } else { - writer.writeBinary((o_dir / fn).string(), *pointcloud); + if (!pointcloud) { + continue; + } + + auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; + // pcl::io::savePCDFileBinary((o_dir / fn).string(), *pointcloud); + + if (needs_open) { + const rosbag2_storage::StorageOptions storage_options_w( + {(o_dir / std::to_string(bag_message->time_stamp)).string(), "sqlite3"}); + const rosbag2_cpp::ConverterOptions converter_options_w( + {rmw_get_serialization_format(), rmw_get_serialization_format()}); + writer_ = std::make_unique(); + writer_->open(storage_options_w, converter_options_w); + writer_->create_topic( + {bag_message->topic_name, "pandar_msgs/msg/PandarScan", rmw_get_serialization_format(), + ""}); + needs_open = false; + } + writer_->write(bag_message); + cnt++; + if (skip_num < cnt) { + out_cnt++; + if (only_xyz) { + pcl::PointCloud cloud_xyz; + pcl::copyPointCloud(*pointcloud, cloud_xyz); + writer.writeBinary((o_dir / fn).string(), cloud_xyz); + } else { + writer.writeBinary((o_dir / fn).string(), *pointcloud); + } + // writer.writeASCII((o_dir / fn).string(), *pointcloud); + } + if (out_num <= out_cnt) { + break; } - // writer.writeASCII((o_dir / fn).string(), *pointcloud); - } - if (out_num <= out_cnt) { - break; } } } diff --git a/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp b/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp index a57d9b96e..bd992ee19 100644 --- a/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp +++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp @@ -56,12 +56,11 @@ HesaiRosOfflineExtractSample::HesaiRosOfflineExtractSample( correction_cfg_ptr_ = std::make_shared(correction_configuration); wrapper_status_ = InitializeDriver( std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_), - std::static_pointer_cast(correction_cfg_ptr_)); + correction_cfg_ptr_); } else { wrapper_status_ = InitializeDriver( std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_)); + calibration_cfg_ptr_); } RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); @@ -69,26 +68,12 @@ HesaiRosOfflineExtractSample::HesaiRosOfflineExtractSample( Status HesaiRosOfflineExtractSample::InitializeDriver( std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) + std::shared_ptr calibration_configuration) { // driver should be initialized here with proper decoder driver_ptr_ = std::make_shared( std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration)); - return driver_ptr_->GetStatus(); -} - -Status HesaiRosOfflineExtractSample::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration, - std::shared_ptr correction_configuration) -{ - // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast( - calibration_configuration), //); - std::static_pointer_cast(correction_configuration)); + calibration_configuration); return driver_ptr_->GetStatus(); } @@ -296,12 +281,18 @@ Status HesaiRosOfflineExtractSample::ReadBag() // nebula::drivers::NebulaPointCloudPtr pointcloud = // driver_ptr_->ConvertScanToPointcloud( // std::make_shared(extracted_msg)); - auto pointcloud_ts = driver_ptr_->ConvertScanToPointcloud( - std::make_shared(extracted_msg)); - auto pointcloud = std::get<0>(pointcloud_ts); - auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; - // pcl::io::savePCDFileBinary((o_dir / fn).string(), *pointcloud); - writer.writeBinary((o_dir / fn).string(), *pointcloud); + for (auto & pkt : extracted_msg.packets) { + auto pointcloud_ts = driver_ptr_->ParseCloudPacket(std::vector(pkt.data.begin(), std::next(pkt.data.begin(), pkt.size))); + auto pointcloud = std::get<0>(pointcloud_ts); + + if (!pointcloud) { + continue; + } + + auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; + // pcl::io::savePCDFileBinary((o_dir / fn).string(), *pointcloud); + writer.writeBinary((o_dir / fn).string(), *pointcloud); + } } } // close on scope exit From 01f5839e406a3bf8b16ad76e28e4e1ad6103fbb8 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 18 Apr 2024 19:29:03 +0900 Subject: [PATCH 37/57] chore(velodyne_calibration_decoder): fix spelling once and for all --- .cspell.json | 2 +- nebula_common/src/velodyne/velodyne_calibration_decoder.cpp | 3 --- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/.cspell.json b/.cspell.json index 5265d4bf1..228d7f45e 100644 --- a/.cspell.json +++ b/.cspell.json @@ -50,4 +50,4 @@ "Piyush", "fout" ] -} +} \ No newline at end of file diff --git a/nebula_common/src/velodyne/velodyne_calibration_decoder.cpp b/nebula_common/src/velodyne/velodyne_calibration_decoder.cpp index bfea254b7..0c03c3c9e 100644 --- a/nebula_common/src/velodyne/velodyne_calibration_decoder.cpp +++ b/nebula_common/src/velodyne/velodyne_calibration_decoder.cpp @@ -1,5 +1,4 @@ /** - * cspell:ignore piyush, piyushk, fout * \author Piyush Khandelwal (piyushk@cs.utexas.edu) * Copyright (C) 2012, Austin Robot Technology, * The University of Texas at Austin @@ -7,8 +6,6 @@ * License: Modified BSD License */ -// cspell:ignore piyush, piyushk, fout - #include #ifdef HAVE_NEW_YAMLCPP From ae28a717565bfe6f9b62d4fcb3a2582caeecdada Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 18 Apr 2024 20:21:56 +0900 Subject: [PATCH 38/57] fix(nebula_examples): fix test failure (remove ament_lint_common) --- nebula_examples/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_examples/package.xml b/nebula_examples/package.xml index 6f1163279..f63847cca 100644 --- a/nebula_examples/package.xml +++ b/nebula_examples/package.xml @@ -28,8 +28,8 @@ velodyne_msgs yaml-cpp + ament_cmake_gtest ament_lint_auto - ament_lint_common ament_cmake From f2494ff045ad43a3da19ec7216b8f7d1f17e8294 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Mon, 22 Apr 2024 11:30:28 +0900 Subject: [PATCH 39/57] fix(hesai_hw_interface): add missing check for PTC error, make error type more readable --- .../nebula_hw_interfaces_hesai/hesai_hw_interface.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp index 0bf3a864b..b0a443495 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp @@ -102,7 +102,6 @@ const int PTP_LOG_MIN_DELAY_INTERVAL = 0; const int HESAI_LIDAR_GPS_CLOCK_SOURCE = 0; const int HESAI_LIDAR_PTP_CLOCK_SOURCE = 1; - /// @brief Hardware interface of hesai driver class HesaiHwInterface : public NebulaHwInterfaceBase { From 36931bc2973c4cec84c8c4d624d9891a3d47f918 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 23 Apr 2024 15:35:45 +0900 Subject: [PATCH 40/57] refactor(hesai): re-introduce parameter update mechanism --- .../include/nebula_common/nebula_common.hpp | 6 +- .../decoders/angle_corrector.hpp | 8 +- .../angle_corrector_calibration_based.hpp | 8 +- .../angle_corrector_correction_based.hpp | 4 +- .../decoders/hesai_decoder.hpp | 18 +- .../nebula_decoders_hesai/hesai_driver.hpp | 8 +- .../nebula_decoders_hesai/hesai_driver.cpp | 14 +- .../hesai_hw_interface.hpp | 18 +- .../hesai_hw_interface.cpp | 16 +- nebula_ros/CMakeLists.txt | 1 + .../common/parameter_descriptors.hpp | 41 ++ .../nebula_ros/hesai/decoder_wrapper.hpp | 41 +- .../nebula_ros/hesai/hesai_ros_wrapper.hpp | 39 +- .../nebula_ros/hesai/hw_interface_wrapper.hpp | 9 +- .../nebula_ros/hesai/hw_monitor_wrapper.hpp | 13 +- .../src/common/parameter_descriptors.cpp | 29 ++ nebula_ros/src/hesai/decoder_wrapper.cpp | 118 +++-- nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 428 ++++++------------ nebula_ros/src/hesai/hw_interface_wrapper.cpp | 46 +- nebula_ros/src/hesai/hw_monitor_wrapper.cpp | 17 +- 20 files changed, 430 insertions(+), 452 deletions(-) create mode 100644 nebula_ros/include/nebula_ros/common/parameter_descriptors.hpp create mode 100644 nebula_ros/src/common/parameter_descriptors.cpp diff --git a/nebula_common/include/nebula_common/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp index 9d04a267a..cf1eb0841 100644 --- a/nebula_common/include/nebula_common/nebula_common.hpp +++ b/nebula_common/include/nebula_common/nebula_common.hpp @@ -346,7 +346,7 @@ enum class PtpProfile { IEEE_1588v2 = 0, IEEE_802_1AS, IEEE_802_1AS_AUTO, - PROFILE_UNKNOWN + UNKNOWN_PROFILE }; enum class PtpTransportType { @@ -624,7 +624,7 @@ inline PtpProfile PtpProfileFromString(const std::string & ptp_profile) if (tmp_str == "802.1as") return PtpProfile::IEEE_802_1AS; if (tmp_str == "automotive") return PtpProfile::IEEE_802_1AS_AUTO; - return PtpProfile::PROFILE_UNKNOWN; + return PtpProfile::UNKNOWN_PROFILE; } /// @brief Convert PtpProfile enum to string (Overloading the << operator) @@ -643,7 +643,7 @@ inline std::ostream & operator<<(std::ostream & os, nebula::drivers::PtpProfile case PtpProfile::IEEE_802_1AS_AUTO: os << "IEEE_802.1AS Automotive"; break; - case PtpProfile::PROFILE_UNKNOWN: + case PtpProfile::UNKNOWN_PROFILE: os << "UNKNOWN"; break; } diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp index 42fd0a008..121913c34 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp @@ -26,13 +26,13 @@ struct CorrectedAngleData class AngleCorrector { protected: - const std::shared_ptr sensor_calibration_; - const std::shared_ptr sensor_correction_; + const std::shared_ptr sensor_calibration_; + const std::shared_ptr sensor_correction_; public: AngleCorrector( - const std::shared_ptr & sensor_calibration, - const std::shared_ptr & sensor_correction) + const std::shared_ptr & sensor_calibration, + const std::shared_ptr & sensor_correction) : sensor_calibration_(sensor_calibration), sensor_correction_(sensor_correction) { } diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp index 3e0c999b0..bee570afc 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp @@ -27,8 +27,8 @@ class AngleCorrectorCalibrationBased : public AngleCorrector public: AngleCorrectorCalibrationBased( - const std::shared_ptr & sensor_calibration, - const std::shared_ptr & sensor_correction) + const std::shared_ptr & sensor_calibration, + const std::shared_ptr & sensor_correction) : AngleCorrector(sensor_calibration, sensor_correction) { if (sensor_calibration == nullptr) { @@ -37,8 +37,8 @@ class AngleCorrectorCalibrationBased : public AngleCorrector } for (size_t channel_id = 0; channel_id < ChannelN; ++channel_id) { - float elevation_angle_deg = sensor_calibration->elev_angle_map[channel_id]; - float azimuth_offset_deg = sensor_calibration->azimuth_offset_map[channel_id]; + float elevation_angle_deg = sensor_calibration->elev_angle_map.at(channel_id); + float azimuth_offset_deg = sensor_calibration->azimuth_offset_map.at(channel_id); elevation_angle_rad_[channel_id] = deg2rad(elevation_angle_deg); azimuth_offset_rad_[channel_id] = deg2rad(azimuth_offset_deg); diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_correction_based.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_correction_based.hpp index be60078de..d23dfc1f2 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_correction_based.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_correction_based.hpp @@ -43,8 +43,8 @@ class AngleCorrectorCorrectionBased : public AngleCorrector public: AngleCorrectorCorrectionBased( - const std::shared_ptr & sensor_calibration, - const std::shared_ptr & sensor_correction) + const std::shared_ptr & sensor_calibration, + const std::shared_ptr & sensor_correction) : AngleCorrector(sensor_calibration, sensor_correction), logger_(rclcpp::get_logger("AngleCorrectorCorrectionBased")) { diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp index daf9406cc..e8a9afc34 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp @@ -18,7 +18,7 @@ class HesaiDecoder : public HesaiScanDecoder { protected: /// @brief Configuration for this decoder - const std::shared_ptr sensor_configuration_; + const std::shared_ptr sensor_configuration_; /// @brief The sensor definition, used for return mode and time offset handling SensorT sensor_{}; @@ -34,13 +34,13 @@ class HesaiDecoder : public HesaiScanDecoder /// @brief The last decoded packet typename SensorT::packet_t packet_; /// @brief The last azimuth processed - int last_phase_; + int last_phase_ = -1; // Dummy value to signal last_phase_ has not been set yet /// @brief The timestamp of the last completed scan in nanoseconds - uint64_t output_scan_timestamp_ns_; + uint64_t output_scan_timestamp_ns_ = 0; /// @brief The timestamp of the scan currently in progress - uint64_t decode_scan_timestamp_ns_; + uint64_t decode_scan_timestamp_ns_ = 0; /// @brief Whether a full scan has been processed - bool has_scanned_; + bool has_scanned_ = false; rclcpp::Logger logger_; @@ -209,9 +209,9 @@ class HesaiDecoder : public HesaiScanDecoder /// @param correction_configuration Correction for this decoder (can be nullptr if /// calibration_configuration is set) explicit HesaiDecoder( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration, - const std::shared_ptr & correction_configuration) + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & calibration_configuration, + const std::shared_ptr & correction_configuration) : sensor_configuration_(sensor_configuration), angle_corrector_(calibration_configuration, correction_configuration), logger_(rclcpp::get_logger("HesaiDecoder")) @@ -224,8 +224,6 @@ class HesaiDecoder : public HesaiScanDecoder decode_pc_->reserve(SensorT::MAX_SCAN_BUFFER_POINTS); output_pc_->reserve(SensorT::MAX_SCAN_BUFFER_POINTS); - - last_phase_ = -1; // Dummy value to signal last_phase_ has not been set yet } int unpack(const std::vector & packet) override diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp index 5ee9892d4..87798dbd1 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp @@ -22,7 +22,7 @@ namespace nebula namespace drivers { /// @brief Hesai driver -class HesaiDriver : NebulaDriverBase +class HesaiDriver { private: /// @brief Current driver status @@ -37,8 +37,8 @@ class HesaiDriver : NebulaDriverBase /// @param calibration_configuration CalibrationConfiguration for this driver (either HesaiCalibrationConfiguration /// for sensors other than AT128 or HesaiCorrection for AT128) explicit HesaiDriver( - const std::shared_ptr& sensor_configuration, - const std::shared_ptr& calibration_configuration = nullptr); + const std::shared_ptr& sensor_configuration, + const std::shared_ptr& calibration_configuration = nullptr); /// @brief Get current status of this driver /// @return Current status @@ -47,7 +47,7 @@ class HesaiDriver : NebulaDriverBase /// @brief Setting CalibrationConfiguration (not used) /// @param calibration_configuration /// @return Resulting status - Status SetCalibrationConfiguration(const CalibrationConfigurationBase& calibration_configuration) override; + Status SetCalibrationConfiguration(const HesaiCalibrationConfigurationBase& calibration_configuration); /// @brief Convert PandarScan message to point cloud /// @param pandar_scan Message diff --git a/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp b/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp index 27624a469..beafd8e71 100644 --- a/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp +++ b/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp @@ -17,22 +17,22 @@ namespace nebula { namespace drivers { -HesaiDriver::HesaiDriver(const std::shared_ptr& sensor_configuration, - const std::shared_ptr& calibration_configuration) +HesaiDriver::HesaiDriver(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; - std::shared_ptr calibration = nullptr; - std::shared_ptr correction = nullptr; + std::shared_ptr calibration = nullptr; + std::shared_ptr correction = nullptr; if (sensor_configuration->sensor_model == SensorModel::HESAI_PANDARAT128) { - correction = std::static_pointer_cast(calibration_configuration); + correction = std::static_pointer_cast(calibration_configuration); } else { - calibration = std::static_pointer_cast(calibration_configuration); + calibration = std::static_pointer_cast(calibration_configuration); } switch (sensor_configuration->sensor_model) @@ -104,7 +104,7 @@ std::tuple HesaiDriver::ParseCloudPacket( } Status HesaiDriver::SetCalibrationConfiguration( - const CalibrationConfigurationBase & calibration_configuration) + const HesaiCalibrationConfigurationBase & calibration_configuration) { throw std::runtime_error( "SetCalibrationConfiguration. Not yet implemented (" + diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp index b0a443495..d3472afbe 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp @@ -103,7 +103,7 @@ const int HESAI_LIDAR_GPS_CLOCK_SOURCE = 0; const int HESAI_LIDAR_PTP_CLOCK_SOURCE = 1; /// @brief Hardware interface of hesai driver -class HesaiHwInterface : public NebulaHwInterfaceBase +class HesaiHwInterface { private: struct ptc_error_t @@ -120,10 +120,12 @@ class HesaiHwInterface : public NebulaHwInterfaceBase std::shared_ptr m_owned_ctx; std::unique_ptr<::drivers::udp_driver::UdpDriver> cloud_udp_driver_; std::shared_ptr<::drivers::tcp_driver::TcpDriver> tcp_driver_; - std::shared_ptr sensor_configuration_; + std::shared_ptr sensor_configuration_; std::function & buffer)> cloud_packet_callback_; /**This function pointer is called when the scan is complete*/ + std::mutex mtx_inflight_tcp_request_; + int prev_phase_{}; bool is_solid_state = false; @@ -195,14 +197,14 @@ class HesaiHwInterface : public NebulaHwInterfaceBase void ReceiveSensorPacketCallback(std::vector & buffer); /// @brief Starting the interface that handles UDP streams /// @return Resulting status - Status SensorInterfaceStart() final; + Status SensorInterfaceStart(); /// @brief Function for stopping the interface that handles UDP streams /// @return Resulting status - Status SensorInterfaceStop() final; + Status SensorInterfaceStop(); /// @brief Printing sensor configuration /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status - Status GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) final; + Status GetSensorConfiguration(const SensorConfigurationBase & sensor_configuration); /// @brief Printing calibration configuration /// @param calibration_configuration CalibrationConfiguration for the checking /// @return Resulting status @@ -211,7 +213,7 @@ class HesaiHwInterface : public NebulaHwInterfaceBase /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status Status SetSensorConfiguration( - std::shared_ptr sensor_configuration) final; + std::shared_ptr sensor_configuration); /// @brief Registering callback for PandarScan /// @param scan_callback Callback function /// @return Resulting status @@ -382,13 +384,13 @@ class HesaiHwInterface : public NebulaHwInterfaceBase /// @param hesai_config Current HesaiConfig /// @return Resulting status HesaiStatus CheckAndSetConfig( - std::shared_ptr sensor_configuration, HesaiConfig hesai_config); + std::shared_ptr sensor_configuration, HesaiConfig hesai_config); /// @brief Checking the current settings and changing the difference point /// @param sensor_configuration Current SensorConfiguration /// @param hesai_lidar_range_all Current HesaiLidarRangeAll /// @return Resulting status HesaiStatus CheckAndSetConfig( - std::shared_ptr sensor_configuration, + std::shared_ptr sensor_configuration, HesaiLidarRangeAll hesai_lidar_range_all); /// @brief Checking the current settings and changing the difference point /// @return Resulting status diff --git a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp index 5a40b6b60..4db22ceb8 100644 --- a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp @@ -28,6 +28,8 @@ HesaiHwInterface::~HesaiHwInterface() HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( const uint8_t command_id, const std::vector & payload) { + std::lock_guard lock(mtx_inflight_tcp_request_); + uint32_t len = payload.size(); std::vector send_buf; @@ -120,10 +122,10 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( } Status HesaiHwInterface::SetSensorConfiguration( - std::shared_ptr sensor_configuration) + std::shared_ptr sensor_configuration) { sensor_configuration_ = - std::static_pointer_cast(sensor_configuration); + std::static_pointer_cast(sensor_configuration); return Status::OK; } @@ -175,7 +177,7 @@ Status HesaiHwInterface::SensorInterfaceStop() return Status::ERROR_1; } -Status HesaiHwInterface::GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) +Status HesaiHwInterface::GetSensorConfiguration(const SensorConfigurationBase & sensor_configuration) { std::stringstream ss; ss << sensor_configuration; @@ -808,7 +810,7 @@ HesaiStatus HesaiHwInterface::GetLidarMonitorAsyncHttp( } HesaiStatus HesaiHwInterface::CheckAndSetConfig( - std::shared_ptr sensor_configuration, HesaiConfig hesai_config) + std::shared_ptr sensor_configuration, HesaiConfig hesai_config) { using namespace std::chrono_literals; #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE @@ -967,7 +969,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( } HesaiStatus HesaiHwInterface::CheckAndSetConfig( - std::shared_ptr sensor_configuration, + std::shared_ptr sensor_configuration, HesaiLidarRangeAll hesai_lidar_range_all) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE @@ -1031,7 +1033,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig() ss << result; PrintInfo(ss.str()); CheckAndSetConfig( - std::static_pointer_cast(sensor_configuration_), result); + std::static_pointer_cast(sensor_configuration_), result); }); t.join(); @@ -1041,7 +1043,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig() ss << result; PrintInfo(ss.str()); CheckAndSetConfig( - std::static_pointer_cast(sensor_configuration_), result); + std::static_pointer_cast(sensor_configuration_), result); }); t2.join(); #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE diff --git a/nebula_ros/CMakeLists.txt b/nebula_ros/CMakeLists.txt index b11339765..324d38074 100644 --- a/nebula_ros/CMakeLists.txt +++ b/nebula_ros/CMakeLists.txt @@ -40,6 +40,7 @@ ament_auto_add_library(hesai_ros_wrapper SHARED src/hesai/decoder_wrapper.cpp src/hesai/hw_interface_wrapper.cpp src/hesai/hw_monitor_wrapper.cpp + src/common/parameter_descriptors.cpp ) rclcpp_components_register_node(hesai_ros_wrapper diff --git a/nebula_ros/include/nebula_ros/common/parameter_descriptors.hpp b/nebula_ros/include/nebula_ros/common/parameter_descriptors.hpp new file mode 100644 index 000000000..b54d8ad63 --- /dev/null +++ b/nebula_ros/include/nebula_ros/common/parameter_descriptors.hpp @@ -0,0 +1,41 @@ +#pragma once + +#include +#include +#include + +namespace nebula +{ +namespace ros +{ + +rcl_interfaces::msg::ParameterDescriptor param_read_write(); + +rcl_interfaces::msg::ParameterDescriptor param_read_only(); + +rcl_interfaces::msg::ParameterDescriptor::_floating_point_range_type float_range(double start, double stop, + double step); + +rcl_interfaces::msg::ParameterDescriptor::_integer_range_type int_range(int start, int stop, int step); + +/// @brief Get a parameter's value from a list of parameters, if that parameter is in the list. +/// @tparam T The parameter's expected value type +/// @param p A vector of parameters +/// @param name Target parameter name +/// @param value (out) Parameter value. Set if parameter is found, left untouched otherwise. +/// @return Whether the target name existed +template +bool get_param(const std::vector& p, const std::string& name, T& value) +{ + auto it = std::find_if(p.cbegin(), p.cend(), + [&name](const rclcpp::Parameter & parameter) { return parameter.get_name() == name; }); + if (it != p.cend()) + { + value = it->template get_value(); + return true; + } + return false; +} + +} // namespace ros +} // namespace nebula \ No newline at end of file diff --git a/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp index 262c16839..3b2af26d4 100644 --- a/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp @@ -2,17 +2,19 @@ #include "nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" +#include "nebula_ros/common/parameter_descriptors.hpp" -#include #include +#include #include #include "nebula_msgs/msg/nebula_packet.hpp" #include "pandar_msgs/msg/pandar_packet.hpp" #include "pandar_msgs/msg/pandar_scan.hpp" -#include #include +#include +#include namespace nebula { @@ -20,22 +22,38 @@ namespace ros { class HesaiDecoderWrapper { + using get_calibration_result_t = + nebula::util::expected, nebula::Status>; + public: HesaiDecoderWrapper(rclcpp::Node* const parent_node, const std::shared_ptr& hw_interface, - std::shared_ptr& config); - - nebula::util::expected, nebula::Status> - GetCalibrationData(); + std::shared_ptr& config); void ProcessCloudPacket(std::unique_ptr packet_msg); - void PublishCloud(std::unique_ptr pointcloud, - const rclcpp::Publisher::SharedPtr& publisher); + void OnConfigChange(const std::shared_ptr& new_config); + + void + OnCalibrationChange(const std::shared_ptr& new_calibration); + + rcl_interfaces::msg::SetParametersResult OnParameterChange(const std::vector& p); nebula::Status Status(); private: + /// @brief Load calibration data from the best available source: + /// 1. If sensor connected, download and save from sensor + /// 2. If downloaded file available, load that file + /// 3. Load the file given by `calibration_file_path` + /// @param calibration_file_path The file to use if no better option is available + /// @param ignore_others If true, skip straight so step 3 above, ignoring better calibration options + /// @return The calibration data if successful, or an error code if not + get_calibration_result_t GetCalibrationData(const std::string& calibration_file_path, bool ignore_others = false); + + void PublishCloud(std::unique_ptr pointcloud, + const rclcpp::Publisher::SharedPtr& publisher); + /// @brief Convert seconds to chrono::nanoseconds /// @param seconds /// @return chrono::nanoseconds @@ -48,12 +66,13 @@ class HesaiDecoderWrapper rclcpp::Logger logger_; const std::shared_ptr hw_interface_; - std::shared_ptr sensor_cfg_; + std::shared_ptr sensor_cfg_; std::string calibration_file_path_{}; - std::shared_ptr calibration_cfg_ptr_{}; + std::shared_ptr calibration_cfg_ptr_{}; - std::shared_ptr driver_ptr_; + std::shared_ptr driver_ptr_{}; + std::mutex mtx_driver_ptr_; rclcpp::Publisher::SharedPtr packets_pub_{}; pandar_msgs::msg::PandarScan::UniquePtr current_scan_msg_{}; diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp index e037bce40..42d8bdc36 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp @@ -1,6 +1,7 @@ #pragma once #include "boost_tcp_driver/tcp_driver.hpp" + #include "nebula_common/hesai/hesai_common.hpp" #include "nebula_common/nebula_common.hpp" #include "nebula_common/nebula_status.hpp" @@ -9,6 +10,7 @@ #include "nebula_ros/hesai/hw_interface_wrapper.hpp" #include "nebula_ros/hesai/hw_monitor_wrapper.hpp" #include "nebula_ros/hesai/mt_queue.hpp" +#include "nebula_ros/common/parameter_descriptors.hpp" #include #include @@ -31,31 +33,12 @@ 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 Ros wrapper of hesai driver class HesaiRosWrapper final : public rclcpp::Node { public: explicit HesaiRosWrapper(const rclcpp::NodeOptions& options); - ~HesaiRosWrapper() noexcept; + ~HesaiRosWrapper() noexcept {}; /// @brief Get current status of this driver /// @return Current status @@ -72,23 +55,16 @@ class HesaiRosWrapper final : public rclcpp::Node Status DeclareAndGetSensorConfigParams(); - /// @brief Get configurations from ros parameters - /// @param sensor_configuration Output of SensorConfiguration - /// @return Resulting status - Status DeclareAndGetWrapperParams(); - /// @brief rclcpp parameter callback /// @param parameters Received parameters /// @return SetParametersResult - rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector& parameters); + rcl_interfaces::msg::SetParametersResult OnParameterChange(const std::vector& p); - /// @brief Updating rclcpp parameter - /// @return SetParametersResult - std::vector updateParameters(); + Status ValidateAndSetConfig(std::shared_ptr& new_config); Status wrapper_status_; - std::shared_ptr sensor_cfg_ptr_{}; + std::shared_ptr sensor_cfg_ptr_{}; /// @brief Stores received packets that have not been processed yet by the decoder thread mt_queue> packet_queue_; @@ -104,7 +80,8 @@ class HesaiRosWrapper final : public rclcpp::Node std::optional decoder_wrapper_; std::mutex mtx_config_; - OnSetParametersCallbackHandle::SharedPtr set_param_res_{}; + + OnSetParametersCallbackHandle::SharedPtr parameter_event_cb_; }; } // namespace ros diff --git a/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp index 30f069c99..cbf78d130 100644 --- a/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp @@ -1,8 +1,10 @@ #pragma once +#include "nebula_ros/common/parameter_descriptors.hpp" + #include -#include +#include #include #include @@ -15,9 +17,9 @@ class HesaiHwInterfaceWrapper { public: HesaiHwInterfaceWrapper(rclcpp::Node* const parent_node, - std::shared_ptr& config); + std::shared_ptr& config); - nebula::Status InitializeHwInterface(); + void OnConfigChange(const std::shared_ptr & new_config); nebula::Status Status(); @@ -27,6 +29,7 @@ class HesaiHwInterfaceWrapper std::shared_ptr hw_interface_; rclcpp::Logger logger_; nebula::Status status_; + bool setup_sensor_; }; } // namespace ros } // namespace nebula \ No newline at end of file diff --git a/nebula_ros/include/nebula_ros/hesai/hw_monitor_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hw_monitor_wrapper.hpp index 5ddfc9e79..5209f64a2 100644 --- a/nebula_ros/include/nebula_ros/hesai/hw_monitor_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hw_monitor_wrapper.hpp @@ -1,7 +1,10 @@ #pragma once +#include "nebula_ros/common/parameter_descriptors.hpp" + #include #include + #include #include #include @@ -22,10 +25,13 @@ class HesaiHwMonitorWrapper public: HesaiHwMonitorWrapper(rclcpp::Node* const parent_node, const std::shared_ptr& hw_interface, - std::shared_ptr& config); + std::shared_ptr& config); - nebula::Status InitializeHwMonitor(); + void OnConfigChange(const std::shared_ptr & /* new_config */) {} + + nebula::Status Status(); +private: void InitializeHesaiDiagnostics(); std::string GetPtreeValue(boost::property_tree::ptree* pt, const std::string& key); @@ -50,9 +56,6 @@ class HesaiHwMonitorWrapper void HesaiCheckVoltage(diagnostic_updater::DiagnosticStatusWrapper& diagnostics); - nebula::Status Status(); - -private: rclcpp::Logger logger_; diagnostic_updater::Updater diagnostics_updater_; nebula::Status status_; diff --git a/nebula_ros/src/common/parameter_descriptors.cpp b/nebula_ros/src/common/parameter_descriptors.cpp new file mode 100644 index 000000000..ffd0fff37 --- /dev/null +++ b/nebula_ros/src/common/parameter_descriptors.cpp @@ -0,0 +1,29 @@ +#include "nebula_ros/common/parameter_descriptors.hpp" + +namespace nebula +{ +namespace ros +{ + +rcl_interfaces::msg::ParameterDescriptor param_read_write() +{ + return rcl_interfaces::msg::ParameterDescriptor{}; +}; + +rcl_interfaces::msg::ParameterDescriptor param_read_only() +{ + return rcl_interfaces::msg::ParameterDescriptor{}.set__read_only(true); +} + +rcl_interfaces::msg::ParameterDescriptor::_floating_point_range_type float_range(double start, double stop, double step) +{ + return { rcl_interfaces::msg::FloatingPointRange().set__from_value(start).set__to_value(stop).set__step(step) }; +} + +rcl_interfaces::msg::ParameterDescriptor::_integer_range_type int_range(int start, int stop, int step) +{ + return { rcl_interfaces::msg::IntegerRange().set__from_value(start).set__to_value(stop).set__step(step) }; +} + +} // namespace ros +} // namespace nebula \ No newline at end of file diff --git a/nebula_ros/src/hesai/decoder_wrapper.cpp b/nebula_ros/src/hesai/decoder_wrapper.cpp index 1ae6d2837..1b7c10582 100644 --- a/nebula_ros/src/hesai/decoder_wrapper.cpp +++ b/nebula_ros/src/hesai/decoder_wrapper.cpp @@ -7,7 +7,7 @@ namespace ros HesaiDecoderWrapper::HesaiDecoderWrapper(rclcpp::Node* const parent_node, const std::shared_ptr& hw_interface, - std::shared_ptr& config) + std::shared_ptr& config) : status_(nebula::Status::NOT_INITIALIZED) , logger_(parent_node->get_logger().get_child("HesaiDecoder")) , hw_interface_(hw_interface) @@ -20,26 +20,14 @@ HesaiDecoderWrapper::HesaiDecoderWrapper(rclcpp::Node* const parent_node, if (config->sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - parent_node->declare_parameter("correction_file", "", descriptor); - calibration_file_path_ = parent_node->get_parameter("correction_file").as_string(); + calibration_file_path_ = parent_node->declare_parameter("correction_file", "", param_read_write()); } else { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - parent_node->declare_parameter("calibration_file", "", descriptor); - calibration_file_path_ = parent_node->get_parameter("calibration_file").as_string(); + calibration_file_path_ = parent_node->declare_parameter("calibration_file", "", param_read_write()); } - auto calibration_result = GetCalibrationData(); + auto calibration_result = GetCalibrationData(calibration_file_path_, false); if (!calibration_result.has_value()) { @@ -78,8 +66,64 @@ HesaiDecoderWrapper::HesaiDecoderWrapper(rclcpp::Node* const parent_node, RCLCPP_INFO_STREAM(logger_, ". Wrapper=" << status_); } -nebula::util::expected, nebula::Status> -HesaiDecoderWrapper::GetCalibrationData() +void HesaiDecoderWrapper::OnConfigChange( + const std::shared_ptr& new_config) +{ + std::lock_guard lock(mtx_driver_ptr_); + auto new_driver = std::make_shared(new_config, calibration_cfg_ptr_); + driver_ptr_ = new_driver; + sensor_cfg_ = new_config; +} + +void HesaiDecoderWrapper::OnCalibrationChange( + const std::shared_ptr& new_calibration) +{ + std::lock_guard lock(mtx_driver_ptr_); + auto new_driver = std::make_shared(sensor_cfg_, new_calibration); + driver_ptr_ = new_driver; + calibration_cfg_ptr_ = new_calibration; + calibration_file_path_ = calibration_cfg_ptr_->calibration_file; +} + +rcl_interfaces::msg::SetParametersResult HesaiDecoderWrapper::OnParameterChange(const std::vector& p) +{ + using namespace rcl_interfaces::msg; + + std::string calibration_path = ""; + + // Only one of the two parameters is defined, so not checking for sensor model explicitly here is fine + bool got_any = get_param(p, "calibration_file", calibration_path) | get_param(p, "correction_file", calibration_path); + if (!got_any) + { + return rcl_interfaces::build().successful(true).reason(""); + } + + if (!std::filesystem::exists(calibration_path)) + { + auto result = SetParametersResult(); + result.successful = false; + result.reason = "The given calibration path does not exist, ignoring: '" + calibration_path + "'"; + return result; + } + + auto get_calibration_result = GetCalibrationData(calibration_path, true); + if (!get_calibration_result.has_value()) + { + auto result = SetParametersResult(); + result.successful = false; + result.reason = (std::stringstream() << "Could not change calibration file to '" << calibration_path + << "': " << get_calibration_result.error()) + .str(); + return result; + } + + OnCalibrationChange(get_calibration_result.value()); + RCLCPP_INFO_STREAM(logger_, "Changed calibration to '" << calibration_cfg_ptr_->calibration_file << "'"); + return rcl_interfaces::build().successful(true).reason(""); +} + +HesaiDecoderWrapper::get_calibration_result_t +HesaiDecoderWrapper::GetCalibrationData(const std::string& calibration_file_path, bool ignore_others) { std::shared_ptr calib; @@ -96,16 +140,15 @@ HesaiDecoderWrapper::GetCalibrationData() std::string calibration_file_path_from_sensor; { - int ext_pos = calibration_file_path_.find_last_of('.'); - calibration_file_path_from_sensor = calibration_file_path_.substr(0, ext_pos); + int ext_pos = calibration_file_path.find_last_of('.'); + calibration_file_path_from_sensor = calibration_file_path.substr(0, ext_pos); // TODO: if multiple different sensors of the same type are used, this will mix up their calibration data calibration_file_path_from_sensor += "_from_sensor"; - calibration_file_path_from_sensor += - calibration_file_path_.substr(ext_pos, calibration_file_path_.size() - ext_pos); + calibration_file_path_from_sensor += calibration_file_path.substr(ext_pos, calibration_file_path.size() - ext_pos); } // If a sensor is connected, try to download and save its calibration data - if (hw_connected) + if (!ignore_others && hw_connected) { try { @@ -128,7 +171,7 @@ HesaiDecoderWrapper::GetCalibrationData() } // If saved calibration data from a sensor exists (either just downloaded above, or previously), try to load it - if (std::filesystem::exists(calibration_file_path_from_sensor)) + if (!ignore_others && std::filesystem::exists(calibration_file_path_from_sensor)) { auto status = calib->LoadFromFile(calibration_file_path_from_sensor); if (status == Status::OK) @@ -139,31 +182,34 @@ HesaiDecoderWrapper::GetCalibrationData() RCLCPP_ERROR_STREAM(logger_, "Could not load downloaded calibration data: " << status); } - else + else if (!ignore_others) { RCLCPP_ERROR(logger_, "No downloaded calibration data found."); } - RCLCPP_WARN(logger_, "Falling back to generic calibration file."); + if (!ignore_others) + { + RCLCPP_WARN(logger_, "Falling back to generic calibration file."); + } // If downloaded data did not exist or could not be loaded, fall back to a generic file. // If that file does not exist either, return an error code - if (!std::filesystem::exists(calibration_file_path_)) + if (!std::filesystem::exists(calibration_file_path)) { RCLCPP_ERROR(logger_, "No calibration data found."); return nebula::Status(Status::INVALID_CALIBRATION_FILE); } // Try to load the existing fallback calibration file. Return an error if this fails - auto status = calib->LoadFromFile(calibration_file_path_); + auto status = calib->LoadFromFile(calibration_file_path); if (status != Status::OK) { - RCLCPP_ERROR(logger_, "Could not load fallback calibration file."); + RCLCPP_ERROR_STREAM(logger_, "Could not load calibration file at '" << calibration_file_path << "'"); return status; } // Return the fallback calibration file - calib->calibration_file = calibration_file_path_; + calib->calibration_file = calibration_file_path; return calib; } @@ -185,9 +231,13 @@ void HesaiDecoderWrapper::ProcessCloudPacket(std::unique_ptrpackets.emplace_back(std::move(pandar_packet_msg)); } - std::tuple pointcloud_ts = - driver_ptr_->ParseCloudPacket(packet_msg->data); - nebula::drivers::NebulaPointCloudPtr pointcloud = std::get<0>(pointcloud_ts); + std::tuple pointcloud_ts{}; + nebula::drivers::NebulaPointCloudPtr pointcloud = nullptr; + { + std::lock_guard lock(mtx_driver_ptr_); + pointcloud_ts = driver_ptr_->ParseCloudPacket(packet_msg->data); + pointcloud = std::get<0>(pointcloud_ts); + } if (pointcloud == nullptr) { @@ -244,6 +294,8 @@ void HesaiDecoderWrapper::PublishCloud(std::unique_ptr("launch_hw", param_read_only()); if (launch_hw_) { @@ -27,10 +35,6 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions& options) decoder_wrapper_.emplace(this, hw_interface_wrapper_ ? hw_interface_wrapper_->HwInterface() : nullptr, sensor_cfg_ptr_); - RCLCPP_INFO_STREAM(get_logger(), "SensorConfig:" << *sensor_cfg_ptr_); - set_param_res_ = - add_on_set_parameters_callback(std::bind(&HesaiRosWrapper::paramCallback, this, std::placeholders::_1)); - RCLCPP_DEBUG(get_logger(), "Starting stream"); decoder_thread_ = std::thread([this]() { @@ -42,263 +46,164 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions& options) if (launch_hw_) { - StreamStart(); hw_interface_wrapper_->HwInterface()->RegisterScanCallback( std::bind(&HesaiRosWrapper::ReceiveCloudPacketCallback, this, std::placeholders::_1)); + StreamStart(); } else { packets_sub_ = create_subscription( "pandar_packets", rclcpp::SensorDataQoS(), std::bind(&HesaiRosWrapper::ReceiveScanMessageCallback, this, std::placeholders::_1)); - RCLCPP_INFO_STREAM(get_logger(), "Hardware connection disabled, listening for packets on " << packets_sub_->get_topic_name()); + RCLCPP_INFO_STREAM(get_logger(), + "Hardware connection disabled, listening for packets on " << packets_sub_->get_topic_name()); } + + // Register parameter callback after all params have been declared. Otherwise it would be called once for each + // declaration + parameter_event_cb_ = + add_on_set_parameters_callback(std::bind(&HesaiRosWrapper::OnParameterChange, this, std::placeholders::_1)); } nebula::Status HesaiRosWrapper::DeclareAndGetSensorConfigParams() { - nebula::drivers::HesaiSensorConfiguration sensor_configuration; + nebula::drivers::HesaiSensorConfiguration config; + + auto _sensor_model = declare_parameter("sensor_model", "", param_read_only()); + config.sensor_model = drivers::SensorModelFromString(_sensor_model); + + auto _return_mode = declare_parameter("return_mode", "", param_read_write()); + config.return_mode = drivers::ReturnModeFromStringHesai(_return_mode, config.sensor_model); + + config.host_ip = declare_parameter("host_ip", "255.255.255.255", param_read_only()); + config.sensor_ip = declare_parameter("sensor_ip", "192.168.1.201", param_read_only()); + config.data_port = declare_parameter("data_port", 2368, param_read_only()); + config.gnss_port = declare_parameter("gnss_port", 2369, param_read_only()); + config.frame_id = declare_parameter("frame_id", "pandar", param_read_write()); { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - declare_parameter("sensor_model", ""); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - declare_parameter("return_mode", "", descriptor); - sensor_configuration.return_mode = nebula::drivers::ReturnModeFromStringHesai( - get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - declare_parameter("host_ip", "255.255.255.255", descriptor); - sensor_configuration.host_ip = 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 = ""; - declare_parameter("sensor_ip", "192.168.1.201", descriptor); - sensor_configuration.sensor_ip = 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 = ""; - declare_parameter("data_port", 2368, descriptor); - sensor_configuration.data_port = 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 = ""; - declare_parameter("gnss_port", 2369, descriptor); - sensor_configuration.gnss_port = get_parameter("gnss_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - declare_parameter("frame_id", "pandar", descriptor); - sensor_configuration.frame_id = 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 }; - declare_parameter("scan_phase", 0., descriptor); - sensor_configuration.scan_phase = get_parameter("scan_phase").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - declare_parameter("min_range", 0.3, descriptor); - sensor_configuration.min_range = get_parameter("min_range").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - declare_parameter("max_range", 300., descriptor); - sensor_configuration.max_range = get_parameter("max_range").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - declare_parameter("packet_mtu_size", 1500, descriptor); - sensor_configuration.packet_mtu_size = get_parameter("packet_mtu_size").as_int(); + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.])"; + descriptor.floating_point_range = float_range(0, 360, 0.01); + config.scan_phase = declare_parameter("scan_phase", 0., descriptor); } + + config.min_range = declare_parameter("min_range", 0.3, param_read_write()); + config.max_range = declare_parameter("max_range", 300., param_read_write()); + config.packet_mtu_size = declare_parameter("packet_mtu_size", 1500, param_read_only()); + { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - rcl_interfaces::msg::IntegerRange range; - RCLCPP_DEBUG_STREAM(get_logger(), sensor_configuration.sensor_model); - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::HESAI_PANDARAT128) + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + uint16_t default_value; + RCLCPP_DEBUG_STREAM(get_logger(), config.sensor_model); + if (config.sensor_model == nebula::drivers::SensorModel::HESAI_PANDARAT128) { - descriptor.additional_constraints = "200, 300, 400, 500"; - // range.set__from_value(200).set__to_value(500).set__step(100); - // descriptor.integer_range = {range}; //todo - declare_parameter("rotation_speed", 200, descriptor); + descriptor.additional_constraints = "200, 400"; + descriptor.integer_range = int_range(200, 400, 200); + default_value = 200; } else { descriptor.additional_constraints = "300, 600, 1200"; - // range.set__from_value(300).set__to_value(1200).set__step(300); - // descriptor.integer_range = {range}; //todo - declare_parameter("rotation_speed", 600, descriptor); + descriptor.integer_range = int_range(300, 1200, 300); + default_value = 600; } - sensor_configuration.rotation_speed = get_parameter("rotation_speed").as_int(); + config.rotation_speed = declare_parameter("rotation_speed", default_value, descriptor); } { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(360).set__step(1); - descriptor.integer_range = { range }; - declare_parameter("cloud_min_angle", 0, descriptor); - sensor_configuration.cloud_min_angle = get_parameter("cloud_min_angle").as_int(); + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.integer_range = int_range(0, 360, 1); + config.cloud_min_angle = declare_parameter("cloud_min_angle", 0, descriptor); } { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(360).set__step(1); - descriptor.integer_range = { range }; - declare_parameter("cloud_max_angle", 360, descriptor); - sensor_configuration.cloud_max_angle = get_parameter("cloud_max_angle").as_int(); + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.integer_range = int_range(0, 360, 1); + config.cloud_max_angle = declare_parameter("cloud_max_angle", 360, descriptor); } { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = false; - descriptor.dynamic_typing = false; + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); 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 }; - declare_parameter("dual_return_distance_threshold", 0.1, descriptor); - sensor_configuration.dual_return_distance_threshold = get_parameter("dual_return_distance_threshold").as_double(); + descriptor.floating_point_range = float_range(0.01, 0.5, 0.01); + config.dual_return_distance_threshold = + declare_parameter("dual_return_distance_threshold", 0.1, descriptor); } + + auto _ptp_profile = declare_parameter("ptp_profile", "", param_read_only()); + config.ptp_profile = drivers::PtpProfileFromString(_ptp_profile); + + auto _ptp_transport = declare_parameter("ptp_transport_type", "", param_read_only()); + config.ptp_transport_type = drivers::PtpTransportTypeFromString(_ptp_transport); + + if (config.ptp_transport_type != drivers::PtpTransportType::L2 && + config.ptp_profile != drivers::PtpProfile::IEEE_1588v2 && + config.ptp_profile != drivers::PtpProfile::UNKNOWN_PROFILE) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - declare_parameter("ptp_profile", ""); - sensor_configuration.ptp_profile = nebula::drivers::PtpProfileFromString(get_parameter("ptp_profile").as_string()); + RCLCPP_WARN_STREAM(get_logger(), "PTP transport was set to '" << _ptp_transport << "' but PTP profile '" + << _ptp_profile + << "' only supports 'L2'. Setting it to 'L2'."); + config.ptp_transport_type = drivers::PtpTransportType::L2; + set_parameter(rclcpp::Parameter("ptp_transport_type", "L2")); } + + auto _ptp_switch = declare_parameter("ptp_switch_type", "", param_read_only()); + config.ptp_switch_type = drivers::PtpSwitchTypeFromString(_ptp_switch); + { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - declare_parameter("ptp_transport_type", ""); - sensor_configuration.ptp_transport_type = - nebula::drivers::PtpTransportTypeFromString(get_parameter("ptp_transport_type").as_string()); - if (static_cast(sensor_configuration.ptp_profile) > 0) - { - sensor_configuration.ptp_transport_type = nebula::drivers::PtpTransportType::L2; - } + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); + descriptor.integer_range = int_range(0, 127, 1); + config.ptp_domain = declare_parameter("ptp_domain", 0, descriptor); } + + auto new_cfg_ptr = std::make_shared(config); + return ValidateAndSetConfig(new_cfg_ptr); +} + +Status HesaiRosWrapper::ValidateAndSetConfig(std::shared_ptr& new_config) +{ + if (new_config->sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - declare_parameter("ptp_switch_type", ""); - sensor_configuration.ptp_switch_type = - nebula::drivers::PtpSwitchTypeFromString(get_parameter("ptp_switch_type").as_string()); + return Status::INVALID_SENSOR_MODEL; } + if (new_config->return_mode == nebula::drivers::ReturnMode::UNKNOWN) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(127).set__step(1); - descriptor.integer_range = { range }; - declare_parameter("ptp_domain", 0, descriptor); - sensor_configuration.ptp_domain = get_parameter("ptp_domain").as_int(); + return Status::INVALID_ECHO_MODE; } - - if (sensor_configuration.ptp_profile == nebula::drivers::PtpProfile::PROFILE_UNKNOWN) + if (new_config->frame_id.empty()) + { + return Status::SENSOR_CONFIG_ERROR; + } + if (new_config->ptp_profile == nebula::drivers::PtpProfile::UNKNOWN_PROFILE) { RCLCPP_ERROR_STREAM(get_logger(), "Invalid PTP Profile Provided. Please use '1588v2', '802.1as' or 'automotive'"); return Status::SENSOR_CONFIG_ERROR; } - if (sensor_configuration.ptp_transport_type == nebula::drivers::PtpTransportType::UNKNOWN_TRANSPORT) + if (new_config->ptp_transport_type == nebula::drivers::PtpTransportType::UNKNOWN_TRANSPORT) { RCLCPP_ERROR_STREAM(get_logger(), "Invalid PTP Transport Provided. Please use 'udp' or 'l2', 'udp' is only available when " "using the '1588v2' PTP Profile"); return Status::SENSOR_CONFIG_ERROR; } - if (sensor_configuration.ptp_switch_type == nebula::drivers::PtpSwitchType::UNKNOWN_SWITCH) + if (new_config->ptp_switch_type == nebula::drivers::PtpSwitchType::UNKNOWN_SWITCH) { RCLCPP_ERROR_STREAM(get_logger(), "Invalid PTP Switch Type Provided. Please use 'tsn' or 'non_tsn'"); return Status::SENSOR_CONFIG_ERROR; } - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) + + if (hw_interface_wrapper_) { - return Status::INVALID_SENSOR_MODEL; + hw_interface_wrapper_->OnConfigChange(new_config); } - if (sensor_configuration.return_mode == nebula::drivers::ReturnMode::UNKNOWN) + if (hw_monitor_wrapper_) { - return Status::INVALID_ECHO_MODE; + hw_monitor_wrapper_->OnConfigChange(new_config); } - if (sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) + if (decoder_wrapper_) { - return Status::SENSOR_CONFIG_ERROR; + decoder_wrapper_->OnConfigChange(new_config); } - auto new_cfg_ptr_ = std::make_shared(sensor_configuration); - sensor_cfg_ptr_.swap(new_cfg_ptr_); + sensor_cfg_ptr_ = new_config; return Status::OK; } @@ -326,25 +231,6 @@ Status HesaiRosWrapper::GetStatus() return wrapper_status_; } -Status HesaiRosWrapper::DeclareAndGetWrapperParams() -{ - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - declare_parameter("launch_hw", "", descriptor); - launch_hw_ = get_parameter("launch_hw").as_bool(); - } - - return Status::OK; -} - -HesaiRosWrapper::~HesaiRosWrapper() -{ -} - Status HesaiRosWrapper::StreamStart() { if (!hw_interface_wrapper_) @@ -360,74 +246,60 @@ Status HesaiRosWrapper::StreamStart() return hw_interface_wrapper_->HwInterface()->SensorInterfaceStart(); } -rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::paramCallback(const std::vector& p) +rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::OnParameterChange(const std::vector& p) { + using namespace rcl_interfaces::msg; + + if (p.empty()) + { + return rcl_interfaces::build().successful(true).reason(""); + } + std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(get_logger(), "add_on_set_parameters_callback"); - RCLCPP_DEBUG_STREAM(get_logger(), p); - RCLCPP_DEBUG_STREAM(get_logger(), *sensor_cfg_ptr_); + + RCLCPP_INFO(get_logger(), "OnParameterChange"); drivers::HesaiSensorConfiguration new_cfg(*sensor_cfg_ptr_); - std::string sensor_model_str; - std::string return_mode_str; - if (get_param(p, "sensor_model", sensor_model_str) | get_param(p, "return_mode", return_mode_str) | - get_param(p, "host_ip", new_cfg.host_ip) | get_param(p, "sensor_ip", new_cfg.sensor_ip) | - get_param(p, "frame_id", new_cfg.frame_id) | get_param(p, "data_port", new_cfg.data_port) | - get_param(p, "gnss_port", new_cfg.gnss_port) | get_param(p, "scan_phase", new_cfg.scan_phase) | - get_param(p, "packet_mtu_size", new_cfg.packet_mtu_size) | - get_param(p, "rotation_speed", new_cfg.rotation_speed) | - get_param(p, "cloud_min_angle", new_cfg.cloud_min_angle) | - get_param(p, "cloud_max_angle", new_cfg.cloud_max_angle) | - get_param(p, "dual_return_distance_threshold", new_cfg.dual_return_distance_threshold)) + std::string _return_mode = ""; + bool got_any = get_param(p, "return_mode", _return_mode) | get_param(p, "frame_id", new_cfg.frame_id) | + get_param(p, "scan_phase", new_cfg.scan_phase) | get_param(p, "min_range", new_cfg.min_range) | + get_param(p, "max_range", new_cfg.max_range) | get_param(p, "rotation_speed", new_cfg.rotation_speed) | + get_param(p, "cloud_min_angle", new_cfg.cloud_min_angle) | + get_param(p, "cloud_max_angle", new_cfg.cloud_max_angle) | + get_param(p, "dual_return_distance_threshold", new_cfg.dual_return_distance_threshold); + + // Currently, HW interface and monitor wrappers have only read-only parameters, so their update logic is not + // implemented + if (decoder_wrapper_) + { + auto result = decoder_wrapper_->OnParameterChange(p); + if (!result.successful) { + return result; + } + } + + if (!got_any) { - if (0 < sensor_model_str.length()) - new_cfg.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_str); - if (0 < return_mode_str.length()) - new_cfg.return_mode = nebula::drivers::ReturnModeFromString(return_mode_str); - - auto new_cfg_ptr = std::make_shared(new_cfg); - sensor_cfg_ptr_.swap(new_cfg_ptr); - RCLCPP_INFO_STREAM(get_logger(), "Update sensor_configuration"); - RCLCPP_DEBUG_STREAM(get_logger(), "hw_interface_.SetSensorConfiguration"); - hw_interface_wrapper_->HwInterface()->SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr_)); - hw_interface_wrapper_->HwInterface()->CheckAndSetConfig(); + return rcl_interfaces::build().successful(true).reason(""); } - auto result = std::make_shared(); - result->successful = true; - result->reason = "success"; + if (_return_mode.length() > 0) + new_cfg.return_mode = nebula::drivers::ReturnModeFromString(_return_mode); - RCLCPP_DEBUG_STREAM(get_logger(), "add_on_set_parameters_callback success"); + auto new_cfg_ptr = std::make_shared(new_cfg); + auto status = ValidateAndSetConfig(new_cfg_ptr); - return *result; -} + if (status != Status::OK) + { + RCLCPP_WARN_STREAM(get_logger(), "OnParameterChange aborted: " << status); + auto result = SetParametersResult(); + result.successful = false; + result.reason = (std::stringstream() << "Invalid configuration: " << status).str(); + return result; + } -std::vector HesaiRosWrapper::updateParameters() -{ - std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(get_logger(), "updateParameters start"); - std::ostringstream os_sensor_model; - os_sensor_model << sensor_cfg_ptr_->sensor_model; - std::ostringstream os_return_mode; - os_return_mode << sensor_cfg_ptr_->return_mode; - RCLCPP_INFO_STREAM(get_logger(), "set_parameters"); - auto results = set_parameters( - { rclcpp::Parameter("sensor_model", os_sensor_model.str()), - rclcpp::Parameter("return_mode", os_return_mode.str()), rclcpp::Parameter("host_ip", sensor_cfg_ptr_->host_ip), - rclcpp::Parameter("sensor_ip", sensor_cfg_ptr_->sensor_ip), - rclcpp::Parameter("frame_id", sensor_cfg_ptr_->frame_id), - rclcpp::Parameter("data_port", sensor_cfg_ptr_->data_port), - rclcpp::Parameter("gnss_port", sensor_cfg_ptr_->gnss_port), - rclcpp::Parameter("scan_phase", sensor_cfg_ptr_->scan_phase), - rclcpp::Parameter("packet_mtu_size", sensor_cfg_ptr_->packet_mtu_size), - rclcpp::Parameter("rotation_speed", sensor_cfg_ptr_->rotation_speed), - rclcpp::Parameter("cloud_min_angle", sensor_cfg_ptr_->cloud_min_angle), - rclcpp::Parameter("cloud_max_angle", sensor_cfg_ptr_->cloud_max_angle), - rclcpp::Parameter("dual_return_distance_threshold", sensor_cfg_ptr_->dual_return_distance_threshold) }); - RCLCPP_DEBUG_STREAM(get_logger(), "updateParameters end"); - return results; + return rcl_interfaces::build().successful(true).reason(""); } void HesaiRosWrapper::ReceiveCloudPacketCallback(std::vector& packet) diff --git a/nebula_ros/src/hesai/hw_interface_wrapper.cpp b/nebula_ros/src/hesai/hw_interface_wrapper.cpp index e38f6b181..c10319c1e 100644 --- a/nebula_ros/src/hesai/hw_interface_wrapper.cpp +++ b/nebula_ros/src/hesai/hw_interface_wrapper.cpp @@ -5,36 +5,20 @@ namespace nebula namespace ros { -HesaiHwInterfaceWrapper::HesaiHwInterfaceWrapper(rclcpp::Node* const parent_node, - std::shared_ptr& config) +HesaiHwInterfaceWrapper::HesaiHwInterfaceWrapper( + rclcpp::Node* const parent_node, std::shared_ptr& config) : hw_interface_(new nebula::drivers::HesaiHwInterface()) , logger_(parent_node->get_logger().get_child("HwInterface")) , status_(Status::NOT_INITIALIZED) { - bool setup_sensor, retry_connect; + setup_sensor_ = parent_node->declare_parameter("setup_sensor", true, param_read_only()); + bool retry_connect = parent_node->declare_parameter("retry_hw", true, param_read_only()); - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - parent_node->declare_parameter("setup_sensor", true, descriptor); - setup_sensor = parent_node->get_parameter("setup_sensor").as_bool(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - parent_node->declare_parameter("retry_hw", true, descriptor); - retry_connect = parent_node->get_parameter("retry_hw").as_bool(); - } - - hw_interface_->SetSensorConfiguration(std::static_pointer_cast(config)); + status_ = hw_interface_->SetSensorConfiguration(std::static_pointer_cast(config)); - status_ = Status::OK; + if (status_ != Status::OK) { + throw std::runtime_error((std::stringstream{} << "Could not initialize HW interface: " << status_).str()); + } hw_interface_->SetLogger(std::make_shared(parent_node->get_logger())); hw_interface_->SetTargetModel(config->sensor_model); @@ -51,7 +35,7 @@ HesaiHwInterfaceWrapper::HesaiHwInterfaceWrapper(rclcpp::Node* const parent_node retry_count++; std::this_thread::sleep_for(std::chrono::milliseconds(8000)); // >5000 - RCLCPP_WARN_STREAM(logger_, "Retry: " << retry_count); + RCLCPP_WARN_STREAM(logger_, status_ << ". Retry #" << retry_count); } if (status_ == Status::OK) @@ -66,10 +50,9 @@ HesaiHwInterfaceWrapper::HesaiHwInterfaceWrapper(rclcpp::Node* const parent_node { RCLCPP_ERROR_STREAM(logger_, "Failed to get model from sensor..."); } - if (setup_sensor) + if (setup_sensor_) { hw_interface_->CheckAndSetConfig(); - // updateParameters(); TODO } } else @@ -80,6 +63,15 @@ HesaiHwInterfaceWrapper::HesaiHwInterfaceWrapper(rclcpp::Node* const parent_node status_ = Status::OK; } +void HesaiHwInterfaceWrapper::OnConfigChange( + const std::shared_ptr& new_config) +{ + hw_interface_->SetSensorConfiguration(std::static_pointer_cast(new_config)); + if (setup_sensor_) { + hw_interface_->CheckAndSetConfig(); + } +} + Status HesaiHwInterfaceWrapper::Status() { return status_; diff --git a/nebula_ros/src/hesai/hw_monitor_wrapper.cpp b/nebula_ros/src/hesai/hw_monitor_wrapper.cpp index 579e32a4e..0a56bec57 100644 --- a/nebula_ros/src/hesai/hw_monitor_wrapper.cpp +++ b/nebula_ros/src/hesai/hw_monitor_wrapper.cpp @@ -6,22 +6,14 @@ namespace ros { HesaiHwMonitorWrapper::HesaiHwMonitorWrapper(rclcpp::Node* const parent_node, const std::shared_ptr& hw_interface, - std::shared_ptr& config) + std::shared_ptr& config) : logger_(parent_node->get_logger().get_child("HwMonitor")) , diagnostics_updater_(parent_node) , status_(Status::OK) , hw_interface_(hw_interface) , parent_node_(parent_node) { - { - 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"; - parent_node->declare_parameter("diag_span", 1000, descriptor); - diag_span_ = parent_node->get_parameter("diag_span").as_int(); - } + diag_span_ = parent_node->declare_parameter("diag_span", 1000, param_read_only()); switch (config->sensor_model) { @@ -66,11 +58,6 @@ HesaiHwMonitorWrapper::HesaiHwMonitorWrapper(rclcpp::Node* const parent_node, RCLCPP_INFO_STREAM(logger_, "Model:" << info_model_); RCLCPP_INFO_STREAM(logger_, "Serial:" << info_serial_); InitializeHesaiDiagnostics(); - - if (Status::OK != status_) - { - throw std::runtime_error((std::stringstream() << "Error getting calibration data: " << status_).str()); - } } void HesaiHwMonitorWrapper::InitializeHesaiDiagnostics() From 7c91ce798b10dfc9e12a5793c46c6eaf675deaab Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 23 Apr 2024 16:58:52 +0900 Subject: [PATCH 41/57] feat(hesai_ros): add watchdog timer for pointcloud output --- .../nebula_ros/common/watchdog_timer.hpp | 52 +++++++++++++++++++ .../nebula_ros/hesai/decoder_wrapper.hpp | 3 ++ nebula_ros/src/hesai/decoder_wrapper.cpp | 10 ++++ 3 files changed, 65 insertions(+) create mode 100644 nebula_ros/include/nebula_ros/common/watchdog_timer.hpp diff --git a/nebula_ros/include/nebula_ros/common/watchdog_timer.hpp b/nebula_ros/include/nebula_ros/common/watchdog_timer.hpp new file mode 100644 index 000000000..ae556e33e --- /dev/null +++ b/nebula_ros/include/nebula_ros/common/watchdog_timer.hpp @@ -0,0 +1,52 @@ +#pragma once + +#include +#include +#include +#include +#include + +namespace nebula +{ +namespace ros +{ + +class WatchdogTimer +{ + using watchdog_cb_t = std::function; + +public: + WatchdogTimer(rclcpp::Node& node, const std::chrono::microseconds& expected_update_interval, const watchdog_cb_t& callback) + : node_(node) + , callback_(callback) + , expected_update_interval_ns_( + std::chrono::duration_cast(expected_update_interval).count()) + { + timer_ = node_.create_wall_timer(expected_update_interval, std::bind(&WatchdogTimer::onTimer, this)); + } + + void update() + { + last_update_ns_ = node_.get_clock()->now().nanoseconds(); + } + +private: + void onTimer() + { + uint64_t now_ns = node_.get_clock()->now().nanoseconds(); + + bool is_late = (now_ns - last_update_ns_) > expected_update_interval_ns_; + + callback_(!is_late); + } + + rclcpp::Node& node_; + rclcpp::TimerBase::SharedPtr timer_; + std::atomic last_update_ns_; + const watchdog_cb_t callback_; + + const uint64_t expected_update_interval_ns_; +}; + +} // namespace ros +} // namespace nebula \ No newline at end of file diff --git a/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp index 3b2af26d4..da5e60ff0 100644 --- a/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp @@ -3,6 +3,7 @@ #include "nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" #include "nebula_ros/common/parameter_descriptors.hpp" +#include "nebula_ros/common/watchdog_timer.hpp" #include #include @@ -80,6 +81,8 @@ class HesaiDecoderWrapper rclcpp::Publisher::SharedPtr nebula_points_pub_{}; rclcpp::Publisher::SharedPtr aw_points_ex_pub_{}; rclcpp::Publisher::SharedPtr aw_points_base_pub_{}; + + std::shared_ptr cloud_watchdog_; }; } // namespace ros } // namespace nebula \ No newline at end of file diff --git a/nebula_ros/src/hesai/decoder_wrapper.cpp b/nebula_ros/src/hesai/decoder_wrapper.cpp index 1b7c10582..0ddfd599e 100644 --- a/nebula_ros/src/hesai/decoder_wrapper.cpp +++ b/nebula_ros/src/hesai/decoder_wrapper.cpp @@ -5,6 +5,8 @@ namespace nebula namespace ros { +using namespace std::chrono_literals; + HesaiDecoderWrapper::HesaiDecoderWrapper(rclcpp::Node* const parent_node, const std::shared_ptr& hw_interface, std::shared_ptr& config) @@ -64,6 +66,12 @@ HesaiDecoderWrapper::HesaiDecoderWrapper(rclcpp::Node* const parent_node, aw_points_ex_pub_ = parent_node->create_publisher("aw_points_ex", pointcloud_qos); RCLCPP_INFO_STREAM(logger_, ". Wrapper=" << status_); + + cloud_watchdog_ = std::make_shared(*parent_node, 100'000us, [this, parent_node](bool ok) { + if (ok) + return; + RCLCPP_WARN_THROTTLE(logger_, *parent_node->get_clock(), 5000, "Missed pointcloud output deadline"); + }); } void HesaiDecoderWrapper::OnConfigChange( @@ -246,6 +254,8 @@ void HesaiDecoderWrapper::ProcessCloudPacket(std::unique_ptrupdate(); + // Publish scan message only if it has been written to if (current_scan_msg_ && !current_scan_msg_->packets.empty()) { From 76fbd3889406b919dcfdbfe7e65afe145ec5e6a6 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 25 Apr 2024 18:29:33 +0900 Subject: [PATCH 42/57] fix(hesai): change to possibly more accurate high_resolution_clock --- nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index 1bce59ffe..6e08c0e3b 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -309,7 +309,7 @@ void HesaiRosWrapper::ReceiveCloudPacketCallback(std::vector& packet) return; } - const auto now = std::chrono::system_clock::now(); + const auto now = std::chrono::high_resolution_clock::now(); const auto timestamp_ns = std::chrono::duration_cast(now.time_since_epoch()).count(); auto msg_ptr = std::make_unique(); From 2e9ca91b704c14ba19b297dfe6856d088cb5ffcf Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 25 Apr 2024 18:34:02 +0900 Subject: [PATCH 43/57] fix(hesai): fix crash on QT128 --- .../hesai_hw_interface.cpp | 47 ++++++++++++------- 1 file changed, 31 insertions(+), 16 deletions(-) diff --git a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp index 4db22ceb8..ac05a5485 100644 --- a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp @@ -261,8 +261,10 @@ HesaiPtpDiagStatus HesaiHwInterface::GetPtpDiagStatus() auto response_or_err = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_STATUS}); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - if (response.size() != sizeof(HesaiPtpDiagStatus)) { - throw std::runtime_error("Unexpected payload size"); + if (response.size() < sizeof(HesaiPtpDiagStatus)) { + throw std::runtime_error("HesaiPtpDiagStatus has unexpected payload size"); + } else if (response.size() > sizeof(HesaiPtpDiagStatus)) { + PrintError("HesaiPtpDiagStatus from Sensor has unknown format. Will parse anyway."); } HesaiPtpDiagStatus hesai_ptp_diag_status; @@ -280,9 +282,12 @@ HesaiPtpDiagPort HesaiHwInterface::GetPtpDiagPort() auto response_or_err = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_PORT_DATA_SET}); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - if (response.size() != sizeof(HesaiPtpDiagPort)) { - throw std::runtime_error("Unexpected payload size"); + if (response.size() < sizeof(HesaiPtpDiagPort)) { + throw std::runtime_error("HesaiPtpDiagPort has unexpected payload size"); + } else if (response.size() > sizeof(HesaiPtpDiagPort)) { + PrintError("HesaiPtpDiagPort from Sensor has unknown format. Will parse anyway."); } + HesaiPtpDiagPort hesai_ptp_diag_port; memcpy(&hesai_ptp_diag_port, response.data(), sizeof(HesaiPtpDiagPort)); @@ -298,8 +303,10 @@ HesaiPtpDiagTime HesaiHwInterface::GetPtpDiagTime() auto response_or_err = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_TIME_STATUS_NP}); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - if (response.size() != sizeof(HesaiPtpDiagTime)) { - throw std::runtime_error("Unexpected payload size"); + if (response.size() < sizeof(HesaiPtpDiagTime)) { + throw std::runtime_error("HesaiPtpDiagTime has unexpected payload size"); + } else if (response.size() > sizeof(HesaiPtpDiagTime)) { + PrintError("HesaiPtpDiagTime from Sensor has unknown format. Will parse anyway."); } HesaiPtpDiagTime hesai_ptp_diag_time; @@ -317,8 +324,10 @@ HesaiPtpDiagGrandmaster HesaiHwInterface::GetPtpDiagGrandmaster() auto response_or_err = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_GRANDMASTER_SETTINGS_NP}); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - if (response.size() != sizeof(HesaiPtpDiagGrandmaster)) { - throw std::runtime_error("Unexpected payload size"); + if (response.size() < sizeof(HesaiPtpDiagGrandmaster)) { + throw std::runtime_error("HesaiPtpDiagGrandmaster has unexpected payload size"); + } else if (response.size() > sizeof(HesaiPtpDiagGrandmaster)) { + PrintError("HesaiPtpDiagGrandmaster from Sensor has unknown format. Will parse anyway."); } HesaiPtpDiagGrandmaster hesai_ptp_diag_grandmaster; @@ -337,7 +346,7 @@ HesaiInventory HesaiHwInterface::GetInventory() auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); if (response.size() < sizeof(HesaiInventory)) { - throw std::runtime_error("Unexpected payload size"); + throw std::runtime_error("HesaiInventory has unexpected payload size"); } else if (response.size() > sizeof(HesaiInventory)) { PrintError("HesaiInventory from Sensor has unknown format. Will parse anyway."); } @@ -353,8 +362,10 @@ HesaiConfig HesaiHwInterface::GetConfig() auto response_or_err = SendReceive(PTC_COMMAND_GET_CONFIG_INFO); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - if (response.size() != sizeof(HesaiConfig)) { - throw std::runtime_error("Unexpected payload size"); + if (response.size() < sizeof(HesaiConfig)) { + throw std::runtime_error("HesaiConfig has unexpected payload size"); + } else if (response.size() > sizeof(HesaiConfig)) { + PrintError("HesaiConfig from Sensor has unknown format. Will parse anyway."); } HesaiConfig hesai_config; @@ -369,8 +380,10 @@ HesaiLidarStatus HesaiHwInterface::GetLidarStatus() auto response_or_err = SendReceive(PTC_COMMAND_GET_LIDAR_STATUS); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - if (response.size() != sizeof(HesaiLidarStatus)) { - throw std::runtime_error("Unexpected payload size"); + if (response.size() < sizeof(HesaiLidarStatus)) { + throw std::runtime_error("HesaiLidarStatus has unexpected payload size"); + } else if (response.size() > sizeof(HesaiLidarStatus)) { + PrintError("HesaiLidarStatus from Sensor has unknown format. Will parse anyway."); } HesaiLidarStatus hesai_status; @@ -589,7 +602,7 @@ HesaiPtpConfig HesaiHwInterface::GetPtpConfig() auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); if (response.size() < sizeof(HesaiPtpConfig)) { - throw std::runtime_error("Unexpected payload size"); + throw std::runtime_error("HesaiPtpConfig has unexpected payload size"); } else if (response.size() > sizeof(HesaiPtpConfig)) { PrintError("HesaiPtpConfig from Sensor has unknown format. Will parse anyway."); } @@ -629,8 +642,10 @@ HesaiLidarMonitor HesaiHwInterface::GetLidarMonitor() auto response_or_err = SendReceive(PTC_COMMAND_LIDAR_MONITOR); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - if (response.size() != sizeof(HesaiLidarMonitor)) { - throw std::runtime_error("Unexpected payload size"); + if (response.size() < sizeof(HesaiLidarMonitor)) { + throw std::runtime_error("HesaiLidarMonitor has unexpected payload size"); + } else if (response.size() > sizeof(HesaiLidarMonitor)) { + PrintError("HesaiLidarMonitor from Sensor has unknown format. Will parse anyway."); } HesaiLidarMonitor hesai_lidar_monitor; From 5c527fa9cd3a1c6c9110372b20de408046242954 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 30 Apr 2024 11:14:00 +0900 Subject: [PATCH 44/57] refactor(hesai_hw_interface): refactor repeated error handling code --- .../hesai_hw_interface.hpp | 6 + .../hesai_hw_interface.cpp | 116 +++++------------- 2 files changed, 35 insertions(+), 87 deletions(-) diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp index d3472afbe..8dc270b8e 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp @@ -168,6 +168,12 @@ class HesaiHwInterface /// @return A string description of all errors in this code std::string PrettyPrintPTCError(ptc_error_t error_code); + /// @brief Checks if the data size matches that of the struct to be parsed, and parses the struct. + /// If data is too small, a std::runtime_error is thrown. If data is too large, a warning is printed + /// and the struct is parsed with the first sizeof(T) bytes. + template + T CheckSizeAndParse(const std::vector & data); + /// @brief Send a PTC request with an optional payload, and return the full response payload. /// Blocking. /// @param command_id PTC command number. diff --git a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp index ac05a5485..a28ffd7d2 100644 --- a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp @@ -260,117 +260,66 @@ HesaiPtpDiagStatus HesaiHwInterface::GetPtpDiagStatus() { auto response_or_err = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_STATUS}); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - - if (response.size() < sizeof(HesaiPtpDiagStatus)) { - throw std::runtime_error("HesaiPtpDiagStatus has unexpected payload size"); - } else if (response.size() > sizeof(HesaiPtpDiagStatus)) { - PrintError("HesaiPtpDiagStatus from Sensor has unknown format. Will parse anyway."); - } - - HesaiPtpDiagStatus hesai_ptp_diag_status; - memcpy(&hesai_ptp_diag_status, response.data(), sizeof(HesaiPtpDiagStatus)); + auto diag_status = CheckSizeAndParse(response); std::stringstream ss; - ss << "HesaiHwInterface::GetPtpDiagStatus: " << hesai_ptp_diag_status; + ss << "HesaiHwInterface::GetPtpDiagStatus: " << diag_status; PrintInfo(ss.str()); - return hesai_ptp_diag_status; + return diag_status; } HesaiPtpDiagPort HesaiHwInterface::GetPtpDiagPort() { auto response_or_err = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_PORT_DATA_SET}); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - - if (response.size() < sizeof(HesaiPtpDiagPort)) { - throw std::runtime_error("HesaiPtpDiagPort has unexpected payload size"); - } else if (response.size() > sizeof(HesaiPtpDiagPort)) { - PrintError("HesaiPtpDiagPort from Sensor has unknown format. Will parse anyway."); - } - - HesaiPtpDiagPort hesai_ptp_diag_port; - memcpy(&hesai_ptp_diag_port, response.data(), sizeof(HesaiPtpDiagPort)); + auto diag_port = CheckSizeAndParse(response); std::stringstream ss; - ss << "HesaiHwInterface::GetPtpDiagPort: " << hesai_ptp_diag_port; + ss << "HesaiHwInterface::GetPtpDiagPort: " << diag_port; PrintInfo(ss.str()); - return hesai_ptp_diag_port; + return diag_port; } HesaiPtpDiagTime HesaiHwInterface::GetPtpDiagTime() { auto response_or_err = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_TIME_STATUS_NP}); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - - if (response.size() < sizeof(HesaiPtpDiagTime)) { - throw std::runtime_error("HesaiPtpDiagTime has unexpected payload size"); - } else if (response.size() > sizeof(HesaiPtpDiagTime)) { - PrintError("HesaiPtpDiagTime from Sensor has unknown format. Will parse anyway."); - } - - HesaiPtpDiagTime hesai_ptp_diag_time; - memcpy(&hesai_ptp_diag_time, response.data(), sizeof(HesaiPtpDiagTime)); + auto diag_time = CheckSizeAndParse(response); std::stringstream ss; - ss << "HesaiHwInterface::GetPtpDiagTime: " << hesai_ptp_diag_time; + ss << "HesaiHwInterface::GetPtpDiagTime: " << diag_time; PrintInfo(ss.str()); - return hesai_ptp_diag_time; + return diag_time; } HesaiPtpDiagGrandmaster HesaiHwInterface::GetPtpDiagGrandmaster() { auto response_or_err = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_GRANDMASTER_SETTINGS_NP}); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - - if (response.size() < sizeof(HesaiPtpDiagGrandmaster)) { - throw std::runtime_error("HesaiPtpDiagGrandmaster has unexpected payload size"); - } else if (response.size() > sizeof(HesaiPtpDiagGrandmaster)) { - PrintError("HesaiPtpDiagGrandmaster from Sensor has unknown format. Will parse anyway."); - } - - HesaiPtpDiagGrandmaster hesai_ptp_diag_grandmaster; - memcpy(&hesai_ptp_diag_grandmaster, response.data(), sizeof(HesaiPtpDiagGrandmaster)); + auto diag_grandmaster = CheckSizeAndParse(response); std::stringstream ss; - ss << "HesaiHwInterface::GetPtpDiagGrandmaster: " << hesai_ptp_diag_grandmaster; + ss << "HesaiHwInterface::GetPtpDiagGrandmaster: " << diag_grandmaster; PrintInfo(ss.str()); - return hesai_ptp_diag_grandmaster; + return diag_grandmaster; } HesaiInventory HesaiHwInterface::GetInventory() { auto response_or_err = SendReceive(PTC_COMMAND_GET_INVENTORY_INFO); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - - if (response.size() < sizeof(HesaiInventory)) { - throw std::runtime_error("HesaiInventory has unexpected payload size"); - } else if (response.size() > sizeof(HesaiInventory)) { - PrintError("HesaiInventory from Sensor has unknown format. Will parse anyway."); - } - - HesaiInventory hesai_inventory; - memcpy(&hesai_inventory, response.data(), sizeof(HesaiInventory)); - - return hesai_inventory; + return CheckSizeAndParse(response); } HesaiConfig HesaiHwInterface::GetConfig() { auto response_or_err = SendReceive(PTC_COMMAND_GET_CONFIG_INFO); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - - if (response.size() < sizeof(HesaiConfig)) { - throw std::runtime_error("HesaiConfig has unexpected payload size"); - } else if (response.size() > sizeof(HesaiConfig)) { - PrintError("HesaiConfig from Sensor has unknown format. Will parse anyway."); - } - - HesaiConfig hesai_config; - memcpy(&hesai_config, response.data(), sizeof(HesaiConfig)); - + auto hesai_config = CheckSizeAndParse(response); std::cout << "Config: " << hesai_config << std::endl; return hesai_config; } @@ -379,17 +328,7 @@ HesaiLidarStatus HesaiHwInterface::GetLidarStatus() { auto response_or_err = SendReceive(PTC_COMMAND_GET_LIDAR_STATUS); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - - if (response.size() < sizeof(HesaiLidarStatus)) { - throw std::runtime_error("HesaiLidarStatus has unexpected payload size"); - } else if (response.size() > sizeof(HesaiLidarStatus)) { - PrintError("HesaiLidarStatus from Sensor has unknown format. Will parse anyway."); - } - - HesaiLidarStatus hesai_status; - memcpy(&hesai_status, response.data(), sizeof(HesaiLidarStatus)); - - return hesai_status; + return CheckSizeAndParse(response); } Status HesaiHwInterface::SetSpinRate(uint16_t rpm) @@ -641,17 +580,7 @@ HesaiLidarMonitor HesaiHwInterface::GetLidarMonitor() { auto response_or_err = SendReceive(PTC_COMMAND_LIDAR_MONITOR); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - - if (response.size() < sizeof(HesaiLidarMonitor)) { - throw std::runtime_error("HesaiLidarMonitor has unexpected payload size"); - } else if (response.size() > sizeof(HesaiLidarMonitor)) { - PrintError("HesaiLidarMonitor from Sensor has unknown format. Will parse anyway."); - } - - HesaiLidarMonitor hesai_lidar_monitor; - memcpy(&hesai_lidar_monitor, response.data(), sizeof(HesaiLidarMonitor)); - - return hesai_lidar_monitor; + return CheckSizeAndParse(response); } void HesaiHwInterface::IOContextRun() @@ -1323,5 +1252,18 @@ std::string HesaiHwInterface::PrettyPrintPTCError(ptc_error_t error_code) { return ss.str(); } +template +T HesaiHwInterface::CheckSizeAndParse(const std::vector & data) { + if (data.size() < sizeof(T)) { + throw std::runtime_error("Attempted to parse too-small payload"); + } else if (data.size() > sizeof(T)) { + PrintError("Sensor returned longer payload than expected. Will parse anyway."); + } + + T parsed; + memcpy(&parsed, data.data(), sizeof(T)); + return parsed; +} + } // namespace drivers } // namespace nebula From b9f446ab788ff9482063c7092b205312f9396ac4 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 30 Apr 2024 13:20:08 +0900 Subject: [PATCH 45/57] fix(hesai_launch_all_hw.xml): set valid RPM when AT128 is selected --- nebula_ros/launch/hesai_launch_all_hw.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/nebula_ros/launch/hesai_launch_all_hw.xml b/nebula_ros/launch/hesai_launch_all_hw.xml index 1bd285766..b0f804441 100644 --- a/nebula_ros/launch/hesai_launch_all_hw.xml +++ b/nebula_ros/launch/hesai_launch_all_hw.xml @@ -33,6 +33,8 @@ + + Date: Tue, 30 Apr 2024 13:20:49 +0900 Subject: [PATCH 46/57] refactor(hesai_decoder): remove redundant arguments for correction/calibration --- .../decoders/angle_corrector.hpp | 12 +--- .../angle_corrector_calibration_based.hpp | 6 +- .../angle_corrector_correction_based.hpp | 27 +++++---- .../decoders/hesai_decoder.hpp | 10 +--- .../nebula_decoders_hesai/hesai_driver.hpp | 22 ++++++-- .../nebula_decoders_hesai/hesai_driver.cpp | 56 +++++++++---------- 6 files changed, 63 insertions(+), 70 deletions(-) diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp index 121913c34..914d2da49 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp @@ -23,19 +23,11 @@ struct CorrectedAngleData /// @brief Handles angle correction for given azimuth/channel combinations, as well as trigonometry /// lookup tables +template class AngleCorrector { -protected: - const std::shared_ptr sensor_calibration_; - const std::shared_ptr sensor_correction_; - public: - AngleCorrector( - const std::shared_ptr & sensor_calibration, - const std::shared_ptr & sensor_correction) - : sensor_calibration_(sensor_calibration), sensor_correction_(sensor_correction) - { - } + using correction_data_t = CorrectionDataT; /// @brief Get the corrected azimuth and elevation for a given block and channel, along with their /// sin/cos values. diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp index bee570afc..d2c078e2d 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp @@ -11,7 +11,7 @@ namespace drivers { template -class AngleCorrectorCalibrationBased : public AngleCorrector +class AngleCorrectorCalibrationBased : public AngleCorrector { private: static constexpr size_t MAX_AZIMUTH_LEN = 360 * AngleUnit; @@ -27,9 +27,7 @@ class AngleCorrectorCalibrationBased : public AngleCorrector public: AngleCorrectorCalibrationBased( - const std::shared_ptr & sensor_calibration, - const std::shared_ptr & sensor_correction) - : AngleCorrector(sensor_calibration, sensor_correction) + const std::shared_ptr & sensor_calibration) { if (sensor_calibration == nullptr) { throw std::runtime_error( diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_correction_based.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_correction_based.hpp index d23dfc1f2..52cbaf315 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_correction_based.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_correction_based.hpp @@ -13,10 +13,11 @@ namespace drivers { template -class AngleCorrectorCorrectionBased : public AngleCorrector +class AngleCorrectorCorrectionBased : public AngleCorrector { private: static constexpr size_t MAX_AZIMUTH_LENGTH = 360 * AngleUnit; + const std::shared_ptr correction_; rclcpp::Logger logger_; std::array cos_{}; @@ -31,26 +32,25 @@ class AngleCorrectorCorrectionBased : public AngleCorrector // * none of the startFrames are defined as > 360 deg (< 0 not possible since they are unsigned) // * the fields are arranged in ascending order (e.g. field 1: 20-140deg, field 2: 140-260deg etc.) // These assumptions hold for AT128E2X. - int field = sensor_correction_->frameNumber - 1; - for (size_t i = 0; i < sensor_correction_->frameNumber; ++i) { - if (azimuth < sensor_correction_->startFrame[i]) return field; + int field = correction_->frameNumber - 1; + for (size_t i = 0; i < correction_->frameNumber; ++i) { + if (azimuth < correction_->startFrame[i]) return field; field = i; } - // This is never reached if sensor_correction_ is correct + // This is never reached if correction_ is correct return field; } public: AngleCorrectorCorrectionBased( - const std::shared_ptr & sensor_calibration, const std::shared_ptr & sensor_correction) - : AngleCorrector(sensor_calibration, sensor_correction), + : correction_(sensor_correction), logger_(rclcpp::get_logger("AngleCorrectorCorrectionBased")) { if (sensor_correction == nullptr) { throw std::runtime_error( - "Cannot instantiate AngleCorrectorCalibrationBased without calibration data"); + "Cannot instantiate AngleCorrectorCorrectionBased without correction data"); } logger_.set_level(rclcpp::Logger::Level::Debug); @@ -64,17 +64,16 @@ class AngleCorrectorCorrectionBased : public AngleCorrector CorrectedAngleData getCorrectedAngleData(uint32_t block_azimuth, uint32_t channel_id) override { - const auto & correction = AngleCorrector::sensor_correction_; int field = findField(block_azimuth); auto elevation = - correction->elevation[channel_id] + - correction->getElevationAdjustV3(channel_id, block_azimuth) * (AngleUnit / 100); + correction_->elevation[channel_id] + + correction_->getElevationAdjustV3(channel_id, block_azimuth) * (AngleUnit / 100); elevation = (MAX_AZIMUTH_LENGTH + elevation) % MAX_AZIMUTH_LENGTH; - auto azimuth = (block_azimuth + MAX_AZIMUTH_LENGTH - correction->startFrame[field]) * 2 - - correction->azimuth[channel_id] + - correction->getAzimuthAdjustV3(channel_id, block_azimuth) * (AngleUnit / 100); + auto azimuth = (block_azimuth + MAX_AZIMUTH_LENGTH - correction_->startFrame[field]) * 2 - + correction_->azimuth[channel_id] + + correction_->getAzimuthAdjustV3(channel_id, block_azimuth) * (AngleUnit / 100); azimuth = (MAX_AZIMUTH_LENGTH + azimuth) % MAX_AZIMUTH_LENGTH; float azimuth_rad = 2.f * azimuth * M_PI / MAX_AZIMUTH_LENGTH; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp index e8a9afc34..ea5dbe727 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp @@ -204,16 +204,12 @@ class HesaiDecoder : public HesaiScanDecoder public: /// @brief Constructor /// @param sensor_configuration SensorConfiguration for this decoder - /// @param calibration_configuration Calibration for this decoder (can be nullptr if - /// correction_configuration is set) - /// @param correction_configuration Correction for this decoder (can be nullptr if - /// calibration_configuration is set) + /// @param correction_data Calibration data for this decoder explicit HesaiDecoder( const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration, - const std::shared_ptr & correction_configuration) + const std::shared_ptr & correction_data) : sensor_configuration_(sensor_configuration), - angle_corrector_(calibration_configuration, correction_configuration), + angle_corrector_(correction_data), logger_(rclcpp::get_logger("HesaiDecoder")) { logger_.set_level(rclcpp::Logger::Level::Debug); diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp index 87798dbd1..099e022c5 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp @@ -16,6 +16,7 @@ #include #include #include +#include namespace nebula { @@ -30,15 +31,22 @@ class HesaiDriver /// @brief Decoder according to the model std::shared_ptr scan_decoder_; + template + std::shared_ptr InitializeDecoder( + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & + calibration_configuration); + public: HesaiDriver() = delete; /// @brief Constructor /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver (either HesaiCalibrationConfiguration - /// for sensors other than AT128 or HesaiCorrection for AT128) + /// @param calibration_configuration CalibrationConfiguration for this driver (either + /// HesaiCalibrationConfiguration for sensors other than AT128 or HesaiCorrection for AT128) explicit HesaiDriver( - const std::shared_ptr& sensor_configuration, - const std::shared_ptr& calibration_configuration = nullptr); + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & + calibration_configuration); /// @brief Get current status of this driver /// @return Current status @@ -47,12 +55,14 @@ class HesaiDriver /// @brief Setting CalibrationConfiguration (not used) /// @param calibration_configuration /// @return Resulting status - Status SetCalibrationConfiguration(const HesaiCalibrationConfigurationBase& calibration_configuration); + Status SetCalibrationConfiguration( + const HesaiCalibrationConfigurationBase & calibration_configuration); /// @brief Convert PandarScan message to point cloud /// @param pandar_scan Message /// @return tuple of Point cloud and timestamp - std::tuple ParseCloudPacket(const std::vector& packet); + std::tuple ParseCloudPacket( + const std::vector & packet); }; } // namespace drivers diff --git a/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp b/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp index beafd8e71..c8aea2b40 100644 --- a/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp +++ b/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp @@ -17,64 +17,62 @@ namespace nebula { namespace drivers { -HesaiDriver::HesaiDriver(const std::shared_ptr& sensor_configuration, - const std::shared_ptr& calibration_configuration) +HesaiDriver::HesaiDriver( + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & calibration_data) { // initialize proper parser from cloud config's model and echo mode driver_status_ = nebula::Status::OK; - std::shared_ptr calibration = nullptr; - std::shared_ptr correction = nullptr; - - if (sensor_configuration->sensor_model == SensorModel::HESAI_PANDARAT128) - { - correction = std::static_pointer_cast(calibration_configuration); - } - else - { - calibration = std::static_pointer_cast(calibration_configuration); - } - - switch (sensor_configuration->sensor_model) - { - case SensorModel::UNKNOWN: - driver_status_ = nebula::Status::INVALID_SENSOR_MODEL; - break; + switch (sensor_configuration->sensor_model) { case SensorModel::HESAI_PANDAR64: - scan_decoder_.reset(new HesaiDecoder(sensor_configuration, calibration, correction)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDAR40P: case SensorModel::HESAI_PANDAR40M: - scan_decoder_.reset(new HesaiDecoder(sensor_configuration, calibration, correction)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDARQT64: - scan_decoder_.reset(new HesaiDecoder(sensor_configuration, calibration, correction)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDARQT128: - scan_decoder_.reset(new HesaiDecoder(sensor_configuration, calibration, correction)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDARXT32: - scan_decoder_.reset(new HesaiDecoder(sensor_configuration, calibration, correction)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDARXT32M: - scan_decoder_.reset(new HesaiDecoder(sensor_configuration, calibration, correction)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDARAT128: - scan_decoder_.reset(new HesaiDecoder(sensor_configuration, calibration, correction)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDAR128_E3X: - scan_decoder_.reset(new HesaiDecoder(sensor_configuration, calibration, correction)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDAR128_E4X: - scan_decoder_.reset(new HesaiDecoder(sensor_configuration, calibration, correction)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; + case SensorModel::UNKNOWN: + driver_status_ = nebula::Status::INVALID_SENSOR_MODEL; + throw std::runtime_error("Invalid sensor model."); default: driver_status_ = nebula::Status::NOT_INITIALIZED; throw std::runtime_error("Driver not Implemented for selected sensor."); - break; } } +template +std::shared_ptr HesaiDriver::InitializeDecoder( + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & + calibration_configuration) +{ + using CalibT = typename SensorT::angle_corrector_t::correction_data_t; + return std::make_shared>( + sensor_configuration, std::dynamic_pointer_cast(calibration_configuration)); +} + std::tuple HesaiDriver::ParseCloudPacket( const std::vector & packet) { From 9fadf8af52d9de6f7136df578a66a3501e2d6fb9 Mon Sep 17 00:00:00 2001 From: Maximilian Schmeller Date: Sat, 11 May 2024 20:18:09 +0900 Subject: [PATCH 47/57] refactor(nebula_ros): move mt_queue to common --- nebula_ros/include/nebula_ros/{hesai => common}/mt_queue.hpp | 0 nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp | 2 +- 2 files changed, 1 insertion(+), 1 deletion(-) rename nebula_ros/include/nebula_ros/{hesai => common}/mt_queue.hpp (100%) diff --git a/nebula_ros/include/nebula_ros/hesai/mt_queue.hpp b/nebula_ros/include/nebula_ros/common/mt_queue.hpp similarity index 100% rename from nebula_ros/include/nebula_ros/hesai/mt_queue.hpp rename to nebula_ros/include/nebula_ros/common/mt_queue.hpp diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp index 42d8bdc36..ed76343c7 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp @@ -9,7 +9,7 @@ #include "nebula_ros/hesai/decoder_wrapper.hpp" #include "nebula_ros/hesai/hw_interface_wrapper.hpp" #include "nebula_ros/hesai/hw_monitor_wrapper.hpp" -#include "nebula_ros/hesai/mt_queue.hpp" +#include "nebula_ros/common/mt_queue.hpp" #include "nebula_ros/common/parameter_descriptors.hpp" #include From bf581c80a0296cc4290f7d174b0e6fb767b92bba Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 22 May 2024 13:46:56 +0900 Subject: [PATCH 48/57] chore(nebula_examples): change output rosbag format to NebulaPackets, clean up code --- .../hesai_ros_offline_extract_bag_pcd.hpp | 17 +- .../hesai_ros_offline_extract_bag_pcd.cpp | 293 ++++++------------ 2 files changed, 105 insertions(+), 205 deletions(-) diff --git a/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp b/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp index 5136cbb12..bbecf43b0 100644 --- a/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp +++ b/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp @@ -15,18 +15,21 @@ #ifndef NEBULA_HesaiRosOfflineExtractBag_H #define NEBULA_HesaiRosOfflineExtractBag_H -#include "nebula_common/hesai/hesai_common.hpp" -#include "nebula_common/nebula_common.hpp" -#include "nebula_common/nebula_status.hpp" -#include "nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp" -#include "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp" +#include +#include +#include +#include +#include +#include #include #include #include -#include "pandar_msgs/msg/pandar_packet.hpp" -#include "pandar_msgs/msg/pandar_scan.hpp" +#include +#include +#include +#include #include #include diff --git a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp index 02cc84c72..51e2d028c 100644 --- a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp +++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp @@ -70,7 +70,6 @@ Status HesaiRosOfflineExtractBag::InitializeDriver( std::shared_ptr sensor_configuration, std::shared_ptr calibration_configuration) { - // driver should be initialized here with proper decoder driver_ptr_ = std::make_shared( std::static_pointer_cast(sensor_configuration), calibration_configuration); @@ -84,141 +83,37 @@ Status HesaiRosOfflineExtractBag::GetParameters( drivers::HesaiCalibrationConfiguration & calibration_configuration, drivers::HesaiCorrection & correction_configuration) { + auto sensor_model_ = this->declare_parameter("sensor_model", "", param_read_only()); + sensor_configuration.sensor_model = + nebula::drivers::SensorModelFromString(sensor_model_); + auto return_mode_ = this->declare_parameter("return_mode", "", param_read_only()); + sensor_configuration.return_mode = + nebula::drivers::ReturnModeFromStringHesai(return_mode_, sensor_configuration.sensor_model); + this->declare_parameter("frame_id", "pandar", param_read_only()); + sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); + { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", ""); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("return_mode", "", descriptor); - sensor_configuration.return_mode = - // nebula::drivers::ReturnModeFromString(this->get_parameter("return_mode").as_string()); - nebula::drivers::ReturnModeFromStringHesai( - this->get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "pandar", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 3; - descriptor.read_only = true; - descriptor.dynamic_typing = false; + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); 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(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("calibration_file", "", descriptor); - calibration_configuration.calibration_file = - this->get_parameter("calibration_file").as_string(); + descriptor.floating_point_range = float_range(0, 360, 0.01); + sensor_configuration.scan_phase = this->declare_parameter("scan_phase", 0., descriptor); + this->get_parameter("scan_phase").as_double(); } + + calibration_configuration.calibration_file =this->declare_parameter("calibration_file", "", param_read_only()); if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("correction_file", "", descriptor); - correction_file_path = this->get_parameter("correction_file").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("bag_path", "", descriptor); - bag_path = this->get_parameter("bag_path").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("storage_id", "sqlite3", descriptor); - storage_id = this->get_parameter("storage_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("out_path", "", descriptor); - out_path = this->get_parameter("out_path").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("format", "cdr", descriptor); - format = this->get_parameter("format").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 2; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("out_num", 3, descriptor); - out_num = this->get_parameter("out_num").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 2; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("skip_num", 3, descriptor); - skip_num = this->get_parameter("skip_num").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 1; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("only_xyz", false, descriptor); - only_xyz = this->get_parameter("only_xyz").as_bool(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("target_topic", "", descriptor); - target_topic = this->get_parameter("target_topic").as_string(); + correction_file_path = this->declare_parameter("correction_file", "", param_read_only()); } + bag_path = this->declare_parameter("bag_path", "", param_read_only()); + storage_id = this->declare_parameter("storage_id", "sqlite3", param_read_only()); + out_path = this->declare_parameter("out_path", "", param_read_only()); + format = this->declare_parameter("format", "cdr", param_read_only()); + out_num = this->declare_parameter("out_num", 3, param_read_only()); + skip_num = this->declare_parameter("skip_num", 3, param_read_only()); + only_xyz = this->declare_parameter("only_xyz", false, param_read_only()); + target_topic = this->declare_parameter("target_topic", "", param_read_only()); + if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { return Status::INVALID_SENSOR_MODEL; } @@ -281,81 +176,83 @@ Status HesaiRosOfflineExtractBag::ReadBag() if (rcpputils::fs::create_directories(o_dir)) { std::cout << "created: " << o_dir << std::endl; } - // return Status::OK; - pcl::PCDWriter writer; + pcl::PCDWriter pcd_writer; - std::unique_ptr writer_; - bool needs_open = true; + std::unique_ptr bag_writer{}; storage_options.uri = bag_path; storage_options.storage_id = storage_id; - converter_options.output_serialization_format = format; // "cdr"; - { - rosbag2_cpp::Reader reader(std::make_unique()); - // reader.open(rosbag_directory.string()); - reader.open(storage_options, converter_options); - int cnt = 0; - int out_cnt = 0; - while (reader.has_next()) { - auto bag_message = reader.read_next(); - - std::cout << "Found topic name " << bag_message->topic_name << std::endl; - - if (bag_message->topic_name == target_topic) { - std::cout << (cnt + 1) << ", " << (out_cnt + 1) << "/" << out_num << std::endl; - pandar_msgs::msg::PandarScan extracted_msg; - rclcpp::Serialization serialization; - rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); - serialization.deserialize_message(&extracted_serialized_msg, &extracted_msg); - - // std::cout<<"Found data in topic " << bag_message->topic_name << ": " << - // extracted_test_msg.data << std::endl; - std::cout << "Found data in topic " << bag_message->topic_name << ": " - << bag_message->time_stamp << std::endl; - - for (auto & pkt : extracted_msg.packets) { - auto pointcloud_ts = driver_ptr_->ParseCloudPacket(std::vector(pkt.data.begin(), std::next(pkt.data.begin(), pkt.size))); - auto pointcloud = std::get<0>(pointcloud_ts); - - if (!pointcloud) { - continue; - } - - auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; - // pcl::io::savePCDFileBinary((o_dir / fn).string(), *pointcloud); - - if (needs_open) { - const rosbag2_storage::StorageOptions storage_options_w( - {(o_dir / std::to_string(bag_message->time_stamp)).string(), "sqlite3"}); - const rosbag2_cpp::ConverterOptions converter_options_w( - {rmw_get_serialization_format(), rmw_get_serialization_format()}); - writer_ = std::make_unique(); - writer_->open(storage_options_w, converter_options_w); - writer_->create_topic( - {bag_message->topic_name, "pandar_msgs/msg/PandarScan", rmw_get_serialization_format(), - ""}); - needs_open = false; - } - writer_->write(bag_message); - cnt++; - if (skip_num < cnt) { - out_cnt++; - if (only_xyz) { - pcl::PointCloud cloud_xyz; - pcl::copyPointCloud(*pointcloud, cloud_xyz); - writer.writeBinary((o_dir / fn).string(), cloud_xyz); - } else { - writer.writeBinary((o_dir / fn).string(), *pointcloud); - } - // writer.writeASCII((o_dir / fn).string(), *pointcloud); - } - if (out_num <= out_cnt) { - break; - } + converter_options.output_serialization_format = format; + + rosbag2_cpp::Reader reader(std::make_unique()); + reader.open(storage_options, converter_options); + int cnt = 0; + int out_cnt = 0; + while (reader.has_next()) { + auto bag_message = reader.read_next(); + + std::cout << "Found topic name " << bag_message->topic_name << std::endl; + + if (bag_message->topic_name != target_topic) { + continue; + } + + std::cout << (cnt + 1) << ", " << (out_cnt + 1) << "/" << out_num << std::endl; + pandar_msgs::msg::PandarScan extracted_msg; + rclcpp::Serialization serialization; + rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); + serialization.deserialize_message(&extracted_serialized_msg, &extracted_msg); + + std::cout << "Found data in topic " << bag_message->topic_name << ": " + << bag_message->time_stamp << std::endl; + + nebula_msgs::msg::NebulaPackets nebula_msg; + nebula_msg.header = extracted_msg.header; + for (auto & pkt : extracted_msg.packets) { + std::vector pkt_data(pkt.data.begin(), std::next(pkt.data.begin(), pkt.size)); + auto pointcloud_ts = driver_ptr_->ParseCloudPacket(pkt_data); + auto pointcloud = std::get<0>(pointcloud_ts); + + nebula_msgs::msg::NebulaPacket nebula_pkt; + nebula_pkt.stamp = pkt.stamp; + nebula_pkt.data.swap(pkt_data); // move storage from `pkt_data` to `data` + nebula_msg.packets.push_back(nebula_pkt); + + if (!pointcloud) { + continue; + } + + auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; + + if (!bag_writer) { + const rosbag2_storage::StorageOptions storage_options_w( + {(o_dir / std::to_string(bag_message->time_stamp)).string(), "sqlite3"}); + const rosbag2_cpp::ConverterOptions converter_options_w( + {rmw_get_serialization_format(), rmw_get_serialization_format()}); + bag_writer = std::make_unique(); + bag_writer->open(storage_options_w, converter_options_w); + bag_writer->create_topic( + {bag_message->topic_name, "nebula_msgs/msg/NebulaPackets", rmw_get_serialization_format(), + ""}); + } + + bag_writer->write(bag_message); + cnt++; + if (skip_num < cnt) { + out_cnt++; + if (only_xyz) { + pcl::PointCloud cloud_xyz; + pcl::copyPointCloud(*pointcloud, cloud_xyz); + pcd_writer.writeBinary((o_dir / fn).string(), cloud_xyz); + } else { + pcd_writer.writeBinary((o_dir / fn).string(), *pointcloud); } } + + if (out_num <= out_cnt) { + break; + } } - // close on scope exit } return Status::OK; } From c32de1e87eb419a40fc8ae89990c1d0818faccc0 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 22 May 2024 13:51:53 +0900 Subject: [PATCH 49/57] chore(hesai/decoder_wrapper): clarify watchdog behavior in decoder --- nebula_ros/src/hesai/decoder_wrapper.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/nebula_ros/src/hesai/decoder_wrapper.cpp b/nebula_ros/src/hesai/decoder_wrapper.cpp index 0ddfd599e..3046dc9c0 100644 --- a/nebula_ros/src/hesai/decoder_wrapper.cpp +++ b/nebula_ros/src/hesai/decoder_wrapper.cpp @@ -247,10 +247,12 @@ void HesaiDecoderWrapper::ProcessCloudPacket(std::unique_ptr(pointcloud_ts); } + // A pointcloud is only emitted when a scan completes (e.g. 3599 packets do not emit, the 3600th emits one) if (pointcloud == nullptr) { - // todo - // RCLCPP_WARN_STREAM(logger_, "Empty cloud parsed."); + // Since this ends the function early, the `cloud_watchdog_` will not be updated. + // Thus, if pointclouds are not emitted for too long (e.g. when decoder settings are wrong or no packets come in), + // the watchdog will log a warning automatically return; }; From 906904f56a52ae08237eaefb7ad52a3d60a49897 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 22 May 2024 13:53:51 +0900 Subject: [PATCH 50/57] chore(hesai/hw_monitor_wrapper): fix typo in diagnostics name --- nebula_ros/src/hesai/hw_monitor_wrapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_ros/src/hesai/hw_monitor_wrapper.cpp b/nebula_ros/src/hesai/hw_monitor_wrapper.cpp index 0a56bec57..b34e181fb 100644 --- a/nebula_ros/src/hesai/hw_monitor_wrapper.cpp +++ b/nebula_ros/src/hesai/hw_monitor_wrapper.cpp @@ -22,7 +22,7 @@ HesaiHwMonitorWrapper::HesaiHwMonitorWrapper(rclcpp::Node* const parent_node, case nebula::drivers::SensorModel::HESAI_PANDARAT128: temperature_names_.emplace_back("Bottom circuit board T1"); temperature_names_.emplace_back("Bottom circuit board T2"); - temperature_names_.emplace_back("Laser emitting board RT_L1(Internal)"); + temperature_names_.emplace_back("Laser emitting board RT_L1 (Internal)"); temperature_names_.emplace_back("Laser emitting board RT_L2"); temperature_names_.emplace_back("Receiving board RT_R"); temperature_names_.emplace_back("Receiving board RT2"); From 539d5afcf07a30305747dfacc978b386cad3db6b Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 22 May 2024 13:55:49 +0900 Subject: [PATCH 51/57] chore(hesai_ros_decoder_test): fix unclear naming in console output --- nebula_tests/hesai/hesai_ros_decoder_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_tests/hesai/hesai_ros_decoder_test.cpp b/nebula_tests/hesai/hesai_ros_decoder_test.cpp index 00f33fa1a..f65a974cc 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test.cpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test.cpp @@ -304,7 +304,7 @@ void HesaiRosDecoderTest::ReadBag( continue; } - std::cerr << "PC size: " << pointcloud->size() << std::endl; + std::cerr << "Pointcloud size: " << pointcloud->size() << std::endl; scan_callback(bag_message->time_stamp, scan_timestamp, pointcloud); } From d8d52c684b7762bc617d50ba5b696925f7417176 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 22 May 2024 14:44:09 +0900 Subject: [PATCH 52/57] chore: run pre-commit and implement suggested fixes (not including copyright yet) --- .cspell.json | 9 +- .../nebula_common/hesai/hesai_common.hpp | 48 ++-- .../include/nebula_common/nebula_common.hpp | 37 ++- .../include/nebula_common/util/expected.hpp | 47 ++-- .../decoders/angle_corrector.hpp | 5 +- .../angle_corrector_calibration_based.hpp | 5 +- .../angle_corrector_correction_based.hpp | 15 +- .../decoders/hesai_decoder.hpp | 14 +- .../decoders/hesai_packet.hpp | 3 +- .../decoders/hesai_scan_decoder.hpp | 1 + .../nebula_decoders_hesai/hesai_driver.hpp | 4 +- .../nebula_decoders_hesai/hesai_driver.cpp | 6 +- .../hesai_ros_offline_extract_bag_pcd.hpp | 7 +- .../hesai_ros_offline_extract_bag_pcd.cpp | 22 +- .../hesai/hesai_ros_offline_extract_pcd.cpp | 10 +- .../hesai_cmd_response.hpp | 38 +-- .../hesai_hw_interface.hpp | 8 +- .../hesai_hw_interface.cpp | 91 ++++--- nebula_messages/nebula_msgs/CMakeLists.txt | 2 +- .../nebula_msgs/msg/NebulaPacket.msg | 2 +- nebula_messages/nebula_msgs/package.xml | 2 +- .../include/nebula_ros/common/mt_queue.hpp | 7 +- .../common/parameter_descriptors.hpp | 23 +- .../nebula_ros/common/watchdog_timer.hpp | 27 +- .../nebula_ros/hesai/decoder_wrapper.hpp | 40 +-- .../nebula_ros/hesai/hesai_ros_wrapper.hpp | 17 +- .../nebula_ros/hesai/hw_interface_wrapper.hpp | 11 +- .../nebula_ros/hesai/hw_monitor_wrapper.hpp | 33 ++- nebula_ros/launch/hesai_launch_all_hw.xml | 6 +- nebula_ros/launch/nebula_launch.py | 47 ++-- nebula_ros/package.xml | 4 +- .../src/common/parameter_descriptors.cpp | 15 +- nebula_ros/src/hesai/decoder_wrapper.cpp | 251 +++++++++-------- nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 199 +++++++------- nebula_ros/src/hesai/hw_interface_wrapper.cpp | 48 ++-- nebula_ros/src/hesai/hw_monitor_wrapper.cpp | 253 ++++++++---------- nebula_tests/hesai/hesai_ros_decoder_test.cpp | 8 +- nebula_tests/hesai/hesai_ros_decoder_test.hpp | 30 ++- .../hesai/hesai_ros_decoder_test_main.cpp | 77 +----- .../velodyne_ros_decoder_test_vls128.cpp | 10 +- ...cc0c413aef9f44cc34d45933658dcd90140bf.json | 1 + scripts/plot_instrumentation.py | 104 +++---- scripts/plot_times.py | 53 ++-- 43 files changed, 816 insertions(+), 824 deletions(-) create mode 100644 node_modules/.cache/prettier/.prettier-caches/71fcc0c413aef9f44cc34d45933658dcd90140bf.json diff --git a/.cspell.json b/.cspell.json index 228d7f45e..30dc5cda2 100644 --- a/.cspell.json +++ b/.cspell.json @@ -13,6 +13,7 @@ "DHAVE", "Difop", "extrinsics", + "fout", "gprmc", "gptp", "Helios", @@ -32,6 +33,8 @@ "PANDARQT", "PANDARXT", "Pdelay", + "Piyush", + "piyushk", "QT", "rclcpp", "schedutil", @@ -46,8 +49,6 @@ "Vdat", "XT", "XTM", - "piyushk", - "Piyush", - "fout" + "yukkysaito" ] -} \ No newline at end of file +} diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index 0c7ae1c1b..8e0600999 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_common.hpp +++ b/nebula_common/include/nebula_common/hesai/hesai_common.hpp @@ -8,7 +8,10 @@ #include #include #include +#include #include +#include +#include namespace nebula { namespace drivers @@ -47,7 +50,8 @@ struct HesaiCalibrationConfigurationBase : public CalibrationConfigurationBase { virtual nebula::Status LoadFromBytes(const std::vector & buf) = 0; virtual nebula::Status LoadFromFile(const std::string & calibration_file) = 0; - virtual nebula::Status SaveToFileFromBytes(const std::string & calibration_file, const std::vector & buf) = 0; + virtual nebula::Status SaveToFileFromBytes( + const std::string & calibration_file, const std::vector & buf) = 0; }; /// @brief struct for Hesai calibration configuration @@ -63,12 +67,13 @@ struct HesaiCalibrationConfiguration : public HesaiCalibrationConfigurationBase return Status::INVALID_CALIBRATION_FILE; } std::ostringstream ss; - ss << ifs.rdbuf(); // reading data + ss << ifs.rdbuf(); // reading data ifs.close(); return LoadFromString(ss.str()); } - nebula::Status LoadFromBytes(const std::vector & buf) { + nebula::Status LoadFromBytes(const std::vector & buf) + { std::string calibration_string = std::string(buf.begin(), buf.end()); return LoadFromString(calibration_string); } @@ -82,14 +87,12 @@ struct HesaiCalibrationConfiguration : public HesaiCalibrationConfigurationBase ss << calibration_content; std::string line; constexpr size_t expected_cols = 3; - while(std::getline(ss, line)) { + while (std::getline(ss, line)) { boost::char_separator sep(","); boost::tokenizer> tok(line, sep); std::vector actual_tokens(tok.begin(), tok.end()); - if (actual_tokens.size() < expected_cols - || actual_tokens.size() > expected_cols - ) { + if (actual_tokens.size() < expected_cols || actual_tokens.size() > expected_cols) { std::cerr << "Ignoring line with unexpected data:" << line << std::endl; continue; } @@ -100,10 +103,9 @@ struct HesaiCalibrationConfiguration : public HesaiCalibrationConfigurationBase float azimuth = std::stof(actual_tokens[2]); elev_angle_map[laser_id - 1] = elevation; azimuth_offset_map[laser_id - 1] = azimuth; - } catch (const std::invalid_argument& ia) { + } catch (const std::invalid_argument & ia) { continue; } - } return Status::OK; } @@ -129,7 +131,9 @@ struct HesaiCalibrationConfiguration : public HesaiCalibrationConfigurationBase return Status::OK; } - nebula::Status SaveToFileFromBytes(const std::string & calibration_file, const std::vector & buf) override { + nebula::Status SaveToFileFromBytes( + const std::string & calibration_file, const std::vector & buf) override + { std::string calibration_string = std::string(buf.begin(), buf.end()); return SaveFileFromString(calibration_file, calibration_string); } @@ -138,7 +142,8 @@ struct HesaiCalibrationConfiguration : public HesaiCalibrationConfigurationBase /// @param calibration_file path /// @param calibration_string calibration string /// @return Resulting status - inline nebula::Status SaveFileFromString(const std::string & calibration_file, const std::string & calibration_string) + inline nebula::Status SaveFileFromString( + const std::string & calibration_file, const std::string & calibration_string) { std::ofstream ofs(calibration_file); if (!ofs) { @@ -176,9 +181,8 @@ struct HesaiCorrection : public HesaiCalibrationConfigurationBase inline nebula::Status LoadFromBytes(const std::vector & buf) override { size_t index; - for (index = 0; index < buf.size()-1; index++) { - if(buf[index]==0xee && buf[index+1]==0xff) - break; + for (index = 0; index < buf.size() - 1; index++) { + if (buf[index] == 0xee && buf[index + 1] == 0xff) break; } delimiter = (buf[index] & 0xff) << 8 | ((buf[index + 1] & 0xff)); versionMajor = buf[index + 2] & 0xff; @@ -285,7 +289,7 @@ struct HesaiCorrection : public HesaiCalibrationConfigurationBase // int cnt = 0; while (!ifs.eof()) { unsigned char c; - ifs.read((char *)&c, sizeof(unsigned char)); + ifs.read(reinterpret_cast(&c), sizeof(unsigned char)); buf.emplace_back(c); } LoadFromBytes(buf); @@ -298,7 +302,8 @@ struct HesaiCorrection : public HesaiCalibrationConfigurationBase /// @param correction_file path /// @param buf correction binary /// @return Resulting status - inline nebula::Status SaveToFileFromBytes(const std::string & correction_file, const std::vector & buf) override + inline nebula::Status SaveToFileFromBytes( + const std::string & correction_file, const std::vector & buf) override { std::cerr << "Saving in:" << correction_file << "\n"; std::ofstream ofs(correction_file, std::ios::trunc | std::ios::binary); @@ -308,21 +313,20 @@ struct HesaiCorrection : public HesaiCalibrationConfigurationBase } std::cerr << "Writing start...." << buf.size() << "\n"; bool sop_received = false; - for (const auto &byte : buf) { + for (const auto & byte : buf) { if (!sop_received) { if (byte == 0xEE) { std::cerr << "SOP received....\n"; sop_received = true; } } - if(sop_received) { + if (sop_received) { ofs << byte; } } std::cerr << "Closing file\n"; ofs.close(); - if(sop_received) - return Status::OK; + if (sop_received) return Status::OK; return Status::INVALID_CALIBRATION_FILE; } @@ -452,8 +456,8 @@ inline int IntFromReturnModeHesai(const ReturnMode return_mode, const SensorMode case SensorModel::HESAI_PANDARQT128: if (return_mode == ReturnMode::LAST) return 0; if (return_mode == ReturnMode::STRONGEST) return 1; - if (return_mode == ReturnMode::DUAL_LAST_STRONGEST - || return_mode == ReturnMode::DUAL) return 2; + if (return_mode == ReturnMode::DUAL_LAST_STRONGEST || return_mode == ReturnMode::DUAL) + return 2; if (return_mode == ReturnMode::FIRST) return 3; if (return_mode == ReturnMode::DUAL_LAST_FIRST) return 4; if (return_mode == ReturnMode::DUAL_FIRST_STRONGEST) return 5; diff --git a/nebula_common/include/nebula_common/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp index cf1eb0841..e43ab49a3 100644 --- a/nebula_common/include/nebula_common/nebula_common.hpp +++ b/nebula_common/include/nebula_common/nebula_common.hpp @@ -2,7 +2,10 @@ #define NEBULA_COMMON_H #include + #include + +#include #include #include #include @@ -342,24 +345,11 @@ enum class datatype { FLOAT64 = 8 }; -enum class PtpProfile { - IEEE_1588v2 = 0, - IEEE_802_1AS, - IEEE_802_1AS_AUTO, - UNKNOWN_PROFILE -}; +enum class PtpProfile { IEEE_1588v2 = 0, IEEE_802_1AS, IEEE_802_1AS_AUTO, UNKNOWN_PROFILE }; -enum class PtpTransportType { - UDP_IP = 0, - L2, - UNKNOWN_TRANSPORT -}; +enum class PtpTransportType { UDP_IP = 0, L2, UNKNOWN_TRANSPORT }; -enum class PtpSwitchType { - NON_TSN = 0, - TSN, - UNKNOWN_SWITCH -}; +enum class PtpSwitchType { NON_TSN = 0, TSN, UNKNOWN_SWITCH }; /// @brief not used? struct PointField @@ -618,8 +608,9 @@ inline PtpProfile PtpProfileFromString(const std::string & ptp_profile) { // Hesai auto tmp_str = ptp_profile; - std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), - [](unsigned char c){ return std::tolower(c); }); + std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), [](unsigned char c) { + return std::tolower(c); + }); if (tmp_str == "1588v2") return PtpProfile::IEEE_1588v2; if (tmp_str == "802.1as") return PtpProfile::IEEE_802_1AS; if (tmp_str == "automotive") return PtpProfile::IEEE_802_1AS_AUTO; @@ -657,8 +648,9 @@ inline PtpTransportType PtpTransportTypeFromString(const std::string & transport { // Hesai auto tmp_str = transport_type; - std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), - [](unsigned char c){ return std::tolower(c); }); + std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), [](unsigned char c) { + return std::tolower(c); + }); if (tmp_str == "udp") return PtpTransportType::UDP_IP; if (tmp_str == "l2") return PtpTransportType::L2; @@ -692,8 +684,9 @@ inline PtpSwitchType PtpSwitchTypeFromString(const std::string & switch_type) { // Hesai auto tmp_str = switch_type; - std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), - [](unsigned char c){ return std::tolower(c); }); + std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), [](unsigned char c) { + return std::tolower(c); + }); if (tmp_str == "tsn") return PtpSwitchType::TSN; if (tmp_str == "non_tsn") return PtpSwitchType::NON_TSN; diff --git a/nebula_common/include/nebula_common/util/expected.hpp b/nebula_common/include/nebula_common/util/expected.hpp index 0f47e9de1..2a59122f5 100644 --- a/nebula_common/include/nebula_common/util/expected.hpp +++ b/nebula_common/include/nebula_common/util/expected.hpp @@ -1,23 +1,22 @@ #pragma once -#include -#include #include +#include +#include namespace nebula { namespace util { -struct bad_expected_access : public std::exception { - bad_expected_access(const std::string & msg) : std::exception(), msg_(msg) {} +struct bad_expected_access : public std::exception +{ + explicit bad_expected_access(const std::string & msg) : std::exception(), msg_(msg) {} - const char* what() const noexcept override { - return msg_.c_str(); - } + const char * what() const noexcept override { return msg_.c_str(); } private: - const std::string msg_; + const std::string msg_; }; /// @brief A poor man's backport of C++23's std::expected. @@ -39,14 +38,16 @@ struct expected /// @brief Retrieve the value, or throw `bad_expected_access` if an error is contained. /// @return The value of type `T` - T value() { + T value() + { if (!has_value()) { throw bad_expected_access("value() called but containing error"); } - return std::get(value_); + return std::get(value_); } - /// @brief Return the contained value, or, if an error is contained, return the given `default_` instead. + /// @brief Return the contained value, or, if an error is contained, return the given `default_` + /// instead. /// @return The contained value, if any, else `default_` T value_or(const T & default_) { @@ -57,34 +58,38 @@ struct expected /// @brief If the instance has a value, return the value, else throw std::runtime_error(error_msg) /// @param error_msg The message to be included in the thrown exception /// @return The value - T value_or_throw(const std::string & error_msg) { + T value_or_throw(const std::string & error_msg) + { if (has_value()) return value(); throw std::runtime_error(error_msg); } /// @brief Retrieve the error, or throw `bad_expected_access` if a value is contained. /// @return The error of type `E` - E error() { + E error() + { if (has_value()) { throw bad_expected_access("error() called but containing value"); } - return std::get(value_); + return std::get(value_); } - /// @brief Return the contained error, or, if a value is contained, return the given `default_` instead. + /// @brief Return the contained error, or, if a value is contained, return the given `default_` + /// instead. /// @return The contained error, if any, else `default_` - E error_or(const E & default_) { + E error_or(const E & default_) + { if (!has_value()) return error(); - return default_; - } + return default_; + } - expected(const T & value) { value_ = value; } + explicit expected(const T & value) { value_ = value; } - expected(const E & error) { value_ = error; } + explicit expected(const E & error) { value_ = error; } private: std::variant value_; }; } // namespace util -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp index 914d2da49..02d7cd535 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp @@ -44,8 +44,9 @@ class AngleCorrector /// @param sync_azimuth The azimuth set in the sensor configuration, for which the /// timestamp is aligned to the full second /// @return true if the current azimuth is in a different scan than the last one, false otherwise - virtual bool hasScanned(uint32_t current_azimuth, uint32_t last_azimuth, uint32_t sync_azimuth) = 0; + virtual bool hasScanned( + uint32_t current_azimuth, uint32_t last_azimuth, uint32_t sync_azimuth) = 0; }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp index d2c078e2d..da9a1b579 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp @@ -4,6 +4,7 @@ #include "nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp" #include +#include namespace nebula { @@ -79,10 +80,10 @@ class AngleCorrectorCalibrationBased : public AngleCorrector - -#define _(x) '"' << #x << "\": " << x << ", " +#include namespace nebula { @@ -30,8 +29,8 @@ class AngleCorrectorCorrectionBased : public AngleCorrector { // Assumes that: // * none of the startFrames are defined as > 360 deg (< 0 not possible since they are unsigned) - // * the fields are arranged in ascending order (e.g. field 1: 20-140deg, field 2: 140-260deg etc.) - // These assumptions hold for AT128E2X. + // * the fields are arranged in ascending order (e.g. field 1: 20-140deg, field 2: 140-260deg + // etc.) These assumptions hold for AT128E2X. int field = correction_->frameNumber - 1; for (size_t i = 0; i < correction_->frameNumber; ++i) { if (azimuth < correction_->startFrame[i]) return field; @@ -43,10 +42,9 @@ class AngleCorrectorCorrectionBased : public AngleCorrector } public: - AngleCorrectorCorrectionBased( + explicit AngleCorrectorCorrectionBased( const std::shared_ptr & sensor_correction) - : correction_(sensor_correction), - logger_(rclcpp::get_logger("AngleCorrectorCorrectionBased")) + : correction_(sensor_correction), logger_(rclcpp::get_logger("AngleCorrectorCorrectionBased")) { if (sensor_correction == nullptr) { throw std::runtime_error( @@ -83,7 +81,8 @@ class AngleCorrectorCorrectionBased : public AngleCorrector cos_[azimuth], sin_[elevation], cos_[elevation]}; } - bool hasScanned(uint32_t current_azimuth, uint32_t last_azimuth, uint32_t /*sync_azimuth*/) override + bool hasScanned( + uint32_t current_azimuth, uint32_t last_azimuth, uint32_t /*sync_azimuth*/) override { // For AT128, the scan is always cut at the beginning of the field: // If we would cut at `sync_azimuth`, the points left of it would be diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp index ea5dbe727..a277137d2 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp @@ -8,6 +8,11 @@ #include "pandar_msgs/msg/pandar_packet.hpp" #include "pandar_msgs/msg/pandar_scan.hpp" +#include +#include +#include +#include + namespace nebula { namespace drivers @@ -34,7 +39,7 @@ class HesaiDecoder : public HesaiScanDecoder /// @brief The last decoded packet typename SensorT::packet_t packet_; /// @brief The last azimuth processed - int last_phase_ = -1; // Dummy value to signal last_phase_ has not been set yet + int last_phase_ = -1; // Dummy value to signal last_phase_ has not been set yet /// @brief The timestamp of the last completed scan in nanoseconds uint64_t output_scan_timestamp_ns_ = 0; /// @brief The timestamp of the scan currently in progress @@ -177,7 +182,7 @@ class HesaiDecoder : public HesaiScanDecoder if (last_phase_ == -1) { return false; } - + return angle_corrector_.hasScanned(current_phase, last_phase_, sync_phase); } @@ -207,7 +212,8 @@ class HesaiDecoder : public HesaiScanDecoder /// @param correction_data Calibration data for this decoder explicit HesaiDecoder( const std::shared_ptr & sensor_configuration, - const std::shared_ptr & correction_data) + const std::shared_ptr & + correction_data) : sensor_configuration_(sensor_configuration), angle_corrector_(correction_data), logger_(rclcpp::get_logger("HesaiDecoder")) @@ -247,7 +253,7 @@ class HesaiDecoder : public HesaiScanDecoder sensor_configuration_->scan_phase * SensorT::packet_t::DEGREE_SUBDIVISIONS); if (scan_completed) { - std::swap(decode_pc_, output_pc_); + std::swap(decode_pc_, output_pc_); decode_pc_->clear(); has_scanned_ = true; output_scan_timestamp_ns_ = decode_scan_timestamp_ns_; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp index 0280f1ff7..aa807e567 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp @@ -221,7 +221,8 @@ uint64_t get_timestamp_ns(const PacketT & packet) return packet.tail.date_time.get_seconds() * 1000000000 + packet.tail.timestamp * 1000; } -/// @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. +/// @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 diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp index 9d06c2702..153435bbe 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp @@ -8,6 +8,7 @@ #include "pandar_msgs/msg/pandar_scan.hpp" #include +#include namespace nebula { diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp index 099e022c5..4c211efcc 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp @@ -14,9 +14,11 @@ #include #include +#include #include #include -#include +#include +#include namespace nebula { diff --git a/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp b/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp index c8aea2b40..b3afa886b 100644 --- a/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp +++ b/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp @@ -79,15 +79,13 @@ std::tuple HesaiDriver::ParseCloudPacket( std::tuple pointcloud; auto logger = rclcpp::get_logger("HesaiDriver"); - if (driver_status_ != nebula::Status::OK) - { + if (driver_status_ != nebula::Status::OK) { RCLCPP_ERROR(logger, "Driver not OK."); return pointcloud; } scan_decoder_->unpack(packet); - if (scan_decoder_->hasScanned()) - { + if (scan_decoder_->hasScanned()) { pointcloud = scan_decoder_->getPointcloud(); } diff --git a/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp b/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp index bbecf43b0..b8bbe345c 100644 --- a/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp +++ b/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp @@ -15,21 +15,20 @@ #ifndef NEBULA_HesaiRosOfflineExtractBag_H #define NEBULA_HesaiRosOfflineExtractBag_H +#include #include #include #include #include #include #include - -#include #include #include -#include -#include #include #include +#include +#include #include #include diff --git a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp index 51e2d028c..649085018 100644 --- a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp +++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp @@ -76,7 +76,10 @@ Status HesaiRosOfflineExtractBag::InitializeDriver( return driver_ptr_->GetStatus(); } -Status HesaiRosOfflineExtractBag::GetStatus() {return wrapper_status_;} +Status HesaiRosOfflineExtractBag::GetStatus() +{ + return wrapper_status_; +} Status HesaiRosOfflineExtractBag::GetParameters( drivers::HesaiSensorConfiguration & sensor_configuration, @@ -84,14 +87,13 @@ Status HesaiRosOfflineExtractBag::GetParameters( drivers::HesaiCorrection & correction_configuration) { auto sensor_model_ = this->declare_parameter("sensor_model", "", param_read_only()); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(sensor_model_); + sensor_configuration.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_); auto return_mode_ = this->declare_parameter("return_mode", "", param_read_only()); sensor_configuration.return_mode = nebula::drivers::ReturnModeFromStringHesai(return_mode_, sensor_configuration.sensor_model); this->declare_parameter("frame_id", "pandar", param_read_only()); sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - + { rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; @@ -100,9 +102,11 @@ Status HesaiRosOfflineExtractBag::GetParameters( this->get_parameter("scan_phase").as_double(); } - calibration_configuration.calibration_file =this->declare_parameter("calibration_file", "", param_read_only()); + calibration_configuration.calibration_file = + this->declare_parameter("calibration_file", "", param_read_only()); if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - correction_file_path = this->declare_parameter("correction_file", "", param_read_only()); + correction_file_path = + this->declare_parameter("correction_file", "", param_read_only()); } bag_path = this->declare_parameter("bag_path", "", param_read_only()); @@ -215,7 +219,7 @@ Status HesaiRosOfflineExtractBag::ReadBag() nebula_msgs::msg::NebulaPacket nebula_pkt; nebula_pkt.stamp = pkt.stamp; - nebula_pkt.data.swap(pkt_data); // move storage from `pkt_data` to `data` + nebula_pkt.data.swap(pkt_data); // move storage from `pkt_data` to `data` nebula_msg.packets.push_back(nebula_pkt); if (!pointcloud) { @@ -233,7 +237,7 @@ Status HesaiRosOfflineExtractBag::ReadBag() bag_writer->open(storage_options_w, converter_options_w); bag_writer->create_topic( {bag_message->topic_name, "nebula_msgs/msg/NebulaPackets", rmw_get_serialization_format(), - ""}); + ""}); } bag_writer->write(bag_message); @@ -248,7 +252,7 @@ Status HesaiRosOfflineExtractBag::ReadBag() pcd_writer.writeBinary((o_dir / fn).string(), *pointcloud); } } - + if (out_num <= out_cnt) { break; } diff --git a/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp b/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp index bd992ee19..8e134f8aa 100644 --- a/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp +++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp @@ -77,7 +77,10 @@ Status HesaiRosOfflineExtractSample::InitializeDriver( return driver_ptr_->GetStatus(); } -Status HesaiRosOfflineExtractSample::GetStatus() {return wrapper_status_;} +Status HesaiRosOfflineExtractSample::GetStatus() +{ + return wrapper_status_; +} Status HesaiRosOfflineExtractSample::GetParameters( drivers::HesaiSensorConfiguration & sensor_configuration, @@ -104,7 +107,7 @@ Status HesaiRosOfflineExtractSample::GetParameters( sensor_configuration.return_mode = // nebula::drivers::ReturnModeFromString(this->get_parameter("return_mode").as_string()); nebula::drivers::ReturnModeFromStringHesai( - this->get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); + this->get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -282,7 +285,8 @@ Status HesaiRosOfflineExtractSample::ReadBag() // driver_ptr_->ConvertScanToPointcloud( // std::make_shared(extracted_msg)); for (auto & pkt : extracted_msg.packets) { - auto pointcloud_ts = driver_ptr_->ParseCloudPacket(std::vector(pkt.data.begin(), std::next(pkt.data.begin(), pkt.size))); + auto pointcloud_ts = driver_ptr_->ParseCloudPacket( + std::vector(pkt.data.begin(), std::next(pkt.data.begin(), pkt.size))); auto pointcloud = std::get<0>(pointcloud_ts); if (!pointcloud) { diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp index a6d78c45e..13ad0ed53 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp @@ -10,7 +10,7 @@ #include #include -using namespace boost::endian; +using namespace boost::endian; // NOLINT(build/namespaces) namespace nebula { @@ -100,7 +100,7 @@ struct HesaiPtpDiagTime os << ", "; os << "gmTimeBaseIndicator: " << arg.gmTimeBaseIndicator; os << ", "; - //FIXME: lastGmPhaseChange is a binary number, displaying it as string is incorrect + // FIXME: lastGmPhaseChange is a binary number, displaying it as string is incorrect os << "lastGmPhaseChange: " << std::string(std::begin(arg.lastGmPhaseChange), std::end(arg.lastGmPhaseChange)); os << ", "; @@ -158,7 +158,9 @@ struct HesaiInventory os << "sn: " << std::string(arg.sn, strnlen(arg.sn, sizeof(arg.sn))); os << ", "; os << "date_of_manufacture: " - << std::string(arg.date_of_manufacture, strnlen(arg.date_of_manufacture, sizeof(arg.date_of_manufacture))); + << std::string( + arg.date_of_manufacture, + strnlen(arg.date_of_manufacture, sizeof(arg.date_of_manufacture))); os << ", "; os << "mac: "; @@ -256,7 +258,7 @@ struct HesaiConfig uint8_t motor_status; uint8_t vlan_flag; big_uint16_buf_t vlan_id; - uint8_t clock_data_fmt; //FIXME: labeled as gps_nmea_sentence in AT128, OT128 datasheets + uint8_t clock_data_fmt; // FIXME: labeled as gps_nmea_sentence in AT128, OT128 datasheets uint8_t noise_filtering; uint8_t reflectivity_mapping; uint8_t reserved[6]; @@ -266,29 +268,17 @@ struct HesaiConfig std::ios initial_format(nullptr); initial_format.copyfmt(os); - os << "ipaddr: " - << +arg.ipaddr[0] << "." - << +arg.ipaddr[1] << "." - << +arg.ipaddr[2] << "." + os << "ipaddr: " << +arg.ipaddr[0] << "." << +arg.ipaddr[1] << "." << +arg.ipaddr[2] << "." << +arg.ipaddr[3]; os << ", "; - os << "mask: " - << +arg.mask[0] << "." - << +arg.mask[1] << "." - << +arg.mask[2] << "." + os << "mask: " << +arg.mask[0] << "." << +arg.mask[1] << "." << +arg.mask[2] << "." << +arg.mask[3]; os << ", "; - os << "gateway: " - << +arg.gateway[0] << "." - << +arg.gateway[1] << "." - << +arg.gateway[2] << "." + os << "gateway: " << +arg.gateway[0] << "." << +arg.gateway[1] << "." << +arg.gateway[2] << "." << +arg.gateway[3]; os << ", "; - os << "dest_ipaddr: " - << +arg.dest_ipaddr[0] << "." - << +arg.dest_ipaddr[1] << "." - << +arg.dest_ipaddr[2] << "." - << +arg.dest_ipaddr[3]; + os << "dest_ipaddr: " << +arg.dest_ipaddr[0] << "." << +arg.dest_ipaddr[1] << "." + << +arg.dest_ipaddr[2] << "." << +arg.dest_ipaddr[3]; os << ", "; os << "dest_LiDAR_udp_port: " << arg.dest_LiDAR_udp_port; os << ", "; @@ -351,7 +341,7 @@ struct HesaiLidarStatus big_uint32_buf_t startup_times; big_uint32_buf_t total_operation_time; uint8_t ptp_clock_status; - uint8_t reserved[5]; // FIXME: 4 bytes labeled as humidity in OT128 datasheet + uint8_t reserved[5]; // FIXME: 4 bytes labeled as humidity in OT128 datasheet friend std::ostream & operator<<(std::ostream & os, nebula::HesaiLidarStatus const & arg) { @@ -463,7 +453,7 @@ struct HesaiPtpConfig int8_t logAnnounceInterval; int8_t logSyncInterval; int8_t logMinDelayReqInterval; - //FIXME: this format is not correct for OT128, or for AT128 on 802.1AS + // FIXME: this format is not correct for OT128, or for AT128 on 802.1AS friend std::ostream & operator<<(std::ostream & os, nebula::HesaiPtpConfig const & arg) { @@ -489,7 +479,7 @@ struct HesaiPtpConfig /// @brief struct of PTC_COMMAND_LIDAR_MONITOR struct HesaiLidarMonitor { - //FIXME: this format is not correct for OT128 + // FIXME: this format is not correct for OT128 big_int32_buf_t input_voltage; big_int32_buf_t input_current; big_int32_buf_t input_power; diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp index 8dc270b8e..10a1ec93f 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp @@ -30,6 +30,7 @@ #include #include +#include #include namespace nebula @@ -169,8 +170,8 @@ class HesaiHwInterface std::string PrettyPrintPTCError(ptc_error_t error_code); /// @brief Checks if the data size matches that of the struct to be parsed, and parses the struct. - /// If data is too small, a std::runtime_error is thrown. If data is too large, a warning is printed - /// and the struct is parsed with the first sizeof(T) bytes. + /// If data is too small, a std::runtime_error is thrown. If data is too large, a warning is + /// printed and the struct is parsed with the first sizeof(T) bytes. template T CheckSizeAndParse(const std::vector & data); @@ -223,8 +224,7 @@ class HesaiHwInterface /// @brief Registering callback for PandarScan /// @param scan_callback Callback function /// @return Resulting status - Status RegisterScanCallback( - std::function &)> scan_callback); + Status RegisterScanCallback(std::function &)> scan_callback); /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION /// @return Resulting status std::string GetLidarCalibrationString(); diff --git a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp index a28ffd7d2..29b260098 100644 --- a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp @@ -43,14 +43,16 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( send_buf.emplace_back(len & 0xff); send_buf.insert(send_buf.end(), payload.begin(), payload.end()); - // These are shared_ptrs so that in case of request timeout, the callback (if ever called) can access valid memory + // These are shared_ptrs so that in case of request timeout, the callback (if ever called) can + // access valid memory auto recv_buf = std::make_shared>(); auto response_complete = std::make_shared(false); auto error_code = std::make_shared(); std::stringstream ss; - ss << "0x" << std::setfill('0') << std::setw(2) << std::hex << static_cast(command_id) << " (" << len << ") "; + ss << "0x" << std::setfill('0') << std::setw(2) << std::hex << static_cast(command_id) + << " (" << len << ") "; std::string log_tag = ss.str(); PrintDebug(log_tag + "Entering lock"); @@ -66,21 +68,26 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( PrintDebug(log_tag + "Sending payload"); tcp_driver_->asyncSendReceiveHeaderPayload( send_buf, - [this, log_tag, command_id, response_complete, error_code](const std::vector & header_bytes) { + [this, log_tag, command_id, response_complete, + error_code](const std::vector & header_bytes) { error_code->ptc_error_code = header_bytes[3]; - size_t payload_len = (header_bytes[4] << 24) | (header_bytes[5] << 16) | (header_bytes[6] << 8) | header_bytes[7]; - PrintDebug(log_tag + "Received header (expecting " + std::to_string(payload_len) + "B payload)"); - // If command_id in the response does not match, we got a response for another command (or rubbish), probably as a - // result of too many simultaneous TCP connections to the sensor (e.g. from GUI, Web UI, another nebula instance, etc.) + size_t payload_len = (header_bytes[4] << 24) | (header_bytes[5] << 16) | + (header_bytes[6] << 8) | header_bytes[7]; + PrintDebug( + log_tag + "Received header (expecting " + std::to_string(payload_len) + "B payload)"); + // If command_id in the response does not match, we got a response for another command (or + // rubbish), probably as a result of too many simultaneous TCP connections to the sensor (e.g. + // from GUI, Web UI, another nebula instance, etc.) if (header_bytes[2] != command_id) { error_code->error_flags |= TCP_ERROR_UNRELATED_RESPONSE; } - if (payload_len == 0) { - *response_complete = true; + if (payload_len == 0) { + *response_complete = true; } }, - [this, log_tag, recv_buf, response_complete, error_code](const std::vector & payload_bytes) { + [this, log_tag, recv_buf, response_complete, + error_code](const std::vector & payload_bytes) { PrintDebug(log_tag + "Received payload"); // Header had payload length 0 (thus, header callback processed request successfully already), @@ -96,7 +103,7 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( }, [this, log_tag, &tm]() { PrintDebug(log_tag + "Unlocking mutex"); - tm.unlock(); + tm.unlock(); PrintDebug(log_tag + "Unlocked mutex"); }); this->IOContextRun(); @@ -177,7 +184,8 @@ Status HesaiHwInterface::SensorInterfaceStop() return Status::ERROR_1; } -Status HesaiHwInterface::GetSensorConfiguration(const SensorConfigurationBase & sensor_configuration) +Status HesaiHwInterface::GetSensorConfiguration( + const SensorConfigurationBase & sensor_configuration) { std::stringstream ss; ss << sensor_configuration; @@ -251,7 +259,8 @@ std::vector HesaiHwInterface::GetLidarCalibrationBytes() std::string HesaiHwInterface::GetLidarCalibrationString() { auto response_or_err = SendReceive(PTC_COMMAND_GET_LIDAR_CALIBRATION); - auto calib_data = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + auto calib_data = + response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); std::string calib_string(calib_data.begin(), calib_data.end()); return calib_string; } @@ -297,7 +306,8 @@ HesaiPtpDiagTime HesaiHwInterface::GetPtpDiagTime() HesaiPtpDiagGrandmaster HesaiHwInterface::GetPtpDiagGrandmaster() { - auto response_or_err = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_GRANDMASTER_SETTINGS_NP}); + auto response_or_err = + SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_GRANDMASTER_SETTINGS_NP}); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); auto diag_grandmaster = CheckSizeAndParse(response); @@ -476,15 +486,15 @@ HesaiLidarRangeAll HesaiHwInterface::GetLidarRange() if (response.size() != 5) { throw std::runtime_error("Unexpected response payload"); } - + memcpy(&hesai_range_all.start, &response[1], 2); memcpy(&hesai_range_all.end, &response[3], 2); break; case 1: // for each channel - //TODO + // TODO(yukkysaito) break; case 2: // multi-section FOV - //TODO + // TODO(yukkysaito) break; } @@ -756,7 +766,7 @@ HesaiStatus HesaiHwInterface::GetLidarMonitorAsyncHttp( HesaiStatus HesaiHwInterface::CheckAndSetConfig( std::shared_ptr sensor_configuration, HesaiConfig hesai_config) { - using namespace std::chrono_literals; + using namespace std::chrono_literals; // NOLINT(build/namespaces) #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << "Start CheckAndSetConfig(HesaiConfig)!!" << std::endl; #endif @@ -788,7 +798,9 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( auto current_rotation_speed = hesai_config.spin_rate; if (sensor_configuration->rotation_speed != current_rotation_speed.value()) { - PrintInfo("current lidar rotation_speed: " + std::to_string(static_cast(current_rotation_speed.value()))); + PrintInfo( + "current lidar rotation_speed: " + + std::to_string(static_cast(current_rotation_speed.value()))); PrintInfo( "current configuration rotation_speed: " + std::to_string(sensor_configuration->rotation_speed)); @@ -806,8 +818,10 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( bool set_flg = false; std::stringstream ss; - ss << static_cast(hesai_config.dest_ipaddr[0]) << "." << static_cast(hesai_config.dest_ipaddr[1]) << "." - << static_cast(hesai_config.dest_ipaddr[2]) << "." << static_cast(hesai_config.dest_ipaddr[3]); + ss << static_cast(hesai_config.dest_ipaddr[0]) << "." + << static_cast(hesai_config.dest_ipaddr[1]) << "." + << static_cast(hesai_config.dest_ipaddr[2]) << "." + << static_cast(hesai_config.dest_ipaddr[3]); auto current_host_addr = ss.str(); if (sensor_configuration->host_ip != current_host_addr) { set_flg = true; @@ -818,7 +832,9 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( auto current_host_dport = hesai_config.dest_LiDAR_udp_port; if (sensor_configuration->data_port != current_host_dport.value()) { set_flg = true; - PrintInfo("current lidar dest_LiDAR_udp_port: " + std::to_string(static_cast(current_host_dport.value()))); + PrintInfo( + "current lidar dest_LiDAR_udp_port: " + + std::to_string(static_cast(current_host_dport.value()))); PrintInfo( "current configuration data_port: " + std::to_string(sensor_configuration->data_port)); } @@ -826,7 +842,9 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( auto current_host_tport = hesai_config.dest_gps_udp_port; if (sensor_configuration->gnss_port != current_host_tport.value()) { set_flg = true; - PrintInfo("current lidar dest_gps_udp_port: " + std::to_string(static_cast(current_host_tport.value()))); + PrintInfo( + "current lidar dest_gps_udp_port: " + + std::to_string(static_cast(current_host_tport.value()))); PrintInfo( "current configuration gnss_port: " + std::to_string(sensor_configuration->gnss_port)); } @@ -930,18 +948,26 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( set_flg = true; } else { auto current_cloud_min_angle = hesai_lidar_range_all.start; - if (static_cast(sensor_configuration->cloud_min_angle * 10) != current_cloud_min_angle.value()) { + if ( + static_cast(sensor_configuration->cloud_min_angle * 10) != + current_cloud_min_angle.value()) { set_flg = true; - PrintInfo("current lidar range.start: " + std::to_string(static_cast(current_cloud_min_angle.value()))); + PrintInfo( + "current lidar range.start: " + + std::to_string(static_cast(current_cloud_min_angle.value()))); PrintInfo( "current configuration cloud_min_angle: " + std::to_string(sensor_configuration->cloud_min_angle)); } auto current_cloud_max_angle = hesai_lidar_range_all.end; - if (static_cast(sensor_configuration->cloud_max_angle * 10) != current_cloud_max_angle.value()) { + if ( + static_cast(sensor_configuration->cloud_max_angle * 10) != + current_cloud_max_angle.value()) { set_flg = true; - PrintInfo("current lidar range.end: " + std::to_string(static_cast(current_cloud_max_angle.value()))); + PrintInfo( + "current lidar range.end: " + + std::to_string(static_cast(current_cloud_max_angle.value()))); PrintInfo( "current configuration cloud_max_angle: " + std::to_string(sensor_configuration->cloud_max_angle)); @@ -1181,7 +1207,8 @@ void HesaiHwInterface::PrintDebug(const std::vector & bytes) PrintDebug(ss.str()); } -std::string HesaiHwInterface::PrettyPrintPTCError(ptc_error_t error_code) { +std::string HesaiHwInterface::PrettyPrintPTCError(ptc_error_t error_code) +{ if (error_code.ok()) { return "No error"; } @@ -1191,10 +1218,11 @@ std::string HesaiHwInterface::PrettyPrintPTCError(ptc_error_t error_code) { std::stringstream ss; if (ptc_error) { - ss << "Sensor error: 0x" << std::setfill('0') << std::setw(2) << std::hex << static_cast(ptc_error) << ' '; + ss << "Sensor error: 0x" << std::setfill('0') << std::setw(2) << std::hex + << static_cast(ptc_error) << ' '; } - switch(ptc_error) { + switch (ptc_error) { case PTC_ERROR_CODE_NO_ERROR: break; case PTC_ERROR_CODE_INVALID_INPUT_PARAM: @@ -1253,7 +1281,8 @@ std::string HesaiHwInterface::PrettyPrintPTCError(ptc_error_t error_code) { } template -T HesaiHwInterface::CheckSizeAndParse(const std::vector & data) { +T HesaiHwInterface::CheckSizeAndParse(const std::vector & data) +{ if (data.size() < sizeof(T)) { throw std::runtime_error("Attempted to parse too-small payload"); } else if (data.size() > sizeof(T)) { diff --git a/nebula_messages/nebula_msgs/CMakeLists.txt b/nebula_messages/nebula_msgs/CMakeLists.txt index e2b798d9d..2ffa068a5 100644 --- a/nebula_messages/nebula_msgs/CMakeLists.txt +++ b/nebula_messages/nebula_msgs/CMakeLists.txt @@ -21,4 +21,4 @@ rosidl_generate_interfaces(${PROJECT_NAME} std_msgs ) -ament_package() \ No newline at end of file +ament_package() diff --git a/nebula_messages/nebula_msgs/msg/NebulaPacket.msg b/nebula_messages/nebula_msgs/msg/NebulaPacket.msg index be5898eb1..f6ba285d8 100644 --- a/nebula_messages/nebula_msgs/msg/NebulaPacket.msg +++ b/nebula_messages/nebula_msgs/msg/NebulaPacket.msg @@ -1,2 +1,2 @@ builtin_interfaces/Time stamp -uint8[] data \ No newline at end of file +uint8[] data diff --git a/nebula_messages/nebula_msgs/package.xml b/nebula_messages/nebula_msgs/package.xml index 3eb05e603..cdd135c37 100644 --- a/nebula_messages/nebula_msgs/package.xml +++ b/nebula_messages/nebula_msgs/package.xml @@ -22,4 +22,4 @@ ament_cmake - \ No newline at end of file + diff --git a/nebula_ros/include/nebula_ros/common/mt_queue.hpp b/nebula_ros/include/nebula_ros/common/mt_queue.hpp index d60b7b4db..aca385ce3 100644 --- a/nebula_ros/include/nebula_ros/common/mt_queue.hpp +++ b/nebula_ros/include/nebula_ros/common/mt_queue.hpp @@ -1,9 +1,10 @@ #pragma once +#include #include #include #include -#include +#include template class mt_queue @@ -15,7 +16,7 @@ class mt_queue size_t capacity_; public: - mt_queue(size_t capacity) : capacity_(capacity) {} + explicit mt_queue(size_t capacity) : capacity_(capacity) {} bool try_push(T && value) { @@ -46,4 +47,4 @@ class mt_queue return return_value; } -}; \ No newline at end of file +}; diff --git a/nebula_ros/include/nebula_ros/common/parameter_descriptors.hpp b/nebula_ros/include/nebula_ros/common/parameter_descriptors.hpp index b54d8ad63..6d4e38504 100644 --- a/nebula_ros/include/nebula_ros/common/parameter_descriptors.hpp +++ b/nebula_ros/include/nebula_ros/common/parameter_descriptors.hpp @@ -1,9 +1,11 @@ #pragma once -#include #include +#include #include +#include +#include namespace nebula { namespace ros @@ -13,10 +15,11 @@ rcl_interfaces::msg::ParameterDescriptor param_read_write(); rcl_interfaces::msg::ParameterDescriptor param_read_only(); -rcl_interfaces::msg::ParameterDescriptor::_floating_point_range_type float_range(double start, double stop, - double step); +rcl_interfaces::msg::ParameterDescriptor::_floating_point_range_type float_range( + double start, double stop, double step); -rcl_interfaces::msg::ParameterDescriptor::_integer_range_type int_range(int start, int stop, int step); +rcl_interfaces::msg::ParameterDescriptor::_integer_range_type int_range( + int start, int stop, int step); /// @brief Get a parameter's value from a list of parameters, if that parameter is in the list. /// @tparam T The parameter's expected value type @@ -25,12 +28,12 @@ rcl_interfaces::msg::ParameterDescriptor::_integer_range_type int_range(int star /// @param value (out) Parameter value. Set if parameter is found, left untouched otherwise. /// @return Whether the target name existed template -bool get_param(const std::vector& p, const std::string& name, T& value) +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()) - { + 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; } @@ -38,4 +41,4 @@ bool get_param(const std::vector& p, const std::string& name, } } // namespace ros -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/common/watchdog_timer.hpp b/nebula_ros/include/nebula_ros/common/watchdog_timer.hpp index ae556e33e..e6320f91e 100644 --- a/nebula_ros/include/nebula_ros/common/watchdog_timer.hpp +++ b/nebula_ros/include/nebula_ros/common/watchdog_timer.hpp @@ -1,9 +1,10 @@ #pragma once #include -#include + #include #include +#include #include namespace nebula @@ -16,19 +17,19 @@ class WatchdogTimer using watchdog_cb_t = std::function; public: - WatchdogTimer(rclcpp::Node& node, const std::chrono::microseconds& expected_update_interval, const watchdog_cb_t& callback) - : node_(node) - , callback_(callback) - , expected_update_interval_ns_( - std::chrono::duration_cast(expected_update_interval).count()) + WatchdogTimer( + rclcpp::Node & node, const std::chrono::microseconds & expected_update_interval, + const watchdog_cb_t & callback) + : node_(node), + callback_(callback), + expected_update_interval_ns_( + std::chrono::duration_cast(expected_update_interval).count()) { - timer_ = node_.create_wall_timer(expected_update_interval, std::bind(&WatchdogTimer::onTimer, this)); + timer_ = + node_.create_wall_timer(expected_update_interval, std::bind(&WatchdogTimer::onTimer, this)); } - void update() - { - last_update_ns_ = node_.get_clock()->now().nanoseconds(); - } + void update() { last_update_ns_ = node_.get_clock()->now().nanoseconds(); } private: void onTimer() @@ -40,7 +41,7 @@ class WatchdogTimer callback_(!is_late); } - rclcpp::Node& node_; + rclcpp::Node & node_; rclcpp::TimerBase::SharedPtr timer_; std::atomic last_update_ns_; const watchdog_cb_t callback_; @@ -49,4 +50,4 @@ class WatchdogTimer }; } // namespace ros -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp index da5e60ff0..b2bdf856c 100644 --- a/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp @@ -16,6 +16,8 @@ #include #include #include +#include +#include namespace nebula { @@ -23,22 +25,26 @@ namespace ros { class HesaiDecoderWrapper { - using get_calibration_result_t = - nebula::util::expected, nebula::Status>; + using get_calibration_result_t = nebula::util::expected< + std::shared_ptr, nebula::Status>; public: - HesaiDecoderWrapper(rclcpp::Node* const parent_node, - const std::shared_ptr& hw_interface, - std::shared_ptr& config); + HesaiDecoderWrapper( + rclcpp::Node * const parent_node, + const std::shared_ptr & hw_interface, + std::shared_ptr & config); void ProcessCloudPacket(std::unique_ptr packet_msg); - void OnConfigChange(const std::shared_ptr& new_config); + void OnConfigChange( + const std::shared_ptr & new_config); - void - OnCalibrationChange(const std::shared_ptr& new_calibration); + void OnCalibrationChange( + const std::shared_ptr & + new_calibration); - rcl_interfaces::msg::SetParametersResult OnParameterChange(const std::vector& p); + rcl_interfaces::msg::SetParametersResult OnParameterChange( + const std::vector & p); nebula::Status Status(); @@ -48,19 +54,23 @@ class HesaiDecoderWrapper /// 2. If downloaded file available, load that file /// 3. Load the file given by `calibration_file_path` /// @param calibration_file_path The file to use if no better option is available - /// @param ignore_others If true, skip straight so step 3 above, ignoring better calibration options + /// @param ignore_others If true, skip straight so step 3 above, ignoring better calibration + /// options /// @return The calibration data if successful, or an error code if not - get_calibration_result_t GetCalibrationData(const std::string& calibration_file_path, bool ignore_others = false); + get_calibration_result_t GetCalibrationData( + const std::string & calibration_file_path, bool ignore_others = false); - void PublishCloud(std::unique_ptr pointcloud, - const rclcpp::Publisher::SharedPtr& publisher); + void PublishCloud( + std::unique_ptr pointcloud, + const rclcpp::Publisher::SharedPtr & publisher); /// @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)); + return std::chrono::duration_cast( + std::chrono::duration(seconds)); } nebula::Status status_; @@ -85,4 +95,4 @@ class HesaiDecoderWrapper std::shared_ptr cloud_watchdog_; }; } // namespace ros -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp index ed76343c7..0a60d3e8e 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp @@ -1,16 +1,15 @@ #pragma once #include "boost_tcp_driver/tcp_driver.hpp" - #include "nebula_common/hesai/hesai_common.hpp" #include "nebula_common/nebula_common.hpp" #include "nebula_common/nebula_status.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" +#include "nebula_ros/common/mt_queue.hpp" +#include "nebula_ros/common/parameter_descriptors.hpp" #include "nebula_ros/hesai/decoder_wrapper.hpp" #include "nebula_ros/hesai/hw_interface_wrapper.hpp" #include "nebula_ros/hesai/hw_monitor_wrapper.hpp" -#include "nebula_ros/common/mt_queue.hpp" -#include "nebula_ros/common/parameter_descriptors.hpp" #include #include @@ -24,9 +23,11 @@ #include #include +#include #include #include #include +#include namespace nebula { @@ -37,7 +38,7 @@ namespace ros class HesaiRosWrapper final : public rclcpp::Node { public: - explicit HesaiRosWrapper(const rclcpp::NodeOptions& options); + explicit HesaiRosWrapper(const rclcpp::NodeOptions & options); ~HesaiRosWrapper() noexcept {}; /// @brief Get current status of this driver @@ -49,7 +50,7 @@ class HesaiRosWrapper final : public rclcpp::Node Status StreamStart(); private: - void ReceiveCloudPacketCallback(std::vector& packet); + void ReceiveCloudPacketCallback(std::vector & packet); void ReceiveScanMessageCallback(std::unique_ptr scan_msg); @@ -58,9 +59,11 @@ class HesaiRosWrapper final : public rclcpp::Node /// @brief rclcpp parameter callback /// @param parameters Received parameters /// @return SetParametersResult - rcl_interfaces::msg::SetParametersResult OnParameterChange(const std::vector& p); + rcl_interfaces::msg::SetParametersResult OnParameterChange( + const std::vector & p); - Status ValidateAndSetConfig(std::shared_ptr& new_config); + Status ValidateAndSetConfig( + std::shared_ptr & new_config); Status wrapper_status_; diff --git a/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp index cbf78d130..0791af5e2 100644 --- a/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp @@ -3,7 +3,6 @@ #include "nebula_ros/common/parameter_descriptors.hpp" #include - #include #include @@ -16,10 +15,12 @@ namespace ros class HesaiHwInterfaceWrapper { public: - HesaiHwInterfaceWrapper(rclcpp::Node* const parent_node, - std::shared_ptr& config); + HesaiHwInterfaceWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr & config); - void OnConfigChange(const std::shared_ptr & new_config); + void OnConfigChange( + const std::shared_ptr & new_config); nebula::Status Status(); @@ -32,4 +33,4 @@ class HesaiHwInterfaceWrapper bool setup_sensor_; }; } // namespace ros -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/hesai/hw_monitor_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hw_monitor_wrapper.hpp index 5209f64a2..0b270623f 100644 --- a/nebula_ros/include/nebula_ros/hesai/hw_monitor_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hw_monitor_wrapper.hpp @@ -4,7 +4,6 @@ #include #include - #include #include #include @@ -15,6 +14,8 @@ #include #include +#include +#include namespace nebula { @@ -23,18 +24,22 @@ namespace ros class HesaiHwMonitorWrapper { public: - HesaiHwMonitorWrapper(rclcpp::Node* const parent_node, - const std::shared_ptr& hw_interface, - std::shared_ptr& config); + HesaiHwMonitorWrapper( + rclcpp::Node * const parent_node, + const std::shared_ptr & hw_interface, + std::shared_ptr & config); - void OnConfigChange(const std::shared_ptr & /* new_config */) {} + void OnConfigChange( + const std::shared_ptr & /* new_config */) + { + } nebula::Status Status(); private: void InitializeHesaiDiagnostics(); - std::string GetPtreeValue(boost::property_tree::ptree* pt, const std::string& key); + std::string GetPtreeValue(boost::property_tree::ptree * pt, const std::string & key); std::string GetFixedPrecisionString(double val, int pre); @@ -44,24 +49,24 @@ class HesaiHwMonitorWrapper void OnHesaiLidarMonitorTimer(); - void HesaiCheckStatus(diagnostic_updater::DiagnosticStatusWrapper& diagnostics); + void HesaiCheckStatus(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void HesaiCheckPtp(diagnostic_updater::DiagnosticStatusWrapper& diagnostics); + void HesaiCheckPtp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void HesaiCheckTemperature(diagnostic_updater::DiagnosticStatusWrapper& diagnostics); + void HesaiCheckTemperature(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void HesaiCheckRpm(diagnostic_updater::DiagnosticStatusWrapper& diagnostics); + void HesaiCheckRpm(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void HesaiCheckVoltageHttp(diagnostic_updater::DiagnosticStatusWrapper& diagnostics); + void HesaiCheckVoltageHttp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void HesaiCheckVoltage(diagnostic_updater::DiagnosticStatusWrapper& diagnostics); + void HesaiCheckVoltage(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); rclcpp::Logger logger_; diagnostic_updater::Updater diagnostics_updater_; nebula::Status status_; const std::shared_ptr hw_interface_; - rclcpp::Node* const parent_node_; + rclcpp::Node * const parent_node_; uint16_t diag_span_; rclcpp::TimerBase::SharedPtr diagnostics_update_timer_{}; @@ -95,4 +100,4 @@ class HesaiHwMonitorWrapper const std::string MSG_SEP = ": "; }; } // namespace ros -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_ros/launch/hesai_launch_all_hw.xml b/nebula_ros/launch/hesai_launch_all_hw.xml index b0f804441..10c7aa218 100644 --- a/nebula_ros/launch/hesai_launch_all_hw.xml +++ b/nebula_ros/launch/hesai_launch_all_hw.xml @@ -35,7 +35,7 @@ - @@ -47,7 +47,7 @@ - + @@ -62,7 +62,7 @@ - + diff --git a/nebula_ros/launch/nebula_launch.py b/nebula_ros/launch/nebula_launch.py index 74d3eb475..6eee53443 100644 --- a/nebula_ros/launch/nebula_launch.py +++ b/nebula_ros/launch/nebula_launch.py @@ -26,6 +26,7 @@ def get_sensor_make(sensor_name): return "Continental", None return "unrecognized_sensor_model", None + def get_plugin_name(sensor_make, sensor_model): if sensor_make.lower() != "continental": return sensor_make @@ -34,9 +35,11 @@ def get_plugin_name(sensor_make, sensor_model): else: return "invalid_plugin" + def is_hw_monitor_available(sensor_make): return sensor_make.lower() != "continental" + def launch_setup(context, *args, **kwargs): # Model and make sensor_model = LaunchConfiguration("sensor_model").perform(context) @@ -44,7 +47,9 @@ def launch_setup(context, *args, **kwargs): sensor_make, sensor_extension = get_sensor_make(sensor_model) if sensor_make.lower() == "hesai": - raise ValueError("\n `nebula_launch.py` is deprecated. For Hesai sensors, use `hesai_launch_all_hw.xml` instead.") + raise ValueError( + "\n `nebula_launch.py` is deprecated. For Hesai sensors, use `hesai_launch_all_hw.xml` instead." + ) nebula_decoders_share_dir = get_package_share_directory("nebula_decoders") nebula_ros_share_dir = get_package_share_directory("nebula_ros") @@ -53,23 +58,36 @@ def launch_setup(context, *args, **kwargs): sensor_params_fp = LaunchConfiguration("config_file").perform(context) 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_params_fp = os.path.join( + nebula_ros_share_dir, "config", sensor_make.lower(), sensor_model + ".yaml" + ) 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_params_fp + ), "Sensor params yaml file under config/ was not found: {}".format(sensor_params_fp) sensor_calib_fp = sensor_corr_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) + 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) if sensor_model.lower() == "pandarat128": sensor_corr_fp = os.path.splitext(sensor_calib_fp)[0] + ".dat" - assert os.path.exists(sensor_corr_fp), "Sensor corr file under calibration/ was not found: {}".format(sensor_corr_fp) + assert os.path.exists( + sensor_corr_fp + ), "Sensor corr file under calibration/ was not found: {}".format(sensor_corr_fp) with open(sensor_params_fp, "r") as f: - sensor_params = yaml.safe_load(f)["/**"]["ros__parameters"] + sensor_params = yaml.safe_load(f)["/**"]["ros__parameters"] nodes = [] launch_hw = LaunchConfiguration("launch_hw").perform(context) == "true" @@ -78,8 +96,8 @@ def launch_setup(context, *args, **kwargs): # HwInterface ComposableNode( package="nebula_ros", - plugin=get_plugin_name(sensor_make, sensor_model)+"HwInterfaceRosWrapper", - name=sensor_make.lower()+"_hw_interface_ros_wrapper_node", + plugin=get_plugin_name(sensor_make, sensor_model) + "HwInterfaceRosWrapper", + name=sensor_make.lower() + "_hw_interface_ros_wrapper_node", parameters=[ sensor_params, { @@ -97,14 +115,13 @@ def launch_setup(context, *args, **kwargs): ), ) - if launch_hw and is_hw_monitor_available(sensor_make): nodes.append( # HwMonitor ComposableNode( package="nebula_ros", - plugin=sensor_make+"HwMonitorRosWrapper", - name=sensor_make.lower()+"_hw_monitor_ros_wrapper_node", + plugin=sensor_make + "HwMonitorRosWrapper", + name=sensor_make.lower() + "_hw_monitor_ros_wrapper_node", parameters=[ sensor_params, { @@ -119,8 +136,8 @@ def launch_setup(context, *args, **kwargs): nodes.append( ComposableNode( package="nebula_ros", - plugin=get_plugin_name(sensor_make, sensor_model)+"DriverRosWrapper", - name=sensor_make.lower()+"_driver_ros_wrapper_node", + plugin=get_plugin_name(sensor_make, sensor_model) + "DriverRosWrapper", + name=sensor_make.lower() + "_driver_ros_wrapper_node", parameters=[ sensor_params, { @@ -146,7 +163,7 @@ def launch_setup(context, *args, **kwargs): container_kwargs = {} if LaunchConfiguration("debug_logging").perform(context) == "true": - container_kwargs["ros_arguments"] = ['--log-level', 'debug'] + container_kwargs["ros_arguments"] = ["--log-level", "debug"] container = ComposableNodeContainer( name="nebula_ros_node", diff --git a/nebula_ros/package.xml b/nebula_ros/package.xml index e918afdc7..a0bdec51f 100644 --- a/nebula_ros/package.xml +++ b/nebula_ros/package.xml @@ -21,6 +21,8 @@ nebula_common nebula_decoders nebula_hw_interfaces + nebula_msgs + pandar_msgs pcl_conversions rclcpp rclcpp_components @@ -31,8 +33,6 @@ velodyne_msgs visualization_msgs yaml-cpp - nebula_msgs - pandar_msgs ament_cmake_gtest ament_lint_auto diff --git a/nebula_ros/src/common/parameter_descriptors.cpp b/nebula_ros/src/common/parameter_descriptors.cpp index ffd0fff37..b62cb8a9e 100644 --- a/nebula_ros/src/common/parameter_descriptors.cpp +++ b/nebula_ros/src/common/parameter_descriptors.cpp @@ -15,15 +15,20 @@ rcl_interfaces::msg::ParameterDescriptor param_read_only() return rcl_interfaces::msg::ParameterDescriptor{}.set__read_only(true); } -rcl_interfaces::msg::ParameterDescriptor::_floating_point_range_type float_range(double start, double stop, double step) +rcl_interfaces::msg::ParameterDescriptor::_floating_point_range_type float_range( + double start, double stop, double step) { - return { rcl_interfaces::msg::FloatingPointRange().set__from_value(start).set__to_value(stop).set__step(step) }; + return { + rcl_interfaces::msg::FloatingPointRange().set__from_value(start).set__to_value(stop).set__step( + step)}; } -rcl_interfaces::msg::ParameterDescriptor::_integer_range_type int_range(int start, int stop, int step) +rcl_interfaces::msg::ParameterDescriptor::_integer_range_type int_range( + int start, int stop, int step) { - return { rcl_interfaces::msg::IntegerRange().set__from_value(start).set__to_value(stop).set__step(step) }; + return { + rcl_interfaces::msg::IntegerRange().set__from_value(start).set__to_value(stop).set__step(step)}; } } // namespace ros -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_ros/src/hesai/decoder_wrapper.cpp b/nebula_ros/src/hesai/decoder_wrapper.cpp index 3046dc9c0..f51378e47 100644 --- a/nebula_ros/src/hesai/decoder_wrapper.cpp +++ b/nebula_ros/src/hesai/decoder_wrapper.cpp @@ -5,77 +5,80 @@ namespace nebula namespace ros { -using namespace std::chrono_literals; - -HesaiDecoderWrapper::HesaiDecoderWrapper(rclcpp::Node* const parent_node, - const std::shared_ptr& hw_interface, - std::shared_ptr& config) - : status_(nebula::Status::NOT_INITIALIZED) - , logger_(parent_node->get_logger().get_child("HesaiDecoder")) - , hw_interface_(hw_interface) - , sensor_cfg_(config) +using namespace std::chrono_literals; // NOLINT(build/namespaces) + +HesaiDecoderWrapper::HesaiDecoderWrapper( + rclcpp::Node * const parent_node, + const std::shared_ptr & hw_interface, + std::shared_ptr & config) +: status_(nebula::Status::NOT_INITIALIZED), + logger_(parent_node->get_logger().get_child("HesaiDecoder")), + hw_interface_(hw_interface), + sensor_cfg_(config) { - if (!config) - { + if (!config) { throw std::runtime_error("HesaiDecoderWrapper cannot be instantiated without a valid config!"); } - if (config->sensor_model == drivers::SensorModel::HESAI_PANDARAT128) - { - calibration_file_path_ = parent_node->declare_parameter("correction_file", "", param_read_write()); - } - else - { - calibration_file_path_ = parent_node->declare_parameter("calibration_file", "", param_read_write()); + if (config->sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { + calibration_file_path_ = + parent_node->declare_parameter("correction_file", "", param_read_write()); + } else { + calibration_file_path_ = + parent_node->declare_parameter("calibration_file", "", param_read_write()); } auto calibration_result = GetCalibrationData(calibration_file_path_, false); - if (!calibration_result.has_value()) - { + if (!calibration_result.has_value()) { throw std::runtime_error( - (std::stringstream() << "No valid calibration found: " << calibration_result.error()).str()); + (std::stringstream() << "No valid calibration found: " << calibration_result.error()).str()); } calibration_cfg_ptr_ = calibration_result.value(); - RCLCPP_INFO_STREAM(logger_, "Using calibration data from " << calibration_cfg_ptr_->calibration_file); + RCLCPP_INFO_STREAM( + logger_, "Using calibration data from " << calibration_cfg_ptr_->calibration_file); RCLCPP_INFO(logger_, "Starting Decoder"); driver_ptr_ = std::make_shared(config, calibration_cfg_ptr_); status_ = driver_ptr_->GetStatus(); - if (Status::OK != status_) - { - throw std::runtime_error((std::stringstream() << "Error instantiating decoder: " << status_).str()); + if (Status::OK != status_) { + throw std::runtime_error( + (std::stringstream() << "Error instantiating decoder: " << status_).str()); } // Publish packets only if HW interface is connected - if (hw_interface_) - { + if (hw_interface_) { current_scan_msg_ = std::make_unique(); - packets_pub_ = - parent_node->create_publisher("pandar_packets", rclcpp::SensorDataQoS()); + packets_pub_ = parent_node->create_publisher( + "pandar_packets", rclcpp::SensorDataQoS()); } auto qos_profile = rmw_qos_profile_sensor_data; - auto pointcloud_qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); + auto pointcloud_qos = + rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); - nebula_points_pub_ = parent_node->create_publisher("pandar_points", pointcloud_qos); - aw_points_base_pub_ = parent_node->create_publisher("aw_points", pointcloud_qos); - aw_points_ex_pub_ = parent_node->create_publisher("aw_points_ex", pointcloud_qos); + nebula_points_pub_ = + parent_node->create_publisher("pandar_points", pointcloud_qos); + aw_points_base_pub_ = + parent_node->create_publisher("aw_points", pointcloud_qos); + aw_points_ex_pub_ = + parent_node->create_publisher("aw_points_ex", pointcloud_qos); RCLCPP_INFO_STREAM(logger_, ". Wrapper=" << status_); - cloud_watchdog_ = std::make_shared(*parent_node, 100'000us, [this, parent_node](bool ok) { - if (ok) - return; - RCLCPP_WARN_THROTTLE(logger_, *parent_node->get_clock(), 5000, "Missed pointcloud output deadline"); - }); + cloud_watchdog_ = + std::make_shared(*parent_node, 100'000us, [this, parent_node](bool ok) { + if (ok) return; + RCLCPP_WARN_THROTTLE( + logger_, *parent_node->get_clock(), 5000, "Missed pointcloud output deadline"); + }); } void HesaiDecoderWrapper::OnConfigChange( - const std::shared_ptr& new_config) + const std::shared_ptr & new_config) { std::lock_guard lock(mtx_driver_ptr_); auto new_driver = std::make_shared(new_config, calibration_cfg_ptr_); @@ -84,7 +87,7 @@ void HesaiDecoderWrapper::OnConfigChange( } void HesaiDecoderWrapper::OnCalibrationChange( - const std::shared_ptr& new_calibration) + const std::shared_ptr & new_calibration) { std::lock_guard lock(mtx_driver_ptr_); auto new_driver = std::make_shared(sensor_cfg_, new_calibration); @@ -93,54 +96,54 @@ void HesaiDecoderWrapper::OnCalibrationChange( calibration_file_path_ = calibration_cfg_ptr_->calibration_file; } -rcl_interfaces::msg::SetParametersResult HesaiDecoderWrapper::OnParameterChange(const std::vector& p) +rcl_interfaces::msg::SetParametersResult HesaiDecoderWrapper::OnParameterChange( + const std::vector & p) { - using namespace rcl_interfaces::msg; + using rcl_interfaces::msg::SetParametersResult; std::string calibration_path = ""; - // Only one of the two parameters is defined, so not checking for sensor model explicitly here is fine - bool got_any = get_param(p, "calibration_file", calibration_path) | get_param(p, "correction_file", calibration_path); - if (!got_any) - { + // Only one of the two parameters is defined, so not checking for sensor model explicitly here is + // fine + bool got_any = get_param(p, "calibration_file", calibration_path) | + get_param(p, "correction_file", calibration_path); + if (!got_any) { return rcl_interfaces::build().successful(true).reason(""); } - if (!std::filesystem::exists(calibration_path)) - { + if (!std::filesystem::exists(calibration_path)) { auto result = SetParametersResult(); result.successful = false; - result.reason = "The given calibration path does not exist, ignoring: '" + calibration_path + "'"; + result.reason = + "The given calibration path does not exist, ignoring: '" + calibration_path + "'"; return result; } auto get_calibration_result = GetCalibrationData(calibration_path, true); - if (!get_calibration_result.has_value()) - { + if (!get_calibration_result.has_value()) { auto result = SetParametersResult(); result.successful = false; - result.reason = (std::stringstream() << "Could not change calibration file to '" << calibration_path - << "': " << get_calibration_result.error()) - .str(); + result.reason = + (std::stringstream() << "Could not change calibration file to '" << calibration_path + << "': " << get_calibration_result.error()) + .str(); return result; } OnCalibrationChange(get_calibration_result.value()); - RCLCPP_INFO_STREAM(logger_, "Changed calibration to '" << calibration_cfg_ptr_->calibration_file << "'"); + RCLCPP_INFO_STREAM( + logger_, "Changed calibration to '" << calibration_cfg_ptr_->calibration_file << "'"); return rcl_interfaces::build().successful(true).reason(""); } -HesaiDecoderWrapper::get_calibration_result_t -HesaiDecoderWrapper::GetCalibrationData(const std::string& calibration_file_path, bool ignore_others) +HesaiDecoderWrapper::get_calibration_result_t HesaiDecoderWrapper::GetCalibrationData( + const std::string & calibration_file_path, bool ignore_others) { std::shared_ptr calib; - if (sensor_cfg_->sensor_model == drivers::SensorModel::HESAI_PANDARAT128) - { + if (sensor_cfg_->sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { calib = std::make_shared(); - } - else - { + } else { calib = std::make_shared(); } @@ -150,69 +153,60 @@ HesaiDecoderWrapper::GetCalibrationData(const std::string& calibration_file_path { int ext_pos = calibration_file_path.find_last_of('.'); calibration_file_path_from_sensor = calibration_file_path.substr(0, ext_pos); - // TODO: if multiple different sensors of the same type are used, this will mix up their calibration data + // TODO(mojomex): if multiple different sensors of the same type are used, this will mix up + // their calibration data calibration_file_path_from_sensor += "_from_sensor"; - calibration_file_path_from_sensor += calibration_file_path.substr(ext_pos, calibration_file_path.size() - ext_pos); + calibration_file_path_from_sensor += + calibration_file_path.substr(ext_pos, calibration_file_path.size() - ext_pos); } // If a sensor is connected, try to download and save its calibration data - if (!ignore_others && hw_connected) - { - try - { + if (!ignore_others && hw_connected) { + try { auto raw_data = hw_interface_->GetLidarCalibrationBytes(); RCLCPP_INFO(logger_, "Downloaded calibration data from sensor."); auto status = calib->SaveToFileFromBytes(calibration_file_path_from_sensor, raw_data); - if (status != Status::OK) - { + if (status != Status::OK) { RCLCPP_ERROR_STREAM(logger_, "Could not save calibration data: " << status); + } else { + RCLCPP_INFO_STREAM( + logger_, "Saved downloaded data to " << calibration_file_path_from_sensor); } - else - { - RCLCPP_INFO_STREAM(logger_, "Saved downloaded data to " << calibration_file_path_from_sensor); - } - } - catch (std::runtime_error& e) - { + } catch (std::runtime_error & e) { RCLCPP_ERROR_STREAM(logger_, "Could not download calibration data: " << e.what()); } } - // If saved calibration data from a sensor exists (either just downloaded above, or previously), try to load it - if (!ignore_others && std::filesystem::exists(calibration_file_path_from_sensor)) - { + // If saved calibration data from a sensor exists (either just downloaded above, or previously), + // try to load it + if (!ignore_others && std::filesystem::exists(calibration_file_path_from_sensor)) { auto status = calib->LoadFromFile(calibration_file_path_from_sensor); - if (status == Status::OK) - { + if (status == Status::OK) { calib->calibration_file = calibration_file_path_from_sensor; return calib; } RCLCPP_ERROR_STREAM(logger_, "Could not load downloaded calibration data: " << status); - } - else if (!ignore_others) - { + } else if (!ignore_others) { RCLCPP_ERROR(logger_, "No downloaded calibration data found."); } - if (!ignore_others) - { + if (!ignore_others) { RCLCPP_WARN(logger_, "Falling back to generic calibration file."); } // If downloaded data did not exist or could not be loaded, fall back to a generic file. // If that file does not exist either, return an error code - if (!std::filesystem::exists(calibration_file_path)) - { + if (!std::filesystem::exists(calibration_file_path)) { RCLCPP_ERROR(logger_, "No calibration data found."); return nebula::Status(Status::INVALID_CALIBRATION_FILE); } // Try to load the existing fallback calibration file. Return an error if this fails auto status = calib->LoadFromFile(calibration_file_path); - if (status != Status::OK) - { - RCLCPP_ERROR_STREAM(logger_, "Could not load calibration file at '" << calibration_file_path << "'"); + if (status != Status::OK) { + RCLCPP_ERROR_STREAM( + logger_, "Could not load calibration file at '" << calibration_file_path << "'"); return status; } @@ -221,14 +215,14 @@ HesaiDecoderWrapper::GetCalibrationData(const std::string& calibration_file_path return calib; } -void HesaiDecoderWrapper::ProcessCloudPacket(std::unique_ptr packet_msg) +void HesaiDecoderWrapper::ProcessCloudPacket( + std::unique_ptr packet_msg) { // Accumulate packets for recording only if someone is subscribed to the topic (for performance) - if (hw_interface_ && - (packets_pub_->get_subscription_count() > 0 || packets_pub_->get_intra_process_subscription_count() > 0)) - { - if (current_scan_msg_->packets.size() == 0) - { + if ( + hw_interface_ && (packets_pub_->get_subscription_count() > 0 || + packets_pub_->get_intra_process_subscription_count() > 0)) { + if (current_scan_msg_->packets.size() == 0) { current_scan_msg_->header.stamp = packet_msg->stamp; } @@ -247,57 +241,61 @@ void HesaiDecoderWrapper::ProcessCloudPacket(std::unique_ptr(pointcloud_ts); } - // A pointcloud is only emitted when a scan completes (e.g. 3599 packets do not emit, the 3600th emits one) - if (pointcloud == nullptr) - { + // A pointcloud is only emitted when a scan completes (e.g. 3599 packets do not emit, the 3600th + // emits one) + if (pointcloud == nullptr) { // Since this ends the function early, the `cloud_watchdog_` will not be updated. - // Thus, if pointclouds are not emitted for too long (e.g. when decoder settings are wrong or no packets come in), - // the watchdog will log a warning automatically + // Thus, if pointclouds are not emitted for too long (e.g. when decoder settings are wrong or no + // packets come in), the watchdog will log a warning automatically return; - }; + } cloud_watchdog_->update(); // Publish scan message only if it has been written to - if (current_scan_msg_ && !current_scan_msg_->packets.empty()) - { + if (current_scan_msg_ && !current_scan_msg_->packets.empty()) { packets_pub_->publish(std::move(current_scan_msg_)); current_scan_msg_ = std::make_unique(); } - if (nebula_points_pub_->get_subscription_count() > 0 || - nebula_points_pub_->get_intra_process_subscription_count() > 0) - { + 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()); + 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); + 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()); + 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)); + 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()); + 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_); } } -void HesaiDecoderWrapper::PublishCloud(std::unique_ptr pointcloud, - const rclcpp::Publisher::SharedPtr& publisher) +void HesaiDecoderWrapper::PublishCloud( + std::unique_ptr pointcloud, + const rclcpp::Publisher::SharedPtr & publisher) { - if (pointcloud->header.stamp.sec < 0) - { + if (pointcloud->header.stamp.sec < 0) { RCLCPP_WARN_STREAM(logger_, "Timestamp error, verify clock source."); } pointcloud->header.frame_id = sensor_cfg_->frame_id; @@ -308,12 +306,11 @@ nebula::Status HesaiDecoderWrapper::Status() { std::lock_guard lock(mtx_driver_ptr_); - if (!driver_ptr_) - { + if (!driver_ptr_) { return nebula::Status::NOT_INITIALIZED; } return driver_ptr_->GetStatus(); } } // namespace ros -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index 6e08c0e3b..8449bdfe5 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -4,65 +4,61 @@ namespace nebula { namespace ros { -HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions& options) - : rclcpp::Node("hesai_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)) - , wrapper_status_(Status::NOT_INITIALIZED) - , sensor_cfg_ptr_(nullptr) - , packet_queue_(3000) - , hw_interface_wrapper_() - , hw_monitor_wrapper_() - , decoder_wrapper_() +HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) +: rclcpp::Node("hesai_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)), + wrapper_status_(Status::NOT_INITIALIZED), + sensor_cfg_ptr_(nullptr), + packet_queue_(3000), + hw_interface_wrapper_(), + hw_monitor_wrapper_(), + decoder_wrapper_() { setvbuf(stdout, NULL, _IONBF, BUFSIZ); wrapper_status_ = DeclareAndGetSensorConfigParams(); - if (wrapper_status_ != Status::OK) - { - throw std::runtime_error((std::stringstream{} << "Sensor configuration invalid: " << wrapper_status_).str()); + if (wrapper_status_ != Status::OK) { + throw std::runtime_error( + (std::stringstream{} << "Sensor configuration invalid: " << wrapper_status_).str()); } RCLCPP_INFO_STREAM(get_logger(), "SensorConfig:" << *sensor_cfg_ptr_); launch_hw_ = declare_parameter("launch_hw", param_read_only()); - if (launch_hw_) - { + if (launch_hw_) { hw_interface_wrapper_.emplace(this, sensor_cfg_ptr_); hw_monitor_wrapper_.emplace(this, hw_interface_wrapper_->HwInterface(), sensor_cfg_ptr_); } - decoder_wrapper_.emplace(this, hw_interface_wrapper_ ? hw_interface_wrapper_->HwInterface() : nullptr, - sensor_cfg_ptr_); + decoder_wrapper_.emplace( + this, hw_interface_wrapper_ ? hw_interface_wrapper_->HwInterface() : nullptr, sensor_cfg_ptr_); RCLCPP_DEBUG(get_logger(), "Starting stream"); decoder_thread_ = std::thread([this]() { - while (true) - { + while (true) { decoder_wrapper_->ProcessCloudPacket(std::move(packet_queue_.pop())); } }); - if (launch_hw_) - { + if (launch_hw_) { hw_interface_wrapper_->HwInterface()->RegisterScanCallback( - std::bind(&HesaiRosWrapper::ReceiveCloudPacketCallback, this, std::placeholders::_1)); + std::bind(&HesaiRosWrapper::ReceiveCloudPacketCallback, this, std::placeholders::_1)); StreamStart(); - } - else - { + } else { packets_sub_ = create_subscription( - "pandar_packets", rclcpp::SensorDataQoS(), - std::bind(&HesaiRosWrapper::ReceiveScanMessageCallback, this, std::placeholders::_1)); - RCLCPP_INFO_STREAM(get_logger(), - "Hardware connection disabled, listening for packets on " << packets_sub_->get_topic_name()); + "pandar_packets", rclcpp::SensorDataQoS(), + std::bind(&HesaiRosWrapper::ReceiveScanMessageCallback, this, std::placeholders::_1)); + RCLCPP_INFO_STREAM( + get_logger(), + "Hardware connection disabled, listening for packets on " << packets_sub_->get_topic_name()); } - // Register parameter callback after all params have been declared. Otherwise it would be called once for each - // declaration - parameter_event_cb_ = - add_on_set_parameters_callback(std::bind(&HesaiRosWrapper::OnParameterChange, this, std::placeholders::_1)); + // Register parameter callback after all params have been declared. Otherwise it would be called + // once for each declaration + parameter_event_cb_ = add_on_set_parameters_callback( + std::bind(&HesaiRosWrapper::OnParameterChange, this, std::placeholders::_1)); } nebula::Status HesaiRosWrapper::DeclareAndGetSensorConfigParams() @@ -76,7 +72,8 @@ nebula::Status HesaiRosWrapper::DeclareAndGetSensorConfigParams() config.return_mode = drivers::ReturnModeFromStringHesai(_return_mode, config.sensor_model); config.host_ip = declare_parameter("host_ip", "255.255.255.255", param_read_only()); - config.sensor_ip = declare_parameter("sensor_ip", "192.168.1.201", param_read_only()); + config.sensor_ip = + declare_parameter("sensor_ip", "192.168.1.201", param_read_only()); config.data_port = declare_parameter("data_port", 2368, param_read_only()); config.gnss_port = declare_parameter("gnss_port", 2369, param_read_only()); config.frame_id = declare_parameter("frame_id", "pandar", param_read_write()); @@ -96,19 +93,17 @@ nebula::Status HesaiRosWrapper::DeclareAndGetSensorConfigParams() rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); uint16_t default_value; RCLCPP_DEBUG_STREAM(get_logger(), config.sensor_model); - if (config.sensor_model == nebula::drivers::SensorModel::HESAI_PANDARAT128) - { + if (config.sensor_model == nebula::drivers::SensorModel::HESAI_PANDARAT128) { descriptor.additional_constraints = "200, 400"; descriptor.integer_range = int_range(200, 400, 200); default_value = 200; - } - else - { + } else { descriptor.additional_constraints = "300, 600, 1200"; descriptor.integer_range = int_range(300, 1200, 300); default_value = 600; } - config.rotation_speed = declare_parameter("rotation_speed", default_value, descriptor); + config.rotation_speed = + declare_parameter("rotation_speed", default_value, descriptor); } { rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); @@ -125,7 +120,7 @@ nebula::Status HesaiRosWrapper::DeclareAndGetSensorConfigParams() descriptor.additional_constraints = "Dual return distance threshold [0.01, 0.5]"; descriptor.floating_point_range = float_range(0.01, 0.5, 0.01); config.dual_return_distance_threshold = - declare_parameter("dual_return_distance_threshold", 0.1, descriptor); + declare_parameter("dual_return_distance_threshold", 0.1, descriptor); } auto _ptp_profile = declare_parameter("ptp_profile", "", param_read_only()); @@ -134,13 +129,14 @@ nebula::Status HesaiRosWrapper::DeclareAndGetSensorConfigParams() auto _ptp_transport = declare_parameter("ptp_transport_type", "", param_read_only()); config.ptp_transport_type = drivers::PtpTransportTypeFromString(_ptp_transport); - if (config.ptp_transport_type != drivers::PtpTransportType::L2 && - config.ptp_profile != drivers::PtpProfile::IEEE_1588v2 && - config.ptp_profile != drivers::PtpProfile::UNKNOWN_PROFILE) - { - RCLCPP_WARN_STREAM(get_logger(), "PTP transport was set to '" << _ptp_transport << "' but PTP profile '" - << _ptp_profile - << "' only supports 'L2'. Setting it to 'L2'."); + if ( + config.ptp_transport_type != drivers::PtpTransportType::L2 && + config.ptp_profile != drivers::PtpProfile::IEEE_1588v2 && + config.ptp_profile != drivers::PtpProfile::UNKNOWN_PROFILE) { + RCLCPP_WARN_STREAM( + get_logger(), "PTP transport was set to '" << _ptp_transport << "' but PTP profile '" + << _ptp_profile + << "' only supports 'L2'. Setting it to 'L2'."); config.ptp_transport_type = drivers::PtpTransportType::L2; set_parameter(rclcpp::Parameter("ptp_transport_type", "L2")); } @@ -158,48 +154,43 @@ nebula::Status HesaiRosWrapper::DeclareAndGetSensorConfigParams() return ValidateAndSetConfig(new_cfg_ptr); } -Status HesaiRosWrapper::ValidateAndSetConfig(std::shared_ptr& new_config) +Status HesaiRosWrapper::ValidateAndSetConfig( + std::shared_ptr & new_config) { - if (new_config->sensor_model == nebula::drivers::SensorModel::UNKNOWN) - { + if (new_config->sensor_model == nebula::drivers::SensorModel::UNKNOWN) { return Status::INVALID_SENSOR_MODEL; } - if (new_config->return_mode == nebula::drivers::ReturnMode::UNKNOWN) - { + if (new_config->return_mode == nebula::drivers::ReturnMode::UNKNOWN) { return Status::INVALID_ECHO_MODE; } - if (new_config->frame_id.empty()) - { + if (new_config->frame_id.empty()) { return Status::SENSOR_CONFIG_ERROR; } - if (new_config->ptp_profile == nebula::drivers::PtpProfile::UNKNOWN_PROFILE) - { - RCLCPP_ERROR_STREAM(get_logger(), "Invalid PTP Profile Provided. Please use '1588v2', '802.1as' or 'automotive'"); + if (new_config->ptp_profile == nebula::drivers::PtpProfile::UNKNOWN_PROFILE) { + RCLCPP_ERROR_STREAM( + get_logger(), "Invalid PTP Profile Provided. Please use '1588v2', '802.1as' or 'automotive'"); return Status::SENSOR_CONFIG_ERROR; } - if (new_config->ptp_transport_type == nebula::drivers::PtpTransportType::UNKNOWN_TRANSPORT) - { - RCLCPP_ERROR_STREAM(get_logger(), - "Invalid PTP Transport Provided. Please use 'udp' or 'l2', 'udp' is only available when " - "using the '1588v2' PTP Profile"); + if (new_config->ptp_transport_type == nebula::drivers::PtpTransportType::UNKNOWN_TRANSPORT) { + RCLCPP_ERROR_STREAM( + get_logger(), + "Invalid PTP Transport Provided. Please use 'udp' or 'l2', 'udp' is only available when " + "using the '1588v2' PTP Profile"); return Status::SENSOR_CONFIG_ERROR; } - if (new_config->ptp_switch_type == nebula::drivers::PtpSwitchType::UNKNOWN_SWITCH) - { - RCLCPP_ERROR_STREAM(get_logger(), "Invalid PTP Switch Type Provided. Please use 'tsn' or 'non_tsn'"); + if (new_config->ptp_switch_type == nebula::drivers::PtpSwitchType::UNKNOWN_SWITCH) { + RCLCPP_ERROR_STREAM( + get_logger(), "Invalid PTP Switch Type Provided. Please use 'tsn' or 'non_tsn'"); return Status::SENSOR_CONFIG_ERROR; } - if (hw_interface_wrapper_) - { + if (hw_interface_wrapper_) { hw_interface_wrapper_->OnConfigChange(new_config); } - if (hw_monitor_wrapper_) - { + if (hw_monitor_wrapper_) { hw_monitor_wrapper_->OnConfigChange(new_config); } - if (decoder_wrapper_) - { + if (decoder_wrapper_) { decoder_wrapper_->OnConfigChange(new_config); } @@ -207,17 +198,17 @@ Status HesaiRosWrapper::ValidateAndSetConfig(std::shared_ptr scan_msg) +void HesaiRosWrapper::ReceiveScanMessageCallback( + std::unique_ptr scan_msg) { - if (hw_interface_wrapper_) - { - RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 1000, - "Ignoring received PandarScan. Launch with launch_hw:=false to enable PandarScan replay."); + if (hw_interface_wrapper_) { + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 1000, + "Ignoring received PandarScan. Launch with launch_hw:=false to enable PandarScan replay."); return; } - for (auto& pkt : scan_msg->packets) - { + for (auto & pkt : scan_msg->packets) { auto nebula_pkt_ptr = std::make_unique(); nebula_pkt_ptr->stamp = pkt.stamp; std::copy(pkt.data.begin(), pkt.data.end(), std::back_inserter(nebula_pkt_ptr->data)); @@ -233,25 +224,23 @@ Status HesaiRosWrapper::GetStatus() Status HesaiRosWrapper::StreamStart() { - if (!hw_interface_wrapper_) - { + if (!hw_interface_wrapper_) { return Status::UDP_CONNECTION_ERROR; } - if (hw_interface_wrapper_->Status() != Status::OK) - { + if (hw_interface_wrapper_->Status() != Status::OK) { return hw_interface_wrapper_->Status(); } return hw_interface_wrapper_->HwInterface()->SensorInterfaceStart(); } -rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::OnParameterChange(const std::vector& p) +rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::OnParameterChange( + const std::vector & p) { - using namespace rcl_interfaces::msg; + using rcl_interfaces::msg::SetParametersResult; - if (p.empty()) - { + if (p.empty()) { return rcl_interfaces::build().successful(true).reason(""); } @@ -262,25 +251,25 @@ rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::OnParameterChange(cons drivers::HesaiSensorConfiguration new_cfg(*sensor_cfg_ptr_); std::string _return_mode = ""; - bool got_any = get_param(p, "return_mode", _return_mode) | get_param(p, "frame_id", new_cfg.frame_id) | - get_param(p, "scan_phase", new_cfg.scan_phase) | get_param(p, "min_range", new_cfg.min_range) | - get_param(p, "max_range", new_cfg.max_range) | get_param(p, "rotation_speed", new_cfg.rotation_speed) | - get_param(p, "cloud_min_angle", new_cfg.cloud_min_angle) | - get_param(p, "cloud_max_angle", new_cfg.cloud_max_angle) | - get_param(p, "dual_return_distance_threshold", new_cfg.dual_return_distance_threshold); - - // Currently, HW interface and monitor wrappers have only read-only parameters, so their update logic is not - // implemented - if (decoder_wrapper_) - { + bool got_any = + get_param(p, "return_mode", _return_mode) | get_param(p, "frame_id", new_cfg.frame_id) | + get_param(p, "scan_phase", new_cfg.scan_phase) | get_param(p, "min_range", new_cfg.min_range) | + get_param(p, "max_range", new_cfg.max_range) | + get_param(p, "rotation_speed", new_cfg.rotation_speed) | + get_param(p, "cloud_min_angle", new_cfg.cloud_min_angle) | + get_param(p, "cloud_max_angle", new_cfg.cloud_max_angle) | + get_param(p, "dual_return_distance_threshold", new_cfg.dual_return_distance_threshold); + + // Currently, HW interface and monitor wrappers have only read-only parameters, so their update + // logic is not implemented + if (decoder_wrapper_) { auto result = decoder_wrapper_->OnParameterChange(p); if (!result.successful) { return result; } } - if (!got_any) - { + if (!got_any) { return rcl_interfaces::build().successful(true).reason(""); } @@ -290,8 +279,7 @@ rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::OnParameterChange(cons auto new_cfg_ptr = std::make_shared(new_cfg); auto status = ValidateAndSetConfig(new_cfg_ptr); - if (status != Status::OK) - { + if (status != Status::OK) { RCLCPP_WARN_STREAM(get_logger(), "OnParameterChange aborted: " << status); auto result = SetParametersResult(); result.successful = false; @@ -302,23 +290,22 @@ rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::OnParameterChange(cons return rcl_interfaces::build().successful(true).reason(""); } -void HesaiRosWrapper::ReceiveCloudPacketCallback(std::vector& packet) +void HesaiRosWrapper::ReceiveCloudPacketCallback(std::vector & packet) { - if (!decoder_wrapper_ || decoder_wrapper_->Status() != Status::OK) - { + if (!decoder_wrapper_ || decoder_wrapper_->Status() != Status::OK) { return; } const auto now = std::chrono::high_resolution_clock::now(); - const auto timestamp_ns = std::chrono::duration_cast(now.time_since_epoch()).count(); + const auto timestamp_ns = + std::chrono::duration_cast(now.time_since_epoch()).count(); auto msg_ptr = std::make_unique(); msg_ptr->stamp.sec = static_cast(timestamp_ns / 1'000'000'000); msg_ptr->stamp.nanosec = static_cast(timestamp_ns % 1'000'000'000); msg_ptr->data.swap(packet); - if (!packet_queue_.try_push(std::move(msg_ptr))) - { + if (!packet_queue_.try_push(std::move(msg_ptr))) { RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped"); } } diff --git a/nebula_ros/src/hesai/hw_interface_wrapper.cpp b/nebula_ros/src/hesai/hw_interface_wrapper.cpp index c10319c1e..18da9da83 100644 --- a/nebula_ros/src/hesai/hw_interface_wrapper.cpp +++ b/nebula_ros/src/hesai/hw_interface_wrapper.cpp @@ -6,18 +6,21 @@ namespace ros { HesaiHwInterfaceWrapper::HesaiHwInterfaceWrapper( - rclcpp::Node* const parent_node, std::shared_ptr& config) - : hw_interface_(new nebula::drivers::HesaiHwInterface()) - , logger_(parent_node->get_logger().get_child("HwInterface")) - , status_(Status::NOT_INITIALIZED) + rclcpp::Node * const parent_node, + std::shared_ptr & config) +: hw_interface_(new nebula::drivers::HesaiHwInterface()), + logger_(parent_node->get_logger().get_child("HwInterface")), + status_(Status::NOT_INITIALIZED) { setup_sensor_ = parent_node->declare_parameter("setup_sensor", true, param_read_only()); bool retry_connect = parent_node->declare_parameter("retry_hw", true, param_read_only()); - status_ = hw_interface_->SetSensorConfiguration(std::static_pointer_cast(config)); + status_ = hw_interface_->SetSensorConfiguration( + std::static_pointer_cast(config)); if (status_ != Status::OK) { - throw std::runtime_error((std::stringstream{} << "Could not initialize HW interface: " << status_).str()); + throw std::runtime_error( + (std::stringstream{} << "Could not initialize HW interface: " << status_).str()); } hw_interface_->SetLogger(std::make_shared(parent_node->get_logger())); @@ -25,11 +28,9 @@ HesaiHwInterfaceWrapper::HesaiHwInterfaceWrapper( int retry_count = 0; - while (true) - { + while (true) { status_ = hw_interface_->InitializeTcpDriver(); - if (status_ == Status::OK || !retry_connect) - { + if (status_ == Status::OK || !retry_connect) { break; } @@ -38,35 +39,30 @@ HesaiHwInterfaceWrapper::HesaiHwInterfaceWrapper( RCLCPP_WARN_STREAM(logger_, status_ << ". Retry #" << retry_count); } - if (status_ == Status::OK) - { - try - { + if (status_ == Status::OK) { + try { auto inventory = hw_interface_->GetInventory(); RCLCPP_INFO_STREAM(logger_, inventory); hw_interface_->SetTargetModel(inventory.model); - } - catch (...) - { + } catch (...) { RCLCPP_ERROR_STREAM(logger_, "Failed to get model from sensor..."); } - if (setup_sensor_) - { + if (setup_sensor_) { hw_interface_->CheckAndSetConfig(); } - } - else - { - RCLCPP_ERROR_STREAM(logger_, "Failed to get model from sensor... Set from config: " << config->sensor_model); + } else { + RCLCPP_ERROR_STREAM( + logger_, "Failed to get model from sensor... Set from config: " << config->sensor_model); } status_ = Status::OK; } void HesaiHwInterfaceWrapper::OnConfigChange( - const std::shared_ptr& new_config) + const std::shared_ptr & new_config) { - hw_interface_->SetSensorConfiguration(std::static_pointer_cast(new_config)); + hw_interface_->SetSensorConfiguration( + std::static_pointer_cast(new_config)); if (setup_sensor_) { hw_interface_->CheckAndSetConfig(); } @@ -83,4 +79,4 @@ std::shared_ptr HesaiHwInterfaceWrapper::HwInterface( } } // namespace ros -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_ros/src/hesai/hw_monitor_wrapper.cpp b/nebula_ros/src/hesai/hw_monitor_wrapper.cpp index b34e181fb..da1f78794 100644 --- a/nebula_ros/src/hesai/hw_monitor_wrapper.cpp +++ b/nebula_ros/src/hesai/hw_monitor_wrapper.cpp @@ -4,19 +4,19 @@ namespace nebula { namespace ros { -HesaiHwMonitorWrapper::HesaiHwMonitorWrapper(rclcpp::Node* const parent_node, - const std::shared_ptr& hw_interface, - std::shared_ptr& config) - : logger_(parent_node->get_logger().get_child("HwMonitor")) - , diagnostics_updater_(parent_node) - , status_(Status::OK) - , hw_interface_(hw_interface) - , parent_node_(parent_node) +HesaiHwMonitorWrapper::HesaiHwMonitorWrapper( + rclcpp::Node * const parent_node, + const std::shared_ptr & hw_interface, + std::shared_ptr & config) +: logger_(parent_node->get_logger().get_child("HwMonitor")), + diagnostics_updater_(parent_node), + status_(Status::OK), + hw_interface_(hw_interface), + parent_node_(parent_node) { diag_span_ = parent_node->declare_parameter("diag_span", 1000, param_read_only()); - switch (config->sensor_model) - { + switch (config->sensor_model) { case nebula::drivers::SensorModel::HESAI_PANDARXT32: case nebula::drivers::SensorModel::HESAI_PANDARXT32M: case nebula::drivers::SensorModel::HESAI_PANDARAT128: @@ -71,7 +71,8 @@ void HesaiHwMonitorWrapper::InitializeHesaiDiagnostics() diagnostics_updater_.add("hesai_status", this, &HesaiHwMonitorWrapper::HesaiCheckStatus); diagnostics_updater_.add("hesai_ptp", this, &HesaiHwMonitorWrapper::HesaiCheckPtp); - diagnostics_updater_.add("hesai_temperature", this, &HesaiHwMonitorWrapper::HesaiCheckTemperature); + diagnostics_updater_.add( + "hesai_temperature", this, &HesaiHwMonitorWrapper::HesaiCheckTemperature); diagnostics_updater_.add("hesai_rpm", this, &HesaiHwMonitorWrapper::HesaiCheckRpm); current_status_.reset(); @@ -83,25 +84,19 @@ void HesaiHwMonitorWrapper::InitializeHesaiDiagnostics() auto fetch_diag_from_sensor = [this]() { OnHesaiStatusTimer(); - if (hw_interface_->UseHttpGetLidarMonitor()) - { + if (hw_interface_->UseHttpGetLidarMonitor()) { OnHesaiLidarMonitorTimerHttp(); - } - else - { + } else { OnHesaiLidarMonitorTimer(); } }; - fetch_diagnostics_timer_ = - parent_node_->create_wall_timer(std::chrono::milliseconds(diag_span_), std::move(fetch_diag_from_sensor)); + fetch_diagnostics_timer_ = parent_node_->create_wall_timer( + std::chrono::milliseconds(diag_span_), std::move(fetch_diag_from_sensor)); - if (hw_interface_->UseHttpGetLidarMonitor()) - { + if (hw_interface_->UseHttpGetLidarMonitor()) { diagnostics_updater_.add("hesai_voltage", this, &HesaiHwMonitorWrapper::HesaiCheckVoltageHttp); - } - else - { + } else { diagnostics_updater_.add("hesai_voltage", this, &HesaiHwMonitorWrapper::HesaiCheckVoltage); } @@ -112,45 +107,37 @@ void HesaiHwMonitorWrapper::InitializeHesaiDiagnostics() RCLCPP_DEBUG_STREAM(logger_, "dif(status): " << dif); - if (diag_span_ * 2.0 < dif * 1000) - { + if (diag_span_ * 2.0 < dif * 1000) { current_diag_status_ = diagnostic_msgs::msg::DiagnosticStatus::STALE; RCLCPP_DEBUG_STREAM(logger_, "STALE"); - } - else - { + } else { current_diag_status_ = diagnostic_msgs::msg::DiagnosticStatus::OK; RCLCPP_DEBUG_STREAM(logger_, "OK"); } dif = (now - *current_lidar_monitor_time_).seconds(); RCLCPP_DEBUG_STREAM(logger_, "dif(monitor): " << dif); - if (diag_span_ * 2.0 < dif * 1000) - { + if (diag_span_ * 2.0 < dif * 1000) { current_monitor_status_ = diagnostic_msgs::msg::DiagnosticStatus::STALE; RCLCPP_DEBUG_STREAM(logger_, "STALE"); - } - else - { + } else { current_monitor_status_ = diagnostic_msgs::msg::DiagnosticStatus::OK; RCLCPP_DEBUG_STREAM(logger_, "OK"); } diagnostics_updater_.force_update(); }; diagnostics_update_timer_ = - parent_node_->create_wall_timer(std::chrono::milliseconds(1000), std::move(on_timer_update)); + parent_node_->create_wall_timer(std::chrono::milliseconds(1000), std::move(on_timer_update)); RCLCPP_DEBUG_STREAM(logger_, "add_timer"); } -std::string HesaiHwMonitorWrapper::GetPtreeValue(boost::property_tree::ptree* pt, const std::string& key) +std::string HesaiHwMonitorWrapper::GetPtreeValue( + boost::property_tree::ptree * pt, const std::string & key) { boost::optional value = pt->get_optional(key); - if (value) - { + if (value) { return value.get(); - } - else - { + } else { return MSG_NOT_SUPPORTED; } } @@ -164,22 +151,19 @@ std::string HesaiHwMonitorWrapper::GetFixedPrecisionString(double val, int pre) void HesaiHwMonitorWrapper::OnHesaiStatusTimer() { RCLCPP_DEBUG_STREAM(logger_, "OnHesaiStatusTimer" << std::endl); - try - { + try { auto result = hw_interface_->GetLidarStatus(); std::scoped_lock lock(mtx_lidar_status_); current_status_time_.reset(new rclcpp::Time(parent_node_->get_clock()->now())); current_status_.reset(new HesaiLidarStatus(result)); - } - catch (const std::system_error& error) - { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiStatusTimer(std::system_error)"), - error.what()); - } - catch (const boost::system::system_error& error) - { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiStatusTimer(boost::system::system_error)"), - error.what()); + } catch (const std::system_error & error) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiStatusTimer(std::system_error)"), + error.what()); + } catch (const boost::system::system_error & error) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiStatusTimer(boost::system::system_error)"), + error.what()); } RCLCPP_DEBUG_STREAM(logger_, "OnHesaiStatusTimer END" << std::endl); } @@ -187,24 +171,23 @@ void HesaiHwMonitorWrapper::OnHesaiStatusTimer() void HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimerHttp() { RCLCPP_DEBUG_STREAM(logger_, "OnHesaiLidarMonitorTimerHttp"); - try - { - hw_interface_->GetLidarMonitorAsyncHttp([this](const std::string& str) { + try { + hw_interface_->GetLidarMonitorAsyncHttp([this](const std::string & str) { std::scoped_lock lock(mtx_lidar_monitor_); current_lidar_monitor_time_.reset(new rclcpp::Time(parent_node_->get_clock()->now())); - current_lidar_monitor_tree_ = std::make_unique(hw_interface_->ParseJson(str)); + current_lidar_monitor_tree_ = + std::make_unique(hw_interface_->ParseJson(str)); }); - } - catch (const std::system_error& error) - { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimerHttp(std::system_error)"), - error.what()); - } - catch (const boost::system::system_error& error) - { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimerHttp(boost::system::system_" - "error)"), - error.what()); + } catch (const std::system_error & error) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimerHttp(std::system_error)"), + error.what()); + } catch (const boost::system::system_error & error) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger( + "HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimerHttp(boost::system::system_" + "error)"), + error.what()); } RCLCPP_DEBUG_STREAM(logger_, "OnHesaiLidarMonitorTimerHttp END"); } @@ -212,52 +195,47 @@ void HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimerHttp() void HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimer() { RCLCPP_DEBUG_STREAM(logger_, "OnHesaiLidarMonitorTimer"); - try - { + try { auto result = hw_interface_->GetLidarMonitor(); std::scoped_lock lock(mtx_lidar_monitor_); current_lidar_monitor_time_.reset(new rclcpp::Time(parent_node_->get_clock()->now())); current_monitor_.reset(new HesaiLidarMonitor(result)); - } - catch (const std::system_error& error) - { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimer(std::system_error)"), - error.what()); - } - catch (const boost::system::system_error& error) - { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimer(boost::system::system_" - "error)"), - error.what()); + } catch (const std::system_error & error) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimer(std::system_error)"), + error.what()); + } catch (const boost::system::system_error & error) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimer(boost::system::system_" + "error)"), + error.what()); } RCLCPP_DEBUG_STREAM(logger_, "OnHesaiLidarMonitorTimer END"); } -void HesaiHwMonitorWrapper::HesaiCheckStatus(diagnostic_updater::DiagnosticStatusWrapper& diagnostics) +void HesaiHwMonitorWrapper::HesaiCheckStatus( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { std::scoped_lock lock(mtx_lidar_status_); - if (current_status_) - { + if (current_status_) { uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::vector msg; diagnostics.add("system_uptime", std::to_string(current_status_->system_uptime.value())); diagnostics.add("startup_times", std::to_string(current_status_->startup_times.value())); - diagnostics.add("total_operation_time", std::to_string(current_status_->total_operation_time.value())); + diagnostics.add( + "total_operation_time", std::to_string(current_status_->total_operation_time.value())); diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } - else - { + } else { diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); } } -void HesaiHwMonitorWrapper::HesaiCheckPtp(diagnostic_updater::DiagnosticStatusWrapper& diagnostics) +void HesaiHwMonitorWrapper::HesaiCheckPtp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { std::scoped_lock lock(mtx_lidar_status_); - if (current_status_) - { + if (current_status_) { uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::vector msg; auto gps_status = current_status_->get_str_gps_pps_lock(); @@ -269,131 +247,116 @@ void HesaiHwMonitorWrapper::HesaiCheckPtp(diagnostic_updater::DiagnosticStatusWr diagnostics.add("gps_pps_lock", gps_status); diagnostics.add("gps_gprmc_status", gprmc_status); diagnostics.add("ptp_clock_status", ptp_status); - if (gps_status != "UNKNOWN") - { + if (gps_status != "UNKNOWN") { msg.emplace_back("gps_pps_lock: " + gps_status); } - if (gprmc_status != "UNKNOWN") - { + if (gprmc_status != "UNKNOWN") { msg.emplace_back("gprmc_status: " + gprmc_status); } - if (ptp_status != "UNKNOWN") - { + if (ptp_status != "UNKNOWN") { msg.emplace_back("ptp_status: " + ptp_status); } - if (ptp_status == "FREE RUN" && gps_status == "UNKNOWN") - { + if (ptp_status == "FREE RUN" && gps_status == "UNKNOWN") { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; } diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } - else - { + } else { diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); } } -void HesaiHwMonitorWrapper::HesaiCheckTemperature(diagnostic_updater::DiagnosticStatusWrapper& diagnostics) +void HesaiHwMonitorWrapper::HesaiCheckTemperature( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { std::scoped_lock lock(mtx_lidar_status_); - if (current_status_) - { + if (current_status_) { uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::vector msg; - for (size_t i = 0; i < std::size(current_status_->temperature); i++) - { - diagnostics.add(temperature_names_[i], - GetFixedPrecisionString(current_status_->temperature[i].value() * 0.01, 3)); + for (size_t i = 0; i < std::size(current_status_->temperature); i++) { + diagnostics.add( + temperature_names_[i], + GetFixedPrecisionString(current_status_->temperature[i].value() * 0.01, 3)); } diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } - else - { + } else { diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); } } -void HesaiHwMonitorWrapper::HesaiCheckRpm(diagnostic_updater::DiagnosticStatusWrapper& diagnostics) +void HesaiHwMonitorWrapper::HesaiCheckRpm(diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { std::scoped_lock lock(mtx_lidar_status_); - if (current_status_) - { + if (current_status_) { uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::vector msg; diagnostics.add("motor_speed", std::to_string(current_status_->motor_speed.value())); diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } - else - { + } else { diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); } } -void HesaiHwMonitorWrapper::HesaiCheckVoltageHttp(diagnostic_updater::DiagnosticStatusWrapper& diagnostics) +void HesaiHwMonitorWrapper::HesaiCheckVoltageHttp( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { std::scoped_lock lock(mtx_lidar_monitor_); - if (current_lidar_monitor_tree_) - { + if (current_lidar_monitor_tree_) { uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::vector msg; std::string key = ""; std::string mes; key = "lidarInCur"; - try - { + try { mes = GetPtreeValue(current_lidar_monitor_tree_.get(), "Body." + key); - } - catch (boost::bad_lexical_cast& ex) - { + } catch (boost::bad_lexical_cast & ex) { level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; mes = MSG_ERROR + std::string(ex.what()); } diagnostics.add(key, mes); key = "lidarInVol"; - try - { + try { mes = GetPtreeValue(current_lidar_monitor_tree_.get(), "Body." + key); - } - catch (boost::bad_lexical_cast& ex) - { + } catch (boost::bad_lexical_cast & ex) { level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; mes = MSG_ERROR + std::string(ex.what()); } diagnostics.add(key, mes); diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } - else - { + } else { diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); } } -void HesaiHwMonitorWrapper::HesaiCheckVoltage(diagnostic_updater::DiagnosticStatusWrapper& diagnostics) +void HesaiHwMonitorWrapper::HesaiCheckVoltage( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { std::scoped_lock lock(mtx_lidar_monitor_); - if (current_monitor_) - { + if (current_monitor_) { uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::vector msg; - diagnostics.add("input_voltage", GetFixedPrecisionString(current_monitor_->input_voltage.value() * 0.01, 3) + " V"); - diagnostics.add("input_current", GetFixedPrecisionString(current_monitor_->input_current.value() * 0.01, 3) + " m" - "A"); - diagnostics.add("input_power", GetFixedPrecisionString(current_monitor_->input_power.value() * 0.01, 3) + " W"); + diagnostics.add( + "input_voltage", + GetFixedPrecisionString(current_monitor_->input_voltage.value() * 0.01, 3) + " V"); + diagnostics.add( + "input_current", GetFixedPrecisionString(current_monitor_->input_current.value() * 0.01, 3) + + " m" + "A"); + diagnostics.add( + "input_power", + GetFixedPrecisionString(current_monitor_->input_power.value() * 0.01, 3) + " W"); diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } - else - { + } else { diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); } } Status HesaiHwMonitorWrapper::Status() { - return Status::NOT_IMPLEMENTED; // TODO + return Status::OK; } } // namespace ros -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_tests/hesai/hesai_ros_decoder_test.cpp b/nebula_tests/hesai/hesai_ros_decoder_test.cpp index f65a974cc..97046e8f8 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test.cpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test.cpp @@ -220,8 +220,7 @@ Status HesaiRosDecoderTest::GetParameters( if (sensor_configuration.return_mode == nebula::drivers::ReturnMode::UNKNOWN) { return Status::INVALID_ECHO_MODE; } - if ( - sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { + if (sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { return Status::SENSOR_CONFIG_ERROR; } if (calibration_configuration.calibration_file.empty()) { @@ -274,7 +273,7 @@ void HesaiRosDecoderTest::ReadBag( storage_options.uri = params_.bag_path; storage_options.storage_id = params_.storage_id; - converter_options.output_serialization_format = params_.format; //"cdr"; + converter_options.output_serialization_format = params_.format; rclcpp::Serialization serialization; rosbag2_cpp::Reader bag_reader(std::make_unique()); @@ -296,7 +295,8 @@ void HesaiRosDecoderTest::ReadBag( auto extracted_msg_ptr = std::make_shared(extracted_msg); for (auto & pkt : extracted_msg_ptr->packets) { - auto pointcloud_ts = driver_ptr_->ParseCloudPacket(std::vector(pkt.data.begin(), std::next(pkt.data.begin(), pkt.size))); + auto pointcloud_ts = driver_ptr_->ParseCloudPacket( + std::vector(pkt.data.begin(), std::next(pkt.data.begin(), pkt.size))); auto pointcloud = std::get<0>(pointcloud_ts); auto scan_timestamp = std::get<1>(pointcloud_ts); diff --git a/nebula_tests/hesai/hesai_ros_decoder_test.hpp b/nebula_tests/hesai/hesai_ros_decoder_test.hpp index 43996b9ac..f12e56020 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test.hpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test.hpp @@ -17,6 +17,8 @@ #include #include +#include +#include #ifndef _SRC_CALIBRATION_DIR_PATH #define _SRC_CALIBRATION_DIR_PATH "" @@ -48,20 +50,22 @@ struct HesaiRosDecoderTestParams double dual_return_distance_threshold = 0.1; }; -inline std::ostream & operator<<(std::ostream & os, nebula::ros::HesaiRosDecoderTestParams const & arg) { +inline std::ostream & operator<<( + std::ostream & os, nebula::ros::HesaiRosDecoderTestParams const & arg) +{ return os << "sensor_model=" << arg.sensor_model << ", " - << "return_mode=" << arg.return_mode << ", " - << "calibration_file=" << arg.calibration_file << ", " - << "bag_path=" << arg.bag_path << ", " - << "correction_file=" << arg.correction_file << ", " - << "frame_id=" << arg.frame_id << ", " - << "scan_phase=" << arg.scan_phase << ", " - << "min_range=" << arg.min_range << ", " - << "max_range=" << arg.max_range << ", " - << "storage_id=" << arg.storage_id << ", " - << "format=" << arg.format << ", " - << "target_topic=" << arg.target_topic << ", " - << "dual_return_distance_threshold=" << arg.dual_return_distance_threshold << ", "; + << "return_mode=" << arg.return_mode << ", " + << "calibration_file=" << arg.calibration_file << ", " + << "bag_path=" << arg.bag_path << ", " + << "correction_file=" << arg.correction_file << ", " + << "frame_id=" << arg.frame_id << ", " + << "scan_phase=" << arg.scan_phase << ", " + << "min_range=" << arg.min_range << ", " + << "max_range=" << arg.max_range << ", " + << "storage_id=" << arg.storage_id << ", " + << "format=" << arg.format << ", " + << "target_topic=" << arg.target_topic << ", " + << "dual_return_distance_threshold=" << arg.dual_return_distance_threshold << ", "; } /// @brief Testing decoder of pandar 40p (Keeps HesaiDriverRosWrapper structure as much as diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp b/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp index d6574ccd4..7f80f6f48 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp @@ -12,6 +12,8 @@ #include #include #include +#include +#include namespace nebula { @@ -19,72 +21,15 @@ namespace test { const nebula::ros::HesaiRosDecoderTestParams TEST_CONFIGS[6] = { - { - "Pandar40P", - "Dual", - "Pandar40P.csv", - "40p/1673400149412331409", - "", - "hesai", - 0, - 0.3f, - 200.f - }, - { - "Pandar64", - "Dual", - "Pandar64.csv", - "64/1673403880599376836", - "", - "hesai", - 0, - 0.3f, - 200.f - }, - { - "PandarAT128", - "LastStrongest", - "PandarAT128.csv", - "at128/1679653308406038376", - "PandarAT128.dat", - "hesai", - 0, - 1.f, - 180.f - }, - { - "PandarQT64", - "Dual", - "PandarQT64.csv", - "qt64/1673401195788312575", - "", - "hesai", - 0, - 0.1f, - 60.f - }, - { - "PandarXT32", - "Dual", - "PandarXT32.csv", - "xt32/1673400677802009732", - "", - "hesai", - 0, - 0.05f, - 120.f - }, - { - "PandarXT32M", - "LastStrongest", - "PandarXT32M.csv", - "xt32m/1660893203042895158", - "", - "hesai", - 0, - 0.5f, - 300.f - }}; + {"Pandar40P", "Dual", "Pandar40P.csv", "40p/1673400149412331409", "", "hesai", 0, 0.3f, 200.f}, + {"Pandar64", "Dual", "Pandar64.csv", "64/1673403880599376836", "", "hesai", 0, 0.3f, 200.f}, + {"PandarAT128", "LastStrongest", "PandarAT128.csv", "at128/1679653308406038376", + "PandarAT128.dat", "hesai", 0, 1.f, 180.f}, + {"PandarQT64", "Dual", "PandarQT64.csv", "qt64/1673401195788312575", "", "hesai", 0, 0.1f, 60.f}, + {"PandarXT32", "Dual", "PandarXT32.csv", "xt32/1673400677802009732", "", "hesai", 0, 0.05f, + 120.f}, + {"PandarXT32M", "LastStrongest", "PandarXT32M.csv", "xt32m/1660893203042895158", "", "hesai", 0, + 0.5f, 300.f}}; // Compares geometrical output of decoder against pre-recorded reference pointcloud. TEST_P(DecoderTest, TestPcd) diff --git a/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.cpp b/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.cpp index b00a9433d..047b2c661 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.cpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.cpp @@ -13,7 +13,9 @@ #include #include +#include #include +#include namespace nebula { @@ -57,7 +59,10 @@ Status VelodyneRosDecoderTest::InitializeDriver( return driver_ptr_->GetStatus(); } -Status VelodyneRosDecoderTest::GetStatus() { return wrapper_status_; } +Status VelodyneRosDecoderTest::GetStatus() +{ + return wrapper_status_; +} Status VelodyneRosDecoderTest::GetParameters( drivers::VelodyneSensorConfiguration & sensor_configuration, @@ -315,10 +320,9 @@ void VelodyneRosDecoderTest::ReadBag() storage_options.uri = bag_path; storage_options.storage_id = storage_id; - converter_options.output_serialization_format = format; //"cdr"; + converter_options.output_serialization_format = format; rclcpp::Serialization serialization; nebula::drivers::NebulaPointCloudPtr pointcloud(new nebula::drivers::NebulaPointCloud); - // nebula::drivers::NebulaPointCloudPtr ref_pointcloud(new nebula::drivers::NebulaPointCloud); pcl::PointCloud::Ptr ref_pointcloud(new pcl::PointCloud); { rosbag2_cpp::Reader bag_reader(std::make_unique()); diff --git a/node_modules/.cache/prettier/.prettier-caches/71fcc0c413aef9f44cc34d45933658dcd90140bf.json b/node_modules/.cache/prettier/.prettier-caches/71fcc0c413aef9f44cc34d45933658dcd90140bf.json new file mode 100644 index 000000000..f1d45e79f --- /dev/null +++ b/node_modules/.cache/prettier/.prettier-caches/71fcc0c413aef9f44cc34d45933658dcd90140bf.json @@ -0,0 +1 @@ +{"89c3ab2f8d568f1de2f4909e98f02c8eb4aa47d7":{"files":{".pre-commit-config.yaml":["IIgVK1mNeWSqgUB4F48/2oXJV2M=",true],"nebula_tests/data/velodyne/vls128/1614315746471294674/metadata.yaml":["rkwp2ScCJvYSp/u0PQ8O9vqff7E=",true],".cspell.json":["qdqZpfVDIuFwYUzZ4njcRyqVt6U=",true]},"modified":1716356405626}} \ No newline at end of file diff --git a/scripts/plot_instrumentation.py b/scripts/plot_instrumentation.py index fe9c12fda..7540aa35f 100755 --- a/scripts/plot_instrumentation.py +++ b/scripts/plot_instrumentation.py @@ -1,77 +1,79 @@ #!/usr/bin/python3 import argparse -from matplotlib import pyplot as plt -import numpy as np -import pandas as pd -import re +from collections import defaultdict import json import os -from collections import defaultdict +import re + +from matplotlib import pyplot as plt +import pandas as pd + def condition_data(log_file: str): - with open(log_file, 'r') as f: - lines = f.readlines() - lines = [re.search(r'(\{"type":.*?"tag":.*?\})', l) for l in lines] - lines = [re.sub(r'([0-9])"', r'\1', l[1]) for l in lines if l] - lines = [json.loads(l) for l in lines if l] + with open(log_file, "r") as f: + lines = f.readlines() + lines = [re.search(r'(\{"type":.*?"tag":.*?\})', line) for line in lines] + lines = [re.sub(r'([0-9])"', r"\1", line[1]) for line in lines if line] + lines = [json.loads(line) for line in lines if line] - cols = defaultdict(list) + cols = defaultdict(list) - for record in lines: - for key in record.keys(): - if key in ["tag", "type"]: - continue + for record in lines: + for key in record.keys(): + if key in ["tag", "type"]: + continue - colname = f"{record['type']}.{key}.{record['tag']}" - cols[colname] += [num for num in record[key] if num is not None] + colname = f"{record['type']}.{key}.{record['tag']}" + cols[colname] += [num for num in record[key] if num is not None] - def quantile_filter(series, quantile): - q = series.quantile(quantile) - return series[series <= q] + def quantile_filter(series, quantile): + q = series.quantile(quantile) + return series[series <= q] - cols = {k: pd.Series(v, name=k) / 1e3 for k, v in cols.items()} - cols = {k: quantile_filter(v, .999) for k, v in cols.items()} + cols = {k: pd.Series(v, name=k) / 1e3 for k, v in cols.items()} + cols = {k: quantile_filter(v, 0.999) for k, v in cols.items()} - for v in cols.values(): - v: pd.Series - v.attrs['file'] = os.path.basename(os.path.splitext(log_file)[0]) + for v in cols.values(): + v: pd.Series + v.attrs["file"] = os.path.basename(os.path.splitext(log_file)[0]) - return cols + return cols -def plot(conditioned_logs): - conditioned_logs = {k: [dic[k] for dic in conditioned_logs] for k in conditioned_logs[0]} +def plot(conditioned_logs): - fig, axs = plt.subplots(1, len(conditioned_logs), figsize=(15, 8), dpi=120) + conditioned_logs = {k: [dic[k] for dic in conditioned_logs] for k in conditioned_logs[0]} - handles, labels = [], [] + fig, axs = plt.subplots(1, len(conditioned_logs), figsize=(15, 8), dpi=120) - for i, k in enumerate(sorted(conditioned_logs.keys())): - k: str - v = conditioned_logs[k] - ax: plt.Axes = axs[i] - art = ax.boxplot(v) - handles.append(art) - labels.append(k) - ax.set_ylabel("dt [µs]") - ax.set_title(k.replace(".", "\n")) - ax.set_xticks([1 + i for i in range(len(v))], [ser.attrs['file'] for ser in v]) - ax.tick_params(axis='x', labelrotation=90) + handles, labels = [], [] + for i, k in enumerate(sorted(conditioned_logs.keys())): + k: str + v = conditioned_logs[k] + ax: plt.Axes = axs[i] + art = ax.boxplot(v) + handles.append(art) + labels.append(k) + ax.set_ylabel("dt [µs]") + ax.set_title(k.replace(".", "\n")) + ax.set_xticks([1 + i for i in range(len(v))], [ser.attrs["file"] for ser in v]) + ax.tick_params(axis="x", labelrotation=90) + fig.tight_layout() + plt.savefig("instrumentation.png") + plt.show() - fig.tight_layout() - plt.savefig("instrumentation.png") - plt.show() def main(args): - conditioned_logs = [condition_data(f) for f in args.log_files] - plot(conditioned_logs) - + conditioned_logs = [condition_data(f) for f in args.log_files] + plot(conditioned_logs) + + if __name__ == "__main__": - parser = argparse.ArgumentParser() - parser.add_argument("log_files", nargs="+") - args = parser.parse_args() + parser = argparse.ArgumentParser() + parser.add_argument("log_files", nargs="+") + args = parser.parse_args() - main(args) \ No newline at end of file + main(args) diff --git a/scripts/plot_times.py b/scripts/plot_times.py index 3cbf93f61..07dbee302 100644 --- a/scripts/plot_times.py +++ b/scripts/plot_times.py @@ -1,11 +1,13 @@ -#%% +# %% +import argparse +import glob import os -import pandas as pd import re -import glob -import argparse + from matplotlib import pyplot as plt +import pandas as pd + def parse_logs(run_name): dfs = [] @@ -15,21 +17,20 @@ def parse_logs(run_name): with open(path) as f: lines = f.readlines() lines = filter(lambda line: "PROFILING" in line, lines) - lines = [re.sub(r'.*PROFILING (\{.*?\}).*', r'\1', line) for line in lines] + lines = [re.sub(r".*PROFILING (\{.*?\}).*", r"\1", line) for line in lines] records = [eval(line) for line in lines] dfs.append(pd.DataFrame(records)) df = pd.concat(dfs) - + for col in [c for c in df.columns if c.startswith("d_")]: df[col] /= 1e6 # ns to ms return df + def plot_timing_comparison(run_names): - scenario_dfs = { - run_name: parse_logs(run_name) for run_name in run_names - } + scenario_dfs = {run_name: parse_logs(run_name) for run_name in run_names} n_cols = sum(1 for c in next(iter(scenario_dfs.values())).columns if c.startswith("d_")) @@ -44,14 +45,16 @@ def plot_timing_comparison(run_names): # ax_d.plot(durations.index, durations, label=label, linestyle='', marker='.') for col in filter(lambda col: col.startswith("n_"), df.columns): - ax_n.plot(df.index, df[col], label=f"{label}::{col}", linestyle='', marker='.', color='black') + ax_n.plot( + df.index, df[col], label=f"{label}::{col}", linestyle="", marker=".", color="black" + ) for col in filter(lambda col: col.startswith("d_"), df.columns): - ax_d.plot(df.index, df[col], label=f"{label}::{col}", linestyle='', marker='.') - + ax_d.plot(df.index, df[col], label=f"{label}::{col}", linestyle="", marker=".") + d_columns = [col for col in df.columns if col.startswith("d_")] n_cols = len(d_columns) for j, col in enumerate(d_columns): - if (i == 0): + if i == 0: boxes[j].set_ylabel(f"{col} (ms)") boxes[j].boxplot(df[col], positions=[i], labels=[label]) @@ -59,13 +62,11 @@ def plot_timing_comparison(run_names): ax_d.set_ylabel("time (ms)") ax_d.set_xlabel("iteration") - df_means = pd.DataFrame({ - label: df.describe().loc["mean"] for label, df in scenario_dfs.items() - }) + df_means = pd.DataFrame( + {label: df.describe().loc["mean"] for label, df in scenario_dfs.items()} + ) - df_stds = pd.DataFrame({ - label: df.describe().loc["std"] for label, df in scenario_dfs.items() - }) + df_stds = pd.DataFrame({label: df.describe().loc["std"] for label, df in scenario_dfs.items()}) df_means = df_means.rename(index=lambda label: f"{label} AVG") df_stds = df_stds.rename(index=lambda label: f"{label} STD") @@ -75,15 +76,23 @@ def plot_timing_comparison(run_names): baseline_name = run_names[0] df_base = df_stat[baseline_name] for label in df_base.index: - df_stat.loc[f"{label} % rel to {baseline_name}", :] = df_stat.loc[label, :] / df_base.loc[label] * 100 + df_stat.loc[f"{label} % rel to {baseline_name}", :] = ( + df_stat.loc[label, :] / df_base.loc[label] * 100 + ) print(df_stat.to_markdown()) plt.show() + + # %% if __name__ == "__main__": parser = argparse.ArgumentParser() - parser.add_argument("run_names", nargs='+', help="The names of the runs to plot, as entered in test_runner.bash (e.g. 'baseline', 'optimized')\nAll files of the form -[0-9].log will be plotted.") + parser.add_argument( + "run_names", + nargs="+", + help="The names of the runs to plot, as entered in test_runner.bash (e.g. 'baseline', 'optimized')\nAll files of the form -[0-9].log will be plotted.", + ) args = parser.parse_args() - plot_timing_comparison(args.run_names) \ No newline at end of file + plot_timing_comparison(args.run_names) From dc1a59cbbdcda59989130142dfa3d4a48c6ecb2c Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 22 May 2024 15:07:30 +0900 Subject: [PATCH 53/57] fix(expected.hpp): revert explicit constructors --- nebula_common/include/nebula_common/util/expected.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nebula_common/include/nebula_common/util/expected.hpp b/nebula_common/include/nebula_common/util/expected.hpp index 2a59122f5..25cb5f83d 100644 --- a/nebula_common/include/nebula_common/util/expected.hpp +++ b/nebula_common/include/nebula_common/util/expected.hpp @@ -83,9 +83,9 @@ struct expected return default_; } - explicit expected(const T & value) { value_ = value; } + expected(const T & value) { value_ = value; } // NOLINT(runtime/explicit) - explicit expected(const E & error) { value_ = error; } + expected(const E & error) { value_ = error; } // NOLINT(runtime/explicit) private: std::variant value_; From 5f3680442426995fba0b1843b25a22c517a25360 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 22 May 2024 15:07:48 +0900 Subject: [PATCH 54/57] chore: remove erroneously added node_modules folder --- .../71fcc0c413aef9f44cc34d45933658dcd90140bf.json | 1 - 1 file changed, 1 deletion(-) delete mode 100644 node_modules/.cache/prettier/.prettier-caches/71fcc0c413aef9f44cc34d45933658dcd90140bf.json diff --git a/node_modules/.cache/prettier/.prettier-caches/71fcc0c413aef9f44cc34d45933658dcd90140bf.json b/node_modules/.cache/prettier/.prettier-caches/71fcc0c413aef9f44cc34d45933658dcd90140bf.json deleted file mode 100644 index f1d45e79f..000000000 --- a/node_modules/.cache/prettier/.prettier-caches/71fcc0c413aef9f44cc34d45933658dcd90140bf.json +++ /dev/null @@ -1 +0,0 @@ -{"89c3ab2f8d568f1de2f4909e98f02c8eb4aa47d7":{"files":{".pre-commit-config.yaml":["IIgVK1mNeWSqgUB4F48/2oXJV2M=",true],"nebula_tests/data/velodyne/vls128/1614315746471294674/metadata.yaml":["rkwp2ScCJvYSp/u0PQ8O9vqff7E=",true],".cspell.json":["qdqZpfVDIuFwYUzZ4njcRyqVt6U=",true]},"modified":1716356405626}} \ No newline at end of file From 18f193a8219774c44573099e5dc57648dc6a2908 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 22 May 2024 15:08:07 +0900 Subject: [PATCH 55/57] chore(gitconfig): exclude node_modules from git --- .gitignore | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.gitignore b/.gitignore index e966ffdfb..f068ec8be 100644 --- a/.gitignore +++ b/.gitignore @@ -29,3 +29,6 @@ site/ # qcreator stuff CMakeLists.txt.user + +# pre-commit +node_modules/ From b2552dc14420cf1969a7ae5ba2f0ca720c3b8112 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 22 May 2024 16:35:44 +0900 Subject: [PATCH 56/57] fix(hesai/decoder_wrapper): store downloaded sensor calibration without colliding filenames --- nebula_ros/src/hesai/decoder_wrapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_ros/src/hesai/decoder_wrapper.cpp b/nebula_ros/src/hesai/decoder_wrapper.cpp index f51378e47..6da62b273 100644 --- a/nebula_ros/src/hesai/decoder_wrapper.cpp +++ b/nebula_ros/src/hesai/decoder_wrapper.cpp @@ -155,7 +155,7 @@ HesaiDecoderWrapper::get_calibration_result_t HesaiDecoderWrapper::GetCalibratio calibration_file_path_from_sensor = calibration_file_path.substr(0, ext_pos); // TODO(mojomex): if multiple different sensors of the same type are used, this will mix up // their calibration data - calibration_file_path_from_sensor += "_from_sensor"; + calibration_file_path_from_sensor += "_from_sensor_" + sensor_cfg_->sensor_ip; calibration_file_path_from_sensor += calibration_file_path.substr(ext_pos, calibration_file_path.size() - ext_pos); } From 11bdda1e5a2725022c75d733c6e4a7a92c42bade Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 22 May 2024 21:40:11 +0900 Subject: [PATCH 57/57] chore(nebula_examples,nebula_tests): remove more dangling comments and improve formatting --- .../hesai/hesai_ros_offline_extract_pcd.hpp | 16 +- ...hesai_ros_offline_extract_bag_pcd_main.cpp | 1 - .../hesai/hesai_ros_offline_extract_pcd.cpp | 195 +++++------------- .../hesai_ros_offline_extract_pcd_main.cpp | 1 - nebula_tests/hesai/hesai_ros_decoder_test.cpp | 1 - 5 files changed, 62 insertions(+), 152 deletions(-) diff --git a/nebula_examples/include/hesai/hesai_ros_offline_extract_pcd.hpp b/nebula_examples/include/hesai/hesai_ros_offline_extract_pcd.hpp index 8e26515bf..4551bd848 100644 --- a/nebula_examples/include/hesai/hesai_ros_offline_extract_pcd.hpp +++ b/nebula_examples/include/hesai/hesai_ros_offline_extract_pcd.hpp @@ -15,18 +15,18 @@ #ifndef NEBULA_HesaiRosOfflineExtractSample_H #define NEBULA_HesaiRosOfflineExtractSample_H -#include "nebula_common/hesai/hesai_common.hpp" -#include "nebula_common/nebula_common.hpp" -#include "nebula_common/nebula_status.hpp" -#include "nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp" -#include "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp" - #include +#include +#include +#include +#include +#include +#include #include #include -#include "pandar_msgs/msg/pandar_packet.hpp" -#include "pandar_msgs/msg/pandar_scan.hpp" +#include +#include #include #include diff --git a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd_main.cpp b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd_main.cpp index f0eed5055..db6a66285 100644 --- a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd_main.cpp +++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd_main.cpp @@ -36,7 +36,6 @@ int main(int argc, char * argv[]) if (driver_status == nebula::Status::OK) { RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Reading Started"); driver_status = hesai_driver->ReadBag(); - // exec.spin(); } else { RCLCPP_ERROR_STREAM(rclcpp::get_logger(node_name), driver_status); } diff --git a/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp b/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp index 8e134f8aa..720236446 100644 --- a/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp +++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp @@ -87,114 +87,34 @@ Status HesaiRosOfflineExtractSample::GetParameters( drivers::HesaiCalibrationConfiguration & calibration_configuration, drivers::HesaiCorrection & correction_configuration) { + auto sensor_model_ = this->declare_parameter("sensor_model", ""); + sensor_configuration.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_); + auto return_mode_ = this->declare_parameter("return_mode", "", param_read_only()); + sensor_configuration.return_mode = + nebula::drivers::ReturnModeFromStringHesai(return_mode_, sensor_configuration.sensor_model); + sensor_configuration.frame_id = + declare_parameter("frame_id", "pandar", param_read_only()); + { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - 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 = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("return_mode", "", descriptor); - sensor_configuration.return_mode = - // nebula::drivers::ReturnModeFromString(this->get_parameter("return_mode").as_string()); - nebula::drivers::ReturnModeFromStringHesai( - this->get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "pandar", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 3; - descriptor.read_only = true; - descriptor.dynamic_typing = false; + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); 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(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("calibration_file", "", descriptor); - calibration_configuration.calibration_file = - this->get_parameter("calibration_file").as_string(); + descriptor.floating_point_range = float_range(0, 360, 0.01); + sensor_configuration.scan_phase = + declare_parameter("scan_phase", 0., param_read_only()); } + + calibration_configuration.calibration_file = + declare_parameter("calibration_file", "", param_read_only()); if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("correction_file", "", descriptor); - correction_file_path = this->get_parameter("correction_file").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("bag_path", "", descriptor); - bag_path = this->get_parameter("bag_path").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("storage_id", "sqlite3", descriptor); - storage_id = this->get_parameter("storage_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("out_path", "", descriptor); - out_path = this->get_parameter("out_path").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("format", "cdr", descriptor); - format = this->get_parameter("format").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("target_topic", "", descriptor); - target_topic = this->get_parameter("target_topic").as_string(); + correction_file_path = declare_parameter("correction_file", "", param_read_only()); } + bag_path = declare_parameter("bag_path", "", param_read_only()); + storage_id = declare_parameter("storage_id", "sqlite3", param_read_only()); + out_path = declare_parameter("out_path", "", param_read_only()); + format = declare_parameter("format", "cdr", param_read_only()); + target_topic = declare_parameter("target_topic", "", param_read_only()); + if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { return Status::INVALID_SENSOR_MODEL; } @@ -254,53 +174,46 @@ Status HesaiRosOfflineExtractSample::ReadBag() if (rcpputils::fs::create_directories(o_dir)) { std::cout << "created: " << o_dir << std::endl; } - // return Status::OK; pcl::PCDWriter writer; storage_options.uri = bag_path; storage_options.storage_id = storage_id; - converter_options.output_serialization_format = format; // "cdr"; - { - rosbag2_cpp::Reader reader(std::make_unique()); - // reader.open(rosbag_directory.string()); - reader.open(storage_options, converter_options); - while (reader.has_next()) { - auto bag_message = reader.read_next(); - - std::cout << "Found topic name " << bag_message->topic_name << std::endl; - - if (bag_message->topic_name == target_topic) { - pandar_msgs::msg::PandarScan extracted_msg; - rclcpp::Serialization serialization; - rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); - serialization.deserialize_message(&extracted_serialized_msg, &extracted_msg); - - // std::cout<<"Found data in topic " << bag_message->topic_name << ": " << - // extracted_test_msg.data << std::endl; - std::cout << "Found data in topic " << bag_message->topic_name << ": " - << bag_message->time_stamp << std::endl; - - // nebula::drivers::NebulaPointCloudPtr pointcloud = - // driver_ptr_->ConvertScanToPointcloud( - // std::make_shared(extracted_msg)); - for (auto & pkt : extracted_msg.packets) { - auto pointcloud_ts = driver_ptr_->ParseCloudPacket( - std::vector(pkt.data.begin(), std::next(pkt.data.begin(), pkt.size))); - auto pointcloud = std::get<0>(pointcloud_ts); - - if (!pointcloud) { - continue; - } - - auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; - // pcl::io::savePCDFileBinary((o_dir / fn).string(), *pointcloud); - writer.writeBinary((o_dir / fn).string(), *pointcloud); - } + converter_options.output_serialization_format = format; + + rosbag2_cpp::Reader reader(std::make_unique()); + reader.open(storage_options, converter_options); + while (reader.has_next()) { + auto bag_message = reader.read_next(); + + std::cout << "Found topic name " << bag_message->topic_name << std::endl; + + if (bag_message->topic_name != target_topic) { + continue; + } + + pandar_msgs::msg::PandarScan extracted_msg; + rclcpp::Serialization serialization; + rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); + serialization.deserialize_message(&extracted_serialized_msg, &extracted_msg); + + std::cout << "Found data in topic " << bag_message->topic_name << ": " + << bag_message->time_stamp << std::endl; + + for (auto & pkt : extracted_msg.packets) { + auto pointcloud_ts = driver_ptr_->ParseCloudPacket( + std::vector(pkt.data.begin(), std::next(pkt.data.begin(), pkt.size))); + auto pointcloud = std::get<0>(pointcloud_ts); + + if (!pointcloud) { + continue; } + + auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; + writer.writeBinary((o_dir / fn).string(), *pointcloud); } - // close on scope exit } + return Status::OK; } diff --git a/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd_main.cpp b/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd_main.cpp index b504605e6..a4839dac0 100644 --- a/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd_main.cpp +++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd_main.cpp @@ -37,7 +37,6 @@ int main(int argc, char * argv[]) if (driver_status == nebula::Status::OK) { RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Reading Started"); driver_status = hesai_driver->ReadBag(); - // exec.spin(); } else { RCLCPP_ERROR_STREAM(rclcpp::get_logger(node_name), driver_status); } diff --git a/nebula_tests/hesai/hesai_ros_decoder_test.cpp b/nebula_tests/hesai/hesai_ros_decoder_test.cpp index 97046e8f8..289e8a711 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test.cpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test.cpp @@ -60,7 +60,6 @@ Status HesaiRosDecoderTest::InitializeDriver( std::shared_ptr sensor_configuration, std::shared_ptr calibration_configuration) { - // driver should be initialized here with proper decoder driver_ptr_ = std::make_shared( std::static_pointer_cast(sensor_configuration), calibration_configuration);