From dd1bbdc145dc6d394ab166d3a2dde3fd419840e7 Mon Sep 17 00:00:00 2001 From: Maximilian Schmeller Date: Thu, 21 Mar 2024 17:01:34 +0900 Subject: [PATCH] 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 | 90 -- .../src/hesai/hesai_decoder_ros_wrapper.cpp | 488 ------- .../hesai/hesai_hw_interface_ros_wrapper.cpp | 510 ------- .../hesai/hesai_hw_monitor_ros_wrapper.cpp | 618 --------- nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 1235 +++++++++++++++++ 12 files changed, 1476 insertions(+), 2034 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 954c81d04..34a4033cc 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 91caca085..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 CloudInterfaceStart of HwInterface) - /// @return Resulting status - Status StreamStart() override; - /// @brief Stop point cloud streaming (not used) - /// @return Resulting status - Status StreamStop() override; - /// @brief Shutdown (not used) - /// @return Resulting status - Status Shutdown() override; - /// @brief Get configurations from ros parameters - /// @param sensor_configuration Output of SensorConfiguration - /// @return Resulting status - Status GetParameters(drivers::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 857f638cd..573723ef1 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,30 +1,36 @@ -#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 namespace nebula { namespace ros { + /// @brief Get parameter from rclcpp::Parameter /// @tparam T /// @param p Parameter from rclcpp parameter callback @@ -44,39 +50,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 @@ -106,6 +179,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 b26193414..71302b746 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 42e96a25e..000000000 --- a/nebula_ros/launch/hesai_launch_component.launch.xml +++ /dev/null @@ -1,90 +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 381a214f0..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_.CloudInterfaceStart(); - } - 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 239f6b8b0..000000000 --- a/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp +++ /dev/null @@ -1,618 +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) - { - 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())); - 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(); - }); - 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 on_timer_status = [this] { OnHesaiStatusTimer(); }; - diagnostics_status_timer_ = std::make_shared>( - this->get_clock(), std::chrono::milliseconds(diag_span_), std::move(on_timer_status), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(diagnostics_status_timer_, cbg_m_); - - if (hw_interface_.UseHttpGetLidarMonitor()) { - diagnostics_updater_.add( - "hesai_voltage", this, &HesaiHwMonitorRosWrapper::HesaiCheckVoltageHttp); - auto on_timer_lidar_monitor = [this] { OnHesaiLidarMonitorTimerHttp(); }; - diagnostics_lidar_monitor_timer_ = - std::make_shared>( - this->get_clock(), std::chrono::milliseconds(diag_span_), std::move(on_timer_lidar_monitor), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(diagnostics_lidar_monitor_timer_, cbg_m2_); - } else { - diagnostics_updater_.add("hesai_voltage", this, &HesaiHwMonitorRosWrapper::HesaiCheckVoltage); - auto on_timer_lidar_monitor = [this] { OnHesaiLidarMonitorTimer(); }; - diagnostics_lidar_monitor_timer_ = - std::make_shared>( - this->get_clock(), std::chrono::milliseconds(diag_span_), std::move(on_timer_lidar_monitor), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(diagnostics_lidar_monitor_timer_, cbg_m2_); - } - - 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()); - } -} - -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()); - } -} - -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()); - } -} - -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)); - 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 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 < 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 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)); - - 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 * 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"); - } -} - - 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..ab13e131e --- /dev/null +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -0,0 +1,1235 @@ +#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())); + 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(); + }); + 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 on_timer_status = [this] { OnHesaiStatusTimer(); }; + diagnostics_status_timer_ = std::make_shared>( + this->get_clock(), std::chrono::milliseconds(diag_span_), std::move(on_timer_status), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(diagnostics_status_timer_, cbg_m_); + + if (hw_interface_.UseHttpGetLidarMonitor()) { + diagnostics_updater_.add( + "hesai_voltage", this, &HesaiRosWrapper::HesaiCheckVoltageHttp); + auto on_timer_lidar_monitor = [this] { OnHesaiLidarMonitorTimerHttp(); }; + diagnostics_lidar_monitor_timer_ = + std::make_shared>( + this->get_clock(), std::chrono::milliseconds(diag_span_), std::move(on_timer_lidar_monitor), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(diagnostics_lidar_monitor_timer_, cbg_m2_); + } else { + diagnostics_updater_.add("hesai_voltage", this, &HesaiRosWrapper::HesaiCheckVoltage); + auto on_timer_lidar_monitor = [this] { OnHesaiLidarMonitorTimer(); }; + diagnostics_lidar_monitor_timer_ = + std::make_shared>( + this->get_clock(), std::chrono::milliseconds(diag_span_), std::move(on_timer_lidar_monitor), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(diagnostics_lidar_monitor_timer_, cbg_m2_); + } + + 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()); + } +} + +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()); + } +} + +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()); + } +} + +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