diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp index 64861d651..65155efb4 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp @@ -68,7 +68,6 @@ struct Packet128E3X : public PacketBase<2, 128, 2, 100> } // namespace hesai_packet -// FIXME(mojomex) support high resolution mode class Pandar128E3X : public HesaiSensor { private: diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp index 57cbe37ed..41cee89b3 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp @@ -20,11 +20,14 @@ typedef Packet128E3X Packet128E4X; } // namespace hesai_packet -// FIXME(mojomex) support high resolution mode +// FIXME(mojomex): +// The OT128 datasheet has entirely different numbers (and more azimuth states). +// With the current sensor version, the numbers from the new datasheet are incorrect +// (clouds do not sync to ToS but ToS+.052s) class Pandar128E4X : public HesaiSensor { private: - enum OperationalState { HIGH_RESOLUTION = 0, STANDARD = 1 }; +enum OperationalState { HIGH_RESOLUTION = 0, STANDARD = 1 }; static constexpr int firing_time_offset_static_ns_[128] = { 49758, 43224, 36690, 30156, 21980, 15446, 8912, 2378, 49758, 43224, 36690, 30156, 2378, diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp index f8d9a4819..21989bf9b 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp @@ -235,40 +235,30 @@ struct HesaiInventory switch (model) { case 0: return "Pandar40P"; - break; case 2: return "Pandar64"; - break; case 3: return "Pandar128"; - break; case 15: return "PandarQT"; - break; case 17: return "Pandar40M"; - break; case 20: return "PandarMind(PM64)"; - break; case 25: return "PandarXT32"; - break; case 26: return "PandarXT16"; - break; case 32: return "QT128C2X"; - break; case 38: return "PandarXT32M"; - break; + case 42: + return "OT128"; case 48: return "PandarAT128"; - break; default: return "Unknown(" + std::to_string(model) + ")"; - break; } } }; diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp index 01081e320..949c2a508 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp @@ -10,13 +10,13 @@ #if (BOOST_VERSION / 100 == 1074) // Boost 1.74 #define BOOST_ALLOW_DEPRECATED_HEADERS #endif +#include "boost_tcp_driver/http_client_driver.hpp" +#include "boost_tcp_driver/tcp_driver.hpp" +#include "boost_udp_driver/udp_driver.hpp" #include "nebula_common/hesai/hesai_common.hpp" #include "nebula_common/hesai/hesai_status.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp" -#include "boost_tcp_driver/http_client_driver.hpp" -#include "boost_tcp_driver/tcp_driver.hpp" -#include "boost_udp_driver/udp_driver.hpp" #include @@ -77,9 +77,12 @@ const uint16_t PANDAR128_E4X_EXTENDED_PACKET_SIZE = 1117; const uint16_t MTU_SIZE = 1500; -const int PTP_LOG_ANNOUNCE_INTERVAL = 1; // Time interval between Announce messages, in units of log seconds (default: 1) -const int PTP_SYNC_INTERVAL = 1; //Time interval between Sync messages, in units of log seconds (default: 1) -const int PTP_LOG_MIN_DELAY_INTERVAL = 0; //Minimum permitted mean time between Delay_Req messages, in units of log seconds (default: 0) +// Time interval between Announce messages, in units of log seconds (default: 1) +const int PTP_LOG_ANNOUNCE_INTERVAL = 1; +// Time interval between Sync messages, in units of log seconds (default: 1) +const int PTP_SYNC_INTERVAL = 1; +// Minimum permitted mean time between Delay_Req messages, in units of log seconds (default: 0) +const int PTP_LOG_MIN_DELAY_INTERVAL = 0; const int HESAI_LIDAR_GPS_CLOCK_SOURCE = 0; const int HESAI_LIDAR_PTP_CLOCK_SOURCE = 1; @@ -90,10 +93,8 @@ class HesaiHwInterface : NebulaHwInterfaceBase private: std::unique_ptr<::drivers::common::IoContext> cloud_io_context_; std::shared_ptr m_owned_ctx; - std::shared_ptr m_owned_ctx_s; std::unique_ptr<::drivers::udp_driver::UdpDriver> cloud_udp_driver_; std::shared_ptr<::drivers::tcp_driver::TcpDriver> tcp_driver_; - std::shared_ptr<::drivers::tcp_driver::TcpDriver> tcp_driver_s_; std::shared_ptr sensor_configuration_; std::shared_ptr calibration_configuration_; size_t azimuth_index_{}; @@ -106,14 +107,6 @@ class HesaiHwInterface : NebulaHwInterfaceBase int prev_phase_{}; - int timeout_ = 2000; - std::timed_mutex tm_; - int tm_fail_cnt = 0; - int tm_fail_cnt_max = 0; - std::timed_mutex tms_; - int tms_fail_cnt = 0; - int tms_fail_cnt_max = 3; - bool wl = false; bool is_solid_state = false; int target_model_no; @@ -134,18 +127,6 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @param str Received string void str_cb(const std::string & str); - /// @brief Lock function during TCP communication - /// @param tm Mutex - /// @param fail_cnt # of failures - /// @param fail_cnt_max # of times to accept failure - /// @param name Confirmation name used in PrintDebug - /// @return Locked - bool CheckLock(std::timed_mutex & tm, int & fail_cnt, const int & fail_cnt_max, std::string name); - /// @brief Unlock function during TCP communication - /// @param tm Mutex - /// @param name Confirmation name used in PrintDebug - void CheckUnlock(std::timed_mutex & tm, std::string name); - std::shared_ptr parent_node_logger; /// @brief Printing the string to RCLCPP_INFO_STREAM /// @param info Target string @@ -160,6 +141,14 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @param bytes Target byte vector void PrintDebug(const std::vector & bytes); + /// @brief Send a PTC request with an optional payload, and return the full response payload. + /// Blocking. + /// @param command_id PTC command number. + /// @param payload Payload bytes of the PTC command. Not including the 8-byte PTC header. + /// @return The returned payload, if successful, or nullptr. + std::shared_ptr> SendReceive( + const uint8_t command_id, const std::vector & payload = {}); + public: /// @brief Constructor HesaiHwInterface(); @@ -168,7 +157,7 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @brief Initializing tcp_driver for TCP communication /// @param setup_sensor Whether to also initialize tcp_driver for sensor configuration /// @return Resulting status - Status InitializeTcpDriver(bool setup_sensor = true); + Status InitializeTcpDriver(); /// @brief Closes the TcpDriver and related resources /// @return Status result Status FinalizeTcpDriver(); @@ -204,427 +193,65 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @return Resulting status Status RegisterScanCallback( std::function)> scan_callback); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION (sync) - /// @param target_tcp_driver TcpDriver used - /// @return Resulting status - Status syncGetLidarCalibration( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function & received_bytes)> bytes_callback); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION (sync) - /// @param bytes_callback callback - /// @param ctx IO Context used - /// @return Resulting status - Status syncGetLidarCalibration( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function str_callback); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION (sync) - /// @param str_callback callback - /// @param ctx IO Context used - /// @return Resulting status - Status syncGetLidarCalibration( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION (sync) - /// @param ctx IO Context used - /// @return Resulting status - Status syncGetLidarCalibration( - std::shared_ptr ctx, - std::function str_callback); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION (sync) - /// @param str_callback callback - /// @return Resulting status - Status syncGetLidarCalibration(std::shared_ptr ctx); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION (sync) - /// @return Resulting status - Status syncGetLidarCalibrationFromSensor( - std::function & received_bytes)> bytes_callback); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION (sync) - /// @param target_tcp_driver TcpDriver used - /// @param bytes_callback callback - /// @return Resulting status - Status syncGetLidarCalibrationFromSensor( - std::function str_callback); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION (sync) - /// @param target_tcp_driver TcpDriver used - /// @param str_callback callback - /// @return Resulting status - Status syncGetLidarCalibrationFromSensor(); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION - /// @param target_tcp_driver TcpDriver used - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetLidarCalibration( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function & received_bytes)> bytes_callback, - bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION - /// @param bytes_callback callback - /// @param ctx IO Context used - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetLidarCalibration( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function str_callback, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION - /// @param str_callback callback - /// @param ctx IO Context used - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetLidarCalibration( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION - /// @param ctx IO Context used - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetLidarCalibration( - std::shared_ptr ctx, - std::function str_callback, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION - /// @param str_callback callback - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetLidarCalibration(std::shared_ptr ctx, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetLidarCalibrationFromSensor( - std::function & received_bytes)> bytes_callback, - bool with_run = true); /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION - /// @param target_tcp_driver TcpDriver used - /// @param bytes_callback callback - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status GetLidarCalibrationFromSensor( - std::function str_callback, bool with_run = true); + std::string GetLidarCalibrationString(); /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION - /// @param target_tcp_driver TcpDriver used - /// @param str_callback callback - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetLidarCalibrationFromSensor(bool with_run = true); - /// @brief Getting data with PTC_COMMAND_PTP_DIAGNOSTICS (PTP STATUS) - /// @param target_tcp_driver TcpDriver used - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status GetPtpDiagStatus( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, bool with_run = true); + std::vector GetLidarCalibrationBytes(); /// @brief Getting data with PTC_COMMAND_PTP_DIAGNOSTICS (PTP STATUS) - /// @param ctx IO Context used - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetPtpDiagStatus(std::shared_ptr ctx, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_PTP_DIAGNOSTICS (PTP STATUS) - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetPtpDiagStatus(bool with_run = true); - /// @brief Getting data with PTC_COMMAND_PTP_DIAGNOSTICS (PTP TLV PORT_DATA_SET) - /// @param target_tcp_driver TcpDriver used - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status GetPtpDiagPort( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, bool with_run = true); + HesaiPtpDiagStatus GetPtpDiagStatus(); /// @brief Getting data with PTC_COMMAND_PTP_DIAGNOSTICS (PTP TLV PORT_DATA_SET) - /// @param ctx IO Context used - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetPtpDiagPort(std::shared_ptr ctx, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_PTP_DIAGNOSTICS (PTP TLV PORT_DATA_SET) - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetPtpDiagPort(bool with_run = true); - /// @brief Getting data with PTC_COMMAND_PTP_DIAGNOSTICS (PTP TLV TIME_STATUS_NP) - /// @param target_tcp_driver TcpDriver used - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetPtpDiagTime( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_PTP_DIAGNOSTICS (PTP TLV TIME_STATUS_NP) - /// @param ctx IO Context used - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status GetPtpDiagTime(std::shared_ptr ctx, bool with_run = true); + HesaiPtpDiagPort GetPtpDiagPort(); /// @brief Getting data with PTC_COMMAND_PTP_DIAGNOSTICS (PTP TLV TIME_STATUS_NP) - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status GetPtpDiagTime(bool with_run = true); + HesaiPtpDiagTime GetPtpDiagTime(); /// @brief Getting data with PTC_COMMAND_PTP_DIAGNOSTICS (PTP TLV GRANDMASTER_SETTINGS_NP) - /// @param target_tcp_driver TcpDriver used - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status GetPtpDiagGrandmaster( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_PTP_DIAGNOSTICS (PTP TLV GRANDMASTER_SETTINGS_NP) - /// @param ctx IO Context used - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetPtpDiagGrandmaster(std::shared_ptr ctx, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_PTP_DIAGNOSTICS (PTP TLV GRANDMASTER_SETTINGS_NP) - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetPtpDiagGrandmaster(bool with_run = true); - - /// @brief Getting data with PTC_COMMAND_GET_INVENTORY_INFO (sync) - /// @param target_tcp_driver TcpDriver used - /// @param callback Callback function for received HesaiInventory - /// @return Resulting status - Status syncGetInventory( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function callback); - /// @brief Getting data with PTC_COMMAND_GET_INVENTORY_INFO (sync) - /// @param target_tcp_driver TcpDriver used - /// @return Resulting status - Status syncGetInventory( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver); - /// @brief Getting data with PTC_COMMAND_GET_INVENTORY_INFO (sync) - /// @param ctx IO Context used - /// @param callback Callback function for received HesaiInventory - /// @return Resulting status - Status syncGetInventory( - std::shared_ptr ctx, - std::function callback); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION (sync) - /// @param ctx IO Context used - /// @return Resulting status - Status syncGetInventory(std::shared_ptr ctx); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION (sync) - /// @param callback Callback function for received HesaiInventory - /// @return Resulting status - Status syncGetInventory(std::function callback); - + HesaiPtpDiagGrandmaster GetPtpDiagGrandmaster(); /// @brief Getting data with PTC_COMMAND_GET_INVENTORY_INFO - /// @param target_tcp_driver TcpDriver used - /// @param callback Callback function for received HesaiInventory - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status GetInventory( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function callback, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_INVENTORY_INFO - /// @param ctx IO Context used - /// @param callback Callback function for received HesaiInventory - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetInventory( - std::shared_ptr ctx, - std::function callback, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_INVENTORY_INFO - /// @param ctx IO Context used - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetInventory(std::shared_ptr ctx, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_INVENTORY_INFO - /// @param callback Callback function for received HesaiInventory - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetInventory(std::function callback, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_INVENTORY_INFO - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetInventory(bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_CONFIG_INFO - /// @param target_tcp_driver TcpDriver used - /// @param callback Callback function for received HesaiConfig - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetConfig( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function callback, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_CONFIG_INFO - /// @param ctx IO Context used - /// @param callback Callback function for received HesaiConfig - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetConfig( - std::shared_ptr ctx, - std::function callback, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_CONFIG_INFO - /// @param ctx IO Context used - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetConfig(std::shared_ptr ctx, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_CONFIG_INFO - /// @param callback Callback function for received HesaiConfig - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetConfig(std::function callback, bool with_run = true); + HesaiInventory GetInventory(); /// @brief Getting data with PTC_COMMAND_GET_CONFIG_INFO - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status GetConfig(bool with_run = true); + HesaiConfig GetConfig(); /// @brief Getting data with PTC_COMMAND_GET_LIDAR_STATUS - /// @param target_tcp_driver TcpDriver used - /// @param callback Callback function for received HesaiLidarStatus - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status GetLidarStatus( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function callback, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_STATUS - /// @param ctx IO Context used - /// @param callback Callback function for received HesaiLidarStatus - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetLidarStatus( - std::shared_ptr ctx, - std::function callback, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_STATUS - /// @param ctx IO Context used - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetLidarStatus(std::shared_ptr ctx, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_STATUS - /// @param callback Callback function for received HesaiLidarStatus - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetLidarStatus( - std::function callback, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_STATUS - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetLidarStatus(bool with_run = true); - /// @brief Setting value with PTC_COMMAND_SET_SPIN_RATE - /// @param target_tcp_driver TcpDriver used - /// @param rpm Spin rate - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetSpinRate( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, uint16_t rpm, - bool with_run = true); + HesaiLidarStatus GetLidarStatus(); /// @brief Setting value with PTC_COMMAND_SET_SPIN_RATE - /// @param ctx IO Context used /// @param rpm Spin rate - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status SetSpinRate( - std::shared_ptr ctx, uint16_t rpm, bool with_run = true); - /// @brief Setting value with PTC_COMMAND_SET_SPIN_RATE - /// @param rpm Spin rate - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetSpinRate(uint16_t rpm, bool with_run = true); + Status SetSpinRate(uint16_t rpm); /// @brief Setting value with PTC_COMMAND_SET_SYNC_ANGLE - /// @param target_tcp_driver TcpDriver used /// @param sync_angle Sync angle enable flag /// @param angle Angle value - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetSyncAngle( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int sync_angle, int angle, - bool with_run = true); - /// @brief Setting value with PTC_COMMAND_SET_SYNC_ANGLE - /// @param ctx IO Context used - /// @param sync_angle Sync angle enable flag - /// @param angle Angle value - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetSyncAngle( - std::shared_ptr ctx, int sync_angle, int angle, bool with_run = true); - /// @brief Setting value with PTC_COMMAND_SET_SYNC_ANGLE - /// @param sync_angle Sync angle enable flag - /// @param angle Angle value - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetSyncAngle(int sync_angle, int angle, bool with_run = true); - /// @brief Setting mode with PTC_COMMAND_SET_TRIGGER_METHOD - /// @param target_tcp_driver TcpDriver used - /// @param trigger_method Trigger method - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetTriggerMethod( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int trigger_method, - bool with_run = true); - /// @brief Setting mode with PTC_COMMAND_SET_TRIGGER_METHOD - /// @param ctx IO Context used - /// @param trigger_method Trigger method - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status SetTriggerMethod( - std::shared_ptr ctx, int trigger_method, bool with_run = true); + Status SetSyncAngle(int sync_angle, int angle); /// @brief Setting mode with PTC_COMMAND_SET_TRIGGER_METHOD /// @param trigger_method Trigger method - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status SetTriggerMethod(int trigger_method, bool with_run = true); + Status SetTriggerMethod(int trigger_method); /// @brief Setting mode with PTC_COMMAND_SET_STANDBY_MODE - /// @param target_tcp_driver TcpDriver used /// @param standby_mode Standby mode - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status SetStandbyMode( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int standby_mode, - bool with_run = true); - /// @brief Setting mode with PTC_COMMAND_SET_STANDBY_MODE - /// @param ctx IO Context used - /// @param standby_mode Standby mode - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetStandbyMode( - std::shared_ptr ctx, int standby_mode, bool with_run = true); - /// @brief Setting mode with PTC_COMMAND_SET_STANDBY_MODE - /// @param standby_mode Standby mode - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetStandbyMode(int standby_mode, bool with_run = true); + Status SetStandbyMode(int standby_mode); /// @brief Setting mode with PTC_COMMAND_SET_RETURN_MODE - /// @param target_tcp_driver TcpDriver used /// @param return_mode Return mode - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status SetReturnMode( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int return_mode, - bool with_run = true); - /// @brief Setting mode with PTC_COMMAND_SET_RETURN_MODE - /// @param ctx IO Context used - /// @param return_mode Return mode - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetReturnMode( - std::shared_ptr ctx, int return_mode, bool with_run = true); - /// @brief Setting mode with PTC_COMMAND_SET_RETURN_MODE - /// @param return_mode Return mode - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetReturnMode(int return_mode, bool with_run = true); + Status SetReturnMode(int return_mode); /// @brief Setting IP with PTC_COMMAND_SET_DESTINATION_IP - /// @param target_tcp_driver TcpDriver used /// @param dest_ip_1 The 1st byte represents the 1st section /// @param dest_ip_2 The 2nd byte represents the 2nd section /// @param dest_ip_3 The 3rd byte represents the 3rd section /// @param dest_ip_4 The 4th byte represents the 4th section /// @param port LiDAR Destination Port /// @param gps_port GPS Destination Port - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status Status SetDestinationIp( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int dest_ip_1, - int dest_ip_2, int dest_ip_3, int dest_ip_4, int port, int gps_port, bool with_run = true); - /// @brief Setting IP with PTC_COMMAND_SET_DESTINATION_IP - /// @param ctx IO Context used - /// @param dest_ip_1 The 1st byte represents the 1st section - /// @param dest_ip_2 The 2nd byte represents the 2nd section - /// @param dest_ip_3 The 3rd byte represents the 3rd section - /// @param dest_ip_4 The 4th byte represents the 4th section - /// @param port LiDAR Destination Port - /// @param gps_port GPS Destination Port - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetDestinationIp( - std::shared_ptr ctx, int dest_ip_1, int dest_ip_2, int dest_ip_3, - int dest_ip_4, int port, int gps_port, bool with_run = true); - /// @brief Setting IP with PTC_COMMAND_SET_DESTINATION_IP - /// @param dest_ip_1 The 1st byte represents the 1st section - /// @param dest_ip_2 The 2nd byte represents the 2nd section - /// @param dest_ip_3 The 3rd byte represents the 3rd section - /// @param dest_ip_4 The 4th byte represents the 4th section - /// @param port LiDAR Destination Port - /// @param gps_port GPS Destination Port - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetDestinationIp( - int dest_ip_1, int dest_ip_2, int dest_ip_3, int dest_ip_4, int port, int gps_port, - bool with_run = true); + int dest_ip_1, int dest_ip_2, int dest_ip_3, int dest_ip_4, int port, int gps_port); /// @brief Setting IP with PTC_COMMAND_SET_CONTROL_PORT - /// @param target_tcp_driver TcpDriver used /// @param ip_1 Device IP of the 1st byte represents the 1st section /// @param ip_2 Device IP of the 2nd byte represents the 2nd section /// @param ip_3 Device IP of the 3rd byte represents the 3rd section @@ -639,140 +266,27 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @param gateway_4 Device gateway of the 4th byte represents the 4th section /// @param vlan_flg VLAN Status /// @param vlan_id VLAN ID - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetControlPort( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int ip_1, int ip_2, - int ip_3, int ip_4, int mask_1, int mask_2, int mask_3, int mask_4, int gateway_1, - int gateway_2, int gateway_3, int gateway_4, int vlan_flg, int vlan_id, bool with_run = true); - /// @brief Setting IP with PTC_COMMAND_SET_CONTROL_PORT - /// @param ctx IO Context used - /// @param ip_1 Device IP of the 1st byte represents the 1st section - /// @param ip_2 Device IP of the 2nd byte represents the 2nd section - /// @param ip_3 Device IP of the 3rd byte represents the 3rd section - /// @param ip_4 Device IP of the 4th byte represents the 4th section - /// @param mask_1 Device subnet mask of the 1st byte represents the 1st section - /// @param mask_2 Device subnet mask of the 2nd byte represents the 2nd section - /// @param mask_3 Device subnet mask of the 3rd byte represents the 3rd section - /// @param mask_4 Device subnet mask of the 4th byte represents the 4th section - /// @param gateway_1 Device gateway of the 1st byte represents the 1st section - /// @param gateway_2 Device gateway of the 2nd byte represents the 2nd section - /// @param gateway_3 Device gateway of the 3rd byte represents the 3rd section - /// @param gateway_4 Device gateway of the 4th byte represents the 4th section - /// @param vlan_flg VLAN Status - /// @param vlan_id VLAN ID - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetControlPort( - std::shared_ptr ctx, int ip_1, int ip_2, int ip_3, int ip_4, - int mask_1, int mask_2, int mask_3, int mask_4, int gateway_1, int gateway_2, int gateway_3, - int gateway_4, int vlan_flg, int vlan_id, bool with_run = true); - /// @brief Setting IP with PTC_COMMAND_SET_CONTROL_PORT - /// @param ip_1 Device IP of the 1st byte represents the 1st section - /// @param ip_2 Device IP of the 2nd byte represents the 2nd section - /// @param ip_3 Device IP of the 3rd byte represents the 3rd section - /// @param ip_4 Device IP of the 4th byte represents the 4th section - /// @param mask_1 Device subnet mask of the 1st byte represents the 1st section - /// @param mask_2 Device subnet mask of the 2nd byte represents the 2nd section - /// @param mask_3 Device subnet mask of the 3rd byte represents the 3rd section - /// @param mask_4 Device subnet mask of the 4th byte represents the 4th section - /// @param gateway_1 Device gateway of the 1st byte represents the 1st section - /// @param gateway_2 Device gateway of the 2nd byte represents the 2nd section - /// @param gateway_3 Device gateway of the 3rd byte represents the 3rd section - /// @param gateway_4 Device gateway of the 4th byte represents the 4th section - /// @param vlan_flg VLAN Status - /// @param vlan_id VLAN ID - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status Status SetControlPort( int ip_1, int ip_2, int ip_3, int ip_4, int mask_1, int mask_2, int mask_3, int mask_4, - int gateway_1, int gateway_2, int gateway_3, int gateway_4, int vlan_flg, int vlan_id, - bool with_run = true); - /// @brief Setting values with PTC_COMMAND_SET_LIDAR_RANGE - /// @param target_tcp_driver TcpDriver used - /// @param method Method - /// @param data Set data - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetLidarRange( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int method, - std::vector data, bool with_run = true); + int gateway_1, int gateway_2, int gateway_3, int gateway_4, int vlan_flg, int vlan_id); /// @brief Setting values with PTC_COMMAND_SET_LIDAR_RANGE - /// @param ctx IO Context used /// @param method Method /// @param data Set data - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetLidarRange( - std::shared_ptr ctx, int method, std::vector data, - bool with_run = true); - /// @brief Setting values with PTC_COMMAND_SET_LIDAR_RANGE - /// @param method Method - /// @param data Set data - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetLidarRange(int method, std::vector data, bool with_run = true); - /// @brief Setting values with PTC_COMMAND_SET_LIDAR_RANGE - /// @param target_tcp_driver TcpDriver used - /// @param start Start angle - /// @param end End angle - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetLidarRange( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int start, int end, - bool with_run = true); - /// @brief Setting values with PTC_COMMAND_SET_LIDAR_RANGE - /// @param ctx IO Context used - /// @param start Start angle - /// @param end End angle - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status SetLidarRange( - std::shared_ptr ctx, int start, int end, bool with_run = true); + Status SetLidarRange(int method, std::vector data); /// @brief Setting values with PTC_COMMAND_SET_LIDAR_RANGE /// @param start Start angle /// @param end End angle - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status SetLidarRange(int start, int end, bool with_run = true); + Status SetLidarRange(int start, int end); /// @brief Getting values with PTC_COMMAND_GET_LIDAR_RANGE - /// @param target_tcp_driver TcpDriver used - /// @param callback Callback function for received HesaiLidarRangeAll - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status GetLidarRange( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function callback, bool with_run = true); - /// @brief Getting values with PTC_COMMAND_GET_LIDAR_RANGE - /// @param ctx IO Context used - /// @param callback Callback function for received HesaiLidarRangeAll - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetLidarRange( - std::shared_ptr ctx, - std::function callback, bool with_run = true); - /// @brief Getting values with PTC_COMMAND_GET_LIDAR_RANGE - /// @param ctx IO Context used - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetLidarRange(std::shared_ptr ctx, bool with_run = true); - /// @brief Getting values with PTC_COMMAND_GET_LIDAR_RANGE - /// @param callback Callback function for received HesaiLidarRangeAll - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetLidarRange( - std::function callback, bool with_run = true); - /// @brief Getting values with PTC_COMMAND_GET_LIDAR_RANGE - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetLidarRange(bool with_run = true); + HesaiLidarRangeAll GetLidarRange(); - Status SetClockSource(std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int clock_source, bool with_run); - Status SetClockSource(std::shared_ptr ctx, int clock_source, bool with_run); - Status SetClockSource(int clock_source, bool with_run = true); + Status SetClockSource(int clock_source); /// @brief Setting values with PTC_COMMAND_SET_PTP_CONFIG - /// @param target_tcp_driver TcpDriver used /// @param profile IEEE timing and synchronization standard /// @param domain Domain attribute of the local clock /// @param network Network transport type of 1588v2 @@ -783,121 +297,23 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// 1) /// @param logMinDelayReqInterval Minimum permitted mean time between Delay_Req messages, in units /// of log seconds (default: 0) - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status Status SetPtpConfig( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int profile, int domain, - int network, int switch_type, int logAnnounceInterval = 1, int logSyncInterval = 1, int logMinDelayReqInterval = 0, - bool with_run = true); - /// @brief Setting values with PTC_COMMAND_SET_PTP_CONFIG - /// @param ctx IO Context used - /// @param profile IEEE timing and synchronization standard - /// @param domain Domain attribute of the local clock - /// @param network Network transport type of 1588v2 - /// @param switch_type Switch type of 802.1AS Automotive - /// @param logAnnounceInterval Time interval between Announce messages, in units of log seconds - /// (default: 1) - /// @param logSyncInterval Time interval between Sync messages, in units of log seconds (default: - /// 1) - /// @param logMinDelayReqInterval Minimum permitted mean time between Delay_Req messages, in units - /// of log seconds (default: 0) - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetPtpConfig( - std::shared_ptr ctx, int profile, int domain, int network, int switch_type, - int logAnnounceInterval = 1, int logSyncInterval = 1, int logMinDelayReqInterval = 0, bool with_run = true); - /// @brief Setting values with PTC_COMMAND_SET_PTP_CONFIG - /// @param profile IEEE timing and synchronization standard - /// @param domain Domain attribute of the local clock - /// @param network Network transport type of 1588v2 - /// @param switch_type Switch type of 802.1AS Automotive - /// @param logAnnounceInterval Time interval between Announce messages, in units of log seconds - /// (default: 1) - /// @param logSyncInterval Time interval between Sync messages, in units of log seconds (default: - /// 1) - /// @param logMinDelayReqInterval Minimum permitted mean time between Delay_Req messages, in units - /// of log seconds (default: 0) - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetPtpConfig( - int profile, int domain, int network, int switch_type, int logAnnounceInterval = 1, int logSyncInterval = 1, - int logMinDelayReqInterval = 0, bool with_run = true); + int profile, int domain, int network, int switch_type, int logAnnounceInterval = 1, + int logSyncInterval = 1, int logMinDelayReqInterval = 0); /// @brief Getting data with PTC_COMMAND_GET_PTP_CONFIG - /// @param target_tcp_driver TcpDriver used - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status GetPtpConfig( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_PTP_CONFIG - /// @param ctx IO Context used - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetPtpConfig(std::shared_ptr ctx, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_PTP_CONFIG - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetPtpConfig(bool with_run = true); + HesaiPtpConfig GetPtpConfig(); /// @brief Sending command with PTC_COMMAND_RESET - /// @param target_tcp_driver TcpDriver used - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SendReset( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, bool with_run = true); - /// @brief Sending command with PTC_COMMAND_RESET - /// @param ctx IO Context used - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SendReset(std::shared_ptr ctx, bool with_run = true); - /// @brief Sending command with PTC_COMMAND_RESET - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SendReset(bool with_run = true); - /// @brief Setting values with PTC_COMMAND_SET_ROTATE_DIRECTION - /// @param target_tcp_driver TcpDriver used - /// @param mode Rotation of the motor - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetRotDir( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int mode, - bool with_run = true); - /// @brief Setting values with PTC_COMMAND_SET_ROTATE_DIRECTION - /// @param ctx IO Context used - /// @param mode Rotation of the motor - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status SetRotDir(std::shared_ptr ctx, int mode, bool with_run = true); + Status SendReset(); /// @brief Setting values with PTC_COMMAND_SET_ROTATE_DIRECTION /// @param mode Rotation of the motor - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status SetRotDir(int mode, bool with_run = true); + Status SetRotDir(int mode); /// @brief Getting data with PTC_COMMAND_LIDAR_MONITOR - /// @param target_tcp_driver TcpDriver used - /// @param callback Callback function for received HesaiLidarMonitor - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status GetLidarMonitor( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function callback, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_LIDAR_MONITOR - /// @param ctx IO Context used - /// @param callback Callback function for received HesaiLidarMonitor - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetLidarMonitor( - std::shared_ptr ctx, - std::function callback, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_LIDAR_MONITOR - /// @param ctx IO Context used - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetLidarMonitor(std::shared_ptr ctx, bool with_run = true); - Status GetLidarMonitor( - std::function callback, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_LIDAR_MONITOR - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetLidarMonitor(bool with_run = true); + HesaiLidarMonitor GetLidarMonitor(); /// @brief Call run() of IO Context void IOContextRun(); @@ -916,27 +332,15 @@ class HesaiHwInterface : NebulaHwInterfaceBase HesaiStatus SetSpinSpeedAsyncHttp(uint16_t rpm); HesaiStatus SetPtpConfigSyncHttp( - std::shared_ptr ctx, - int profile, - int domain, - int network, - int logAnnounceInterval, - int logSyncInterval, + std::shared_ptr ctx, int profile, int domain, int network, + int logAnnounceInterval, int logSyncInterval, int logMinDelayReqInterval); + HesaiStatus SetPtpConfigSyncHttp( + int profile, int domain, int network, int logAnnounceInterval, int logSyncInterval, int logMinDelayReqInterval); - HesaiStatus SetPtpConfigSyncHttp(int profile, - int domain, - int network, - int logAnnounceInterval, - int logSyncInterval, - int logMinDelayReqInterval); HesaiStatus SetSyncAngleSyncHttp( - std::shared_ptr ctx, - int enable, - int angle); + std::shared_ptr ctx, int enable, int angle); HesaiStatus SetSyncAngleSyncHttp(int enable, int angle); - - /// @brief Getting lidar_monitor via HTTP API /// @param ctx IO Context /// @param str_callback Callback function for received string @@ -967,8 +371,8 @@ class HesaiHwInterface : NebulaHwInterfaceBase HesaiStatus CheckAndSetConfig(); /// @brief Convert to model in Hesai protocol from nebula::drivers::SensorModel - /// @param model - /// @return + /// @param model + /// @return int NebulaModelToHesaiModelNo(nebula::drivers::SensorModel model); /// @brief Set target model number (for proper use of HTTP and TCP according to the support of the diff --git a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp index 6be12fc22..b35162439 100644 --- a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp @@ -1,12 +1,14 @@ #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" -//#define WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE +// #define WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE #include #include #endif +#include + namespace nebula { namespace drivers @@ -14,10 +16,8 @@ namespace drivers HesaiHwInterface::HesaiHwInterface() : cloud_io_context_{new ::drivers::common::IoContext(1)}, m_owned_ctx{new boost::asio::io_context(1)}, - m_owned_ctx_s{new boost::asio::io_context(1)}, cloud_udp_driver_{new ::drivers::udp_driver::UdpDriver(*cloud_io_context_)}, tcp_driver_{new ::drivers::tcp_driver::TcpDriver(m_owned_ctx)}, - tcp_driver_s_{new ::drivers::tcp_driver::TcpDriver(m_owned_ctx_s)}, scan_cloud_ptr_{std::make_unique()} { } @@ -26,13 +26,11 @@ HesaiHwInterface::~HesaiHwInterface() #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << ".......................st: HesaiHwInterface::~HesaiHwInterface()" << std::endl; #endif - if(tcp_driver_) - { + if (tcp_driver_) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << ".......................tcp_driver_ is available" << std::endl; #endif - if(tcp_driver_ && tcp_driver_->isOpen()) - { + if (tcp_driver_ && tcp_driver_->isOpen()) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << ".......................st: tcp_driver_->close();" << std::endl; #endif @@ -43,25 +41,6 @@ HesaiHwInterface::~HesaiHwInterface() } #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << ".......................ed: if(tcp_driver_)" << std::endl; -#endif - } - if(tcp_driver_s_) - { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << ".......................tcp_driver_s_ is available" << std::endl; -#endif - if(tcp_driver_s_->isOpen()) - { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << ".......................st: tcp_driver_s_->close();" << std::endl; -#endif - tcp_driver_s_->close(); -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << ".......................ed: tcp_driver_s_->close();" << std::endl; -#endif - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << ".......................ed: if(tcp_driver_s_)" << std::endl; #endif } #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE @@ -69,6 +48,80 @@ HesaiHwInterface::~HesaiHwInterface() #endif } +std::shared_ptr> HesaiHwInterface::SendReceive( + const uint8_t command_id, const std::vector & payload) +{ + uint32_t len = payload.size(); + + std::vector send_buf; + send_buf.emplace_back(PTC_COMMAND_HEADER_HIGH); + send_buf.emplace_back(PTC_COMMAND_HEADER_LOW); + send_buf.emplace_back(command_id); + send_buf.emplace_back(PTC_COMMAND_DUMMY_BYTE); + send_buf.emplace_back((len >> 24) & 0xff); + send_buf.emplace_back((len >> 16) & 0xff); + send_buf.emplace_back((len >> 8) & 0xff); + send_buf.emplace_back(len & 0xff); + send_buf.insert(send_buf.end(), payload.begin(), payload.end()); + + auto recv_buf = std::make_shared>(); + bool success = false; + + std::stringstream ss; + ss << "0x" << std::setfill('0') << std::setw(2) << std::hex << static_cast(command_id) << " (" << len << ") "; + std::string log_tag = ss.str(); + + PrintDebug(log_tag + "Entering lock"); + + std::timed_mutex tm; + tm.lock(); + + if (tcp_driver_->GetIOContext()->stopped()) { + PrintDebug(log_tag + "IOContext was stopped"); + tcp_driver_->GetIOContext()->restart(); + } + + PrintDebug(log_tag + "Sending payload"); + tcp_driver_->asyncSendReceiveHeaderPayload( + send_buf, + [this, log_tag, &success](const std::vector & header_bytes) { + size_t payload_len = (header_bytes[4] << 24) | (header_bytes[5] << 16) | (header_bytes[6] << 8) | header_bytes[7]; + PrintDebug(log_tag + "Received header (expecting " + std::to_string(payload_len) + "B payload)"); + if (payload_len == 0) { success = true; } + }, + [this, log_tag, &recv_buf, &success](const std::vector & payload_bytes) { + PrintDebug(log_tag + "Received payload"); + + // Header had payload length 0 (thus, header callback processed request successfully already), + // but we still received a payload: invalid state + if (success == true) { + throw std::runtime_error("Received payload despite payload length 0 in header"); + } + + // Skip 8 header bytes + recv_buf->insert(recv_buf->end(), std::next(payload_bytes.begin(), 8), payload_bytes.end()); + success = true; + }, + [this, log_tag, &tm]() { + PrintDebug(log_tag + "Unlocking mutex"); + tm.unlock(); + PrintDebug(log_tag + "Unlocked mutex"); + }); + this->IOContextRun(); + if (!tm.try_lock_for(std::chrono::seconds(1))) { + PrintError(log_tag + "Request did not finish within 1s"); + return nullptr; + } + + if (!success) { + PrintError(log_tag + "Did not receive response"); + return nullptr; + } + + PrintDebug(log_tag + "Received response"); + return recv_buf; +} + Status HesaiHwInterface::SetSensorConfiguration( std::shared_ptr sensor_configuration) { @@ -221,7 +274,10 @@ void HesaiHwInterface::ReceiveCloudPacketCallback(const std::vector & b } } } -Status HesaiHwInterface::CloudInterfaceStop() { return Status::ERROR_1; } +Status HesaiHwInterface::CloudInterfaceStop() +{ + return Status::ERROR_1; +} Status HesaiHwInterface::GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) { @@ -238,10 +294,10 @@ Status HesaiHwInterface::GetCalibrationConfiguration( return Status::ERROR_1; } -Status HesaiHwInterface::InitializeTcpDriver(bool setup_sensor) +Status HesaiHwInterface::InitializeTcpDriver() { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "HesaiHwInterface::InitializeTcpDriver, setup_sensor=" << setup_sensor << std::endl; + std::cout << "HesaiHwInterface::InitializeTcpDriver" << std::endl; std::cout << "st: tcp_driver_->init_socket" << std::endl; std::cout << "sensor_configuration_->sensor_ip=" << sensor_configuration_->sensor_ip << std::endl; std::cout << "sensor_configuration_->host_ip=" << sensor_configuration_->host_ip << std::endl; @@ -255,34 +311,20 @@ Status HesaiHwInterface::InitializeTcpDriver(bool setup_sensor) #endif if (!tcp_driver_->open()) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "!tcp_driver_->open()" << std::endl; + std::cout << "!tcp_driver_->open()" << std::endl; #endif -// tcp_driver_->close(); + // tcp_driver_->close(); tcp_driver_->closeSync(); return Status::ERROR_1; } - if (setup_sensor && tcp_driver_s_) { - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - tcp_driver_s_->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - if (!tcp_driver_s_->open()) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "!tcp_driver_s_->open()" << std::endl; -#endif -// tcp_driver_s_->close(); - tcp_driver_s_->closeSync(); - return Status::ERROR_1; - } - } return Status::OK; } -Status HesaiHwInterface::FinalizeTcpDriver() { +Status HesaiHwInterface::FinalizeTcpDriver() +{ try { tcp_driver_->close(); - } - catch(std::exception &e) { + } catch (std::exception & e) { PrintError("Error while finalizing the TcpDriver"); return Status::UDP_CONNECTION_ERROR; } @@ -298,2030 +340,519 @@ boost::property_tree::ptree HesaiHwInterface::ParseJson(const std::string & str) std::cerr << e.what() << std::endl; } return tree; -} - -Status HesaiHwInterface::syncGetLidarCalibration( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function & bytes)> bytes_callback) -{ - std::vector buf_vec; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_GET_LIDAR_CALIBRATION); - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - // if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max, "GetLidarCalibration")) { - // return GetLidarCalibration(target_tcp_driver, with_run); - // } - PrintDebug("syncGetLidarCalibration: start"); - - target_tcp_driver->syncSendReceiveHeaderPayload( - buf_vec, - [this]([[maybe_unused]]const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - PrintDebug(received_bytes); -#endif - }, - [this, target_tcp_driver, bytes_callback](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - - std::cout << "GetLidarCalib getHeader: "; - for (const auto & b : target_tcp_driver->getHeader()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - std::cout << "GetLidarCalib getPayload: "; - for (const auto & b : target_tcp_driver->getPayload()) { - std::cout << static_cast(b); - } - std::cout << std::endl; -#endif - bytes_callback(received_bytes); - }, - [this]() { CheckUnlock(tm_, "GetLidarCalibration"); }); - - return Status::OK; -} -Status HesaiHwInterface::syncGetLidarCalibration( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function str_callback) -{ - return syncGetLidarCalibration( - target_tcp_driver, [this, str_callback](const std::vector & received_bytes) { - std::string calib_string = - std::string(received_bytes.data(), received_bytes.data() + received_bytes.size()); - PrintInfo(calib_string); - str_callback(calib_string); - }); -} -Status HesaiHwInterface::syncGetLidarCalibration( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver) -{ - return syncGetLidarCalibration( - target_tcp_driver, [this](const std::vector & received_bytes) { - std::string calib_string = - std::string(received_bytes.data(), received_bytes.data() + received_bytes.size()); - PrintInfo(calib_string); - }); -} -Status HesaiHwInterface::syncGetLidarCalibration( - std::shared_ptr ctx, - std::function str_callback) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - return syncGetLidarCalibration(tcp_driver_local, str_callback); -} -Status HesaiHwInterface::syncGetLidarCalibration(std::shared_ptr ctx) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - return syncGetLidarCalibration(tcp_driver_local); -} -Status HesaiHwInterface::syncGetLidarCalibrationFromSensor( - std::function & received_bytes)> bytes_callback) -{ - return syncGetLidarCalibration(tcp_driver_, bytes_callback); -} -Status HesaiHwInterface::syncGetLidarCalibrationFromSensor( - std::function str_callback) -{ - return syncGetLidarCalibration( - tcp_driver_, [this, str_callback](const std::vector & received_bytes) { - std::string calib_string = - std::string(received_bytes.data(), received_bytes.data() + received_bytes.size()); - str_callback(calib_string); - }); -} -Status HesaiHwInterface::syncGetLidarCalibrationFromSensor() -{ - return syncGetLidarCalibrationFromSensor([this](const std::string & str) { PrintDebug(str); }); -} - -Status HesaiHwInterface::GetLidarCalibration( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function & bytes)> bytes_callback, bool with_run) -{ - std::vector buf_vec; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_GET_LIDAR_CALIBRATION); - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - PrintDebug("GetLidarCalibration: start"); - - target_tcp_driver->asyncSendReceiveHeaderPayload( - buf_vec, - [this]([[maybe_unused]]const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - PrintDebug(received_bytes); -#endif - }, - [this, target_tcp_driver, bytes_callback](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - - std::cout << "GetLidarCalib getHeader: "; - for (const auto & b : target_tcp_driver->getHeader()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - std::cout << "GetLidarCalib getPayload: "; - for (const auto & b : target_tcp_driver->getPayload()) { - std::cout << static_cast(b); - } - std::cout << std::endl; -#endif - bytes_callback(received_bytes); - }, - [this]() { CheckUnlock(tm_, "GetLidarCalibration"); }); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::GetLidarCalibration: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): GetLidarCalib" << std::endl; -#endif - } - - return Status::WAITING_FOR_SENSOR_RESPONSE; -} -Status HesaiHwInterface::GetLidarCalibration( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function str_callback, bool with_run) -{ - return GetLidarCalibration( - target_tcp_driver, - [this, str_callback](const std::vector & received_bytes) { - std::string calib_string = - std::string(received_bytes.data(), received_bytes.data() + received_bytes.size()); - PrintInfo(calib_string); - str_callback(calib_string); - }, - with_run); -} -Status HesaiHwInterface::GetLidarCalibration( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, bool with_run) -{ - return GetLidarCalibration( - target_tcp_driver, - [this](const std::vector & received_bytes) { - std::string calib_string = - std::string(received_bytes.data(), received_bytes.data() + received_bytes.size()); - PrintInfo(calib_string); - }, - with_run); -} -Status HesaiHwInterface::GetLidarCalibration( - std::shared_ptr ctx, - std::function str_callback, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - return GetLidarCalibration(tcp_driver_local, str_callback, with_run); -} -Status HesaiHwInterface::GetLidarCalibration( - std::shared_ptr ctx, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - return GetLidarCalibration(tcp_driver_local, with_run); -} -Status HesaiHwInterface::GetLidarCalibrationFromSensor( - std::function & received_bytes)> bytes_callback, bool with_run) -{ - if (with_run) { - if (tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } - } - return GetLidarCalibration(tcp_driver_, bytes_callback, with_run); -} -Status HesaiHwInterface::GetLidarCalibrationFromSensor( - std::function str_callback, bool with_run) -{ - if (with_run) { - if (tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } - } - return GetLidarCalibration( - tcp_driver_, - [this, str_callback](const std::vector & received_bytes) { - std::string calib_string = - std::string(received_bytes.data(), received_bytes.data() + received_bytes.size()); - str_callback(calib_string); - }, - with_run); -} -Status HesaiHwInterface::GetLidarCalibrationFromSensor(bool with_run) -{ - return GetLidarCalibrationFromSensor( - [this](const std::string & str) { PrintDebug(str); }, with_run); -} - -Status HesaiHwInterface::GetPtpDiagStatus( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, bool with_run) -{ - std::vector buf_vec; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_PTP_DIAGNOSTICS); // Cmd PTC_COMMAND_PTP_DIAGNOSTICS - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - - buf_vec.emplace_back(PTC_COMMAND_PTP_STATUS); // PTP STATUS - - if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max, "GetPtpDiagStatus")) { - return GetPtpDiagStatus(target_tcp_driver, with_run); - } - PrintDebug("GetPtpDiagStatus: start"); - - target_tcp_driver->asyncSendReceiveHeaderPayload( - buf_vec, - [this](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - }, - [this, target_tcp_driver](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - - std::cout << "GetPtpDiagStatus getHeader: "; - for (const auto & b : target_tcp_driver->getHeader()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - std::cout << "GetPtpDiagStatus getPayload: "; - for (const auto & b : target_tcp_driver->getPayload()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - - auto response = target_tcp_driver->getPayload(); - HesaiPtpDiagStatus hesai_ptp_diag_status{}; - if (8 < response.size()) { - int payload_pos = 8; - hesai_ptp_diag_status.master_offset = static_cast(response[payload_pos++]) << 56; - hesai_ptp_diag_status.master_offset = hesai_ptp_diag_status.master_offset | - static_cast(response[payload_pos++]) << 48; - hesai_ptp_diag_status.master_offset = hesai_ptp_diag_status.master_offset | - static_cast(response[payload_pos++]) << 40; - hesai_ptp_diag_status.master_offset = hesai_ptp_diag_status.master_offset | - static_cast(response[payload_pos++]) << 32; - hesai_ptp_diag_status.master_offset = hesai_ptp_diag_status.master_offset | - static_cast(response[payload_pos++]) << 24; - hesai_ptp_diag_status.master_offset = hesai_ptp_diag_status.master_offset | - static_cast(response[payload_pos++]) << 16; - hesai_ptp_diag_status.master_offset = hesai_ptp_diag_status.master_offset | - static_cast(response[payload_pos++]) << 8; - hesai_ptp_diag_status.master_offset = - hesai_ptp_diag_status.master_offset | static_cast(response[payload_pos++]); - hesai_ptp_diag_status.ptp_state = response[payload_pos++] << 24; - hesai_ptp_diag_status.ptp_state = hesai_ptp_diag_status.ptp_state | response[payload_pos++] - << 16; - hesai_ptp_diag_status.ptp_state = hesai_ptp_diag_status.ptp_state | response[payload_pos++] - << 8; - hesai_ptp_diag_status.ptp_state = hesai_ptp_diag_status.ptp_state | response[payload_pos++]; - hesai_ptp_diag_status.elapsed_millisec = response[payload_pos++] << 24; - hesai_ptp_diag_status.elapsed_millisec = - hesai_ptp_diag_status.elapsed_millisec | response[payload_pos++] << 16; - hesai_ptp_diag_status.elapsed_millisec = - hesai_ptp_diag_status.elapsed_millisec | response[payload_pos++] << 8; - hesai_ptp_diag_status.elapsed_millisec = - hesai_ptp_diag_status.elapsed_millisec | response[payload_pos++]; - - std::stringstream ss; - ss << "HesaiHwInterface::GetPtpDiagStatus: " << hesai_ptp_diag_status; - PrintInfo(ss.str()); - } - }, - [this]() { CheckUnlock(tm_, "GetPtpDiagStatus"); }); - - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::GetPtpDiagStatus: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): GetPtpDiagStatus" << std::endl; -#endif - } - - return Status::WAITING_FOR_SENSOR_RESPONSE; -} -Status HesaiHwInterface::GetPtpDiagStatus( - std::shared_ptr ctx, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - return GetPtpDiagStatus(tcp_driver_local, with_run); -} -Status HesaiHwInterface::GetPtpDiagStatus(bool with_run) -{ - if (with_run) { - if (tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } - } - return GetPtpDiagStatus(tcp_driver_, with_run); -} - -Status HesaiHwInterface::GetPtpDiagPort( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, bool with_run) -{ - std::vector buf_vec; - int len = 1; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_PTP_DIAGNOSTICS); // Cmd PTC_COMMAND_PTP_DIAGNOSTICS - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - - buf_vec.emplace_back(PTC_COMMAND_PTP_PORT_DATA_SET); // PTP TLV PORT_DATA_SET - - if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max, "GetPtpDiagPort")) { - return GetPtpDiagPort(target_tcp_driver, with_run); - } - PrintDebug("GetPtpDiagPort: start"); - - target_tcp_driver->asyncSendReceiveHeaderPayload( - buf_vec, - [this](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - }, - [this, target_tcp_driver](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - - std::cout << "GetPtpDiagPort getHeader: "; - for (const auto & b : target_tcp_driver->getHeader()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - std::cout << "GetPtpDiagPort getPayload: "; - for (const auto & b : target_tcp_driver->getPayload()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - - auto response = target_tcp_driver->getPayload(); - HesaiPtpDiagPort hesai_ptp_diag_port; - if (8 < response.size()) { - int payload_pos = 8; - - for (size_t i = 0; i < hesai_ptp_diag_port.portIdentity.size(); i++) { - hesai_ptp_diag_port.portIdentity[i] = response[payload_pos++]; - } - hesai_ptp_diag_port.portState = static_cast(response[payload_pos++]); - hesai_ptp_diag_port.logMinDelayReqInterval = static_cast(response[payload_pos++]); - hesai_ptp_diag_port.peerMeanPathDelay = static_cast(response[payload_pos++]) - << 56; - hesai_ptp_diag_port.peerMeanPathDelay = - hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) - << 48; - hesai_ptp_diag_port.peerMeanPathDelay = - hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) - << 40; - hesai_ptp_diag_port.peerMeanPathDelay = - hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) - << 32; - hesai_ptp_diag_port.peerMeanPathDelay = - hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) - << 24; - hesai_ptp_diag_port.peerMeanPathDelay = - hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) - << 16; - hesai_ptp_diag_port.peerMeanPathDelay = - hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) - << 8; - hesai_ptp_diag_port.peerMeanPathDelay = - hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]); - hesai_ptp_diag_port.logAnnounceInterval = static_cast(response[payload_pos++]); - hesai_ptp_diag_port.announceReceiptTimeout = static_cast(response[payload_pos++]); - hesai_ptp_diag_port.logSyncInterval = static_cast(response[payload_pos++]); - hesai_ptp_diag_port.delayMechanism = static_cast(response[payload_pos++]); - hesai_ptp_diag_port.logMinPdelayReqInterval = static_cast(response[payload_pos++]); - hesai_ptp_diag_port.versionNumber = static_cast(response[payload_pos++]); - - std::stringstream ss; - ss << "HesaiHwInterface::GetPtpDiagPort: " << hesai_ptp_diag_port; - PrintInfo(ss.str()); - } - }, - [this]() { CheckUnlock(tm_, "GetPtpDiagPort"); }); - - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::GetPtpDiagPort: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): GetPtpDiagPort" << std::endl; -#endif - } - - return Status::WAITING_FOR_SENSOR_RESPONSE; -} -Status HesaiHwInterface::GetPtpDiagPort(std::shared_ptr ctx, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - return GetPtpDiagPort(tcp_driver_local, with_run); -} -Status HesaiHwInterface::GetPtpDiagPort(bool with_run) -{ - if (with_run) { - if (tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } - } - return GetPtpDiagPort(tcp_driver_, with_run); -} - -Status HesaiHwInterface::GetPtpDiagTime( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, bool with_run) -{ - std::vector buf_vec; - int len = 1; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_PTP_DIAGNOSTICS); // Cmd PTC_COMMAND_PTP_DIAGNOSTICS - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - - buf_vec.emplace_back(PTC_COMMAND_PTP_TIME_STATUS_NP); // PTP TLV TIME_STATUS_NP - if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max, "GetPtpDiagTime")) { - return GetPtpDiagTime(target_tcp_driver, with_run); - } - PrintDebug("GetPtpDiagTime: start"); - - target_tcp_driver->asyncSendReceiveHeaderPayload( - buf_vec, - [this](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - }, - [this, target_tcp_driver](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - - std::cout << "GetPtpDiagTime getHeader: "; - for (const auto & b : target_tcp_driver->getHeader()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - std::cout << "GetPtpDiagTime getPayload: "; - for (const auto & b : target_tcp_driver->getPayload()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - - auto response = target_tcp_driver->getPayload(); - HesaiPtpDiagTime hesai_ptp_diag_time; - if (8 < response.size()) { - int payload_pos = 8; - hesai_ptp_diag_time.master_offset = static_cast(response[payload_pos++]) << 56; - hesai_ptp_diag_time.master_offset = - hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 48; - hesai_ptp_diag_time.master_offset = - hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 40; - hesai_ptp_diag_time.master_offset = - hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 32; - hesai_ptp_diag_time.master_offset = - hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 24; - hesai_ptp_diag_time.master_offset = - hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 16; - hesai_ptp_diag_time.master_offset = - hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 8; - hesai_ptp_diag_time.master_offset = - hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]); - hesai_ptp_diag_time.ingress_time = static_cast(response[payload_pos++]) << 56; - hesai_ptp_diag_time.ingress_time = - hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 48; - hesai_ptp_diag_time.ingress_time = - hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 40; - hesai_ptp_diag_time.ingress_time = - hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 32; - hesai_ptp_diag_time.ingress_time = - hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 24; - hesai_ptp_diag_time.ingress_time = - hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 16; - hesai_ptp_diag_time.ingress_time = - hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 8; - hesai_ptp_diag_time.ingress_time = - hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]); - hesai_ptp_diag_time.cumulativeScaledRateOffset = response[payload_pos++] << 24; - hesai_ptp_diag_time.cumulativeScaledRateOffset = - hesai_ptp_diag_time.cumulativeScaledRateOffset | response[payload_pos++] << 16; - hesai_ptp_diag_time.cumulativeScaledRateOffset = - hesai_ptp_diag_time.cumulativeScaledRateOffset | response[payload_pos++] << 8; - hesai_ptp_diag_time.cumulativeScaledRateOffset = - hesai_ptp_diag_time.cumulativeScaledRateOffset | response[payload_pos++]; - hesai_ptp_diag_time.scaledLastGmPhaseChange = response[payload_pos++] << 24; - hesai_ptp_diag_time.scaledLastGmPhaseChange = - hesai_ptp_diag_time.scaledLastGmPhaseChange | response[payload_pos++] << 16; - hesai_ptp_diag_time.scaledLastGmPhaseChange = - hesai_ptp_diag_time.scaledLastGmPhaseChange | response[payload_pos++] << 8; - hesai_ptp_diag_time.scaledLastGmPhaseChange = - hesai_ptp_diag_time.scaledLastGmPhaseChange | response[payload_pos++]; - hesai_ptp_diag_time.gmTimeBaseIndicator = response[payload_pos++] << 8; - hesai_ptp_diag_time.gmTimeBaseIndicator = - hesai_ptp_diag_time.gmTimeBaseIndicator | response[payload_pos++]; - for (size_t i = 0; i < hesai_ptp_diag_time.lastGmPhaseChange.size(); i++) { - hesai_ptp_diag_time.lastGmPhaseChange[i] = response[payload_pos++]; - } - hesai_ptp_diag_time.gmPresent = response[payload_pos++] << 24; - hesai_ptp_diag_time.gmPresent = hesai_ptp_diag_time.gmPresent | response[payload_pos++] - << 16; - hesai_ptp_diag_time.gmPresent = hesai_ptp_diag_time.gmPresent | response[payload_pos++] - << 8; - hesai_ptp_diag_time.gmPresent = hesai_ptp_diag_time.gmPresent | response[payload_pos++]; - hesai_ptp_diag_time.gmIdentity = static_cast(response[payload_pos++]) << 56; - hesai_ptp_diag_time.gmIdentity = - hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 48; - hesai_ptp_diag_time.gmIdentity = - hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 40; - hesai_ptp_diag_time.gmIdentity = - hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 32; - hesai_ptp_diag_time.gmIdentity = - hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 24; - hesai_ptp_diag_time.gmIdentity = - hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 16; - hesai_ptp_diag_time.gmIdentity = - hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 8; - hesai_ptp_diag_time.gmIdentity = - hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]); - - std::stringstream ss; - ss << "HesaiHwInterface::GetPtpDiagTime: " << hesai_ptp_diag_time; - PrintInfo(ss.str()); - } - }, - [this]() { CheckUnlock(tm_, "GetPtpDiagTime"); }); - - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::GetPtpDiagTime: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): GetPtpDiagTime" << std::endl; -#endif - } - - return Status::WAITING_FOR_SENSOR_RESPONSE; -} -Status HesaiHwInterface::GetPtpDiagTime(std::shared_ptr ctx, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - return GetPtpDiagTime(tcp_driver_local, with_run); -} -Status HesaiHwInterface::GetPtpDiagTime(bool with_run) -{ - if (with_run) { - if (tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } - } - return GetPtpDiagTime(tcp_driver_, with_run); -} - -Status HesaiHwInterface::GetPtpDiagGrandmaster( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, bool with_run) -{ - std::vector buf_vec; - int len = 1; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_PTP_DIAGNOSTICS); // Cmd PTC_COMMAND_PTP_DIAGNOSTICS - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - - buf_vec.emplace_back(PTC_COMMAND_PTP_GRANDMASTER_SETTINGS_NP); // PTP TLV GRANDMASTER_SETTINGS_NP - if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max, "GetPtpDiagGrandmaster")) { - return GetPtpDiagGrandmaster(target_tcp_driver, with_run); - } - PrintDebug("GetPtpDiagGrandmaster: start"); - - target_tcp_driver->asyncSendReceiveHeaderPayload( - buf_vec, - [this](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - }, - [this, target_tcp_driver](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - - std::cout << "GetPtpDiagGrandmaster getHeader: "; - for (const auto & b : target_tcp_driver->getHeader()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - std::cout << "GetPtpDiagGrandmaster getPayload: "; - for (const auto & b : target_tcp_driver->getPayload()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - - auto response = target_tcp_driver->getPayload(); - HesaiPtpDiagGrandmaster hesai_ptp_diag_grandmaster; - if (8 < response.size()) { - int payload_pos = 8; - - hesai_ptp_diag_grandmaster.clockQuality = response[payload_pos++] << 24; - hesai_ptp_diag_grandmaster.clockQuality = - hesai_ptp_diag_grandmaster.clockQuality | response[payload_pos++] << 16; - hesai_ptp_diag_grandmaster.clockQuality = - hesai_ptp_diag_grandmaster.clockQuality | response[payload_pos++] << 8; - hesai_ptp_diag_grandmaster.clockQuality = - hesai_ptp_diag_grandmaster.clockQuality | response[payload_pos++]; - hesai_ptp_diag_grandmaster.utc_offset = response[payload_pos++] << 8; - hesai_ptp_diag_grandmaster.utc_offset = - hesai_ptp_diag_grandmaster.utc_offset | response[payload_pos++]; - hesai_ptp_diag_grandmaster.time_flags = static_cast(response[payload_pos++]); - hesai_ptp_diag_grandmaster.time_source = static_cast(response[payload_pos++]); - - std::cout << hesai_ptp_diag_grandmaster << std::endl; - } - }, - [this]() { CheckUnlock(tm_, "GetPtpDiagGrandmaster"); }); - - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::GetPtpDiagGrandmaster: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): GetPtpDiagGrandmaster" << std::endl; -#endif - } - - return Status::WAITING_FOR_SENSOR_RESPONSE; -} -Status HesaiHwInterface::GetPtpDiagGrandmaster( - std::shared_ptr ctx, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - return GetPtpDiagGrandmaster(tcp_driver_local, with_run); -} -Status HesaiHwInterface::GetPtpDiagGrandmaster(bool with_run) -{ - if (with_run) { - if (tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } - } - return GetPtpDiagGrandmaster(tcp_driver_, with_run); -} - -Status HesaiHwInterface::syncGetInventory( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function callback) -{ - std::cout << "Status HesaiHwInterface::syncGetInventory(std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, std::function callback)" << std::endl; - std::vector buf_vec; - int len = 0; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_GET_INVENTORY_INFO); // Cmd PTC_COMMAND_GET_INVENTORY_INFO - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - - if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max, "GetInventory")) { - return syncGetInventory(target_tcp_driver, callback); - } - - // It doesn't work even when the sensor is not connected... - if(!target_tcp_driver){ - PrintError("!target_tcp_driver"); - return Status::ERROR_1; - } - - // It doesn't work even when the sensor is not connected... - if(!target_tcp_driver->isOpen()){ - PrintError("!target_tcp_driver->isOpen()"); - return Status::ERROR_1; - } - - PrintDebug("syncGetInventory: start"); - - target_tcp_driver->syncSendReceiveHeaderPayload( - buf_vec, - [this](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - }, - [this, target_tcp_driver, callback]([[maybe_unused]]const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - - std::cout << "syncGetInventory getHeader: "; - for (const auto & b : target_tcp_driver->getHeader()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - std::cout << "syncGetInventory getPayload: "; - for (const auto & b : target_tcp_driver->getPayload()) { - std::cout << static_cast(b); - } - std::cout << std::endl; -#endif - auto response = target_tcp_driver->getPayload(); - HesaiInventory hesai_inventory; - if (8 < response.size()) { - int payload_pos = 8; - for (size_t i = 0; i < hesai_inventory.sn.size(); i++) { - hesai_inventory.sn[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.date_of_manufacture.size(); i++) { - hesai_inventory.date_of_manufacture[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.mac.size(); i++) { - hesai_inventory.mac[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.sw_ver.size(); i++) { - hesai_inventory.sw_ver[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.hw_ver.size(); i++) { - hesai_inventory.hw_ver[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.control_fw_ver.size(); i++) { - hesai_inventory.control_fw_ver[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.sensor_fw_ver.size(); i++) { - hesai_inventory.sensor_fw_ver[i] = response[payload_pos++]; - } - hesai_inventory.angle_offset = response[payload_pos++] << 8; - hesai_inventory.angle_offset = hesai_inventory.angle_offset | response[payload_pos++]; - hesai_inventory.model = static_cast(response[payload_pos++]); - hesai_inventory.motor_type = static_cast(response[payload_pos++]); - hesai_inventory.num_of_lines = static_cast(response[payload_pos++]); - for (size_t i = 0; i < hesai_inventory.reserved.size(); i++) { - hesai_inventory.reserved[i] = static_cast(response[payload_pos++]); - } - callback(hesai_inventory); - } - }, - [this]() { CheckUnlock(tm_, "syncGetInventory"); }); - - return Status::OK; -} -Status HesaiHwInterface::syncGetInventory( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver) -{ - return syncGetInventory(target_tcp_driver, - [this](HesaiInventory & result) { std::cout << result << std::endl; }); -} -Status HesaiHwInterface::syncGetInventory( - std::shared_ptr ctx, - std::function callback) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - return syncGetInventory(tcp_driver_local, callback); -} -Status HesaiHwInterface::syncGetInventory(std::shared_ptr ctx) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - return syncGetInventory(tcp_driver_local); -} -Status HesaiHwInterface::syncGetInventory(std::function callback) -{ - return syncGetInventory( - tcp_driver_, [this, callback](HesaiInventory & result) { - callback(result); - }); -} - -Status HesaiHwInterface::GetInventory( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function callback, bool with_run) -{ - std::vector buf_vec; - int len = 0; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_GET_INVENTORY_INFO); // Cmd PTC_COMMAND_GET_INVENTORY_INFO - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - - if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max, "GetInventory")) { - return GetInventory(target_tcp_driver, callback, with_run); - } - PrintDebug("GetInventory: start"); - - target_tcp_driver->asyncSendReceiveHeaderPayload( - buf_vec, - [this](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - }, - [this, target_tcp_driver, callback](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - - std::cout << "GetInventory getHeader: "; - for (const auto & b : target_tcp_driver->getHeader()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - std::cout << "GetInventory getPayload: "; - for (const auto & b : target_tcp_driver->getPayload()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - - auto response = target_tcp_driver->getPayload(); - HesaiInventory hesai_inventory; - if (8 < response.size()) { - int payload_pos = 8; - for (size_t i = 0; i < hesai_inventory.sn.size(); i++) { - hesai_inventory.sn[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.date_of_manufacture.size(); i++) { - hesai_inventory.date_of_manufacture[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.mac.size(); i++) { - hesai_inventory.mac[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.sw_ver.size(); i++) { - hesai_inventory.sw_ver[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.hw_ver.size(); i++) { - hesai_inventory.hw_ver[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.control_fw_ver.size(); i++) { - hesai_inventory.control_fw_ver[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.sensor_fw_ver.size(); i++) { - hesai_inventory.sensor_fw_ver[i] = response[payload_pos++]; - } - hesai_inventory.angle_offset = response[payload_pos++] << 8; - hesai_inventory.angle_offset = hesai_inventory.angle_offset | response[payload_pos++]; - hesai_inventory.model = static_cast(response[payload_pos++]); - hesai_inventory.motor_type = static_cast(response[payload_pos++]); - hesai_inventory.num_of_lines = static_cast(response[payload_pos++]); - for (size_t i = 0; i < hesai_inventory.reserved.size(); i++) { - hesai_inventory.reserved[i] = static_cast(response[payload_pos++]); - } - callback(hesai_inventory); - } - }, - [this]() { CheckUnlock(tm_, "GetInventory"); }); - - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::GetInventory: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): GetInventory" << std::endl; -#endif - } - - return Status::WAITING_FOR_SENSOR_RESPONSE; -} -Status HesaiHwInterface::GetInventory( - std::shared_ptr ctx, - std::function callback, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return GetInventory(tcp_driver_local, callback, with_run); -} -Status HesaiHwInterface::GetInventory(std::shared_ptr ctx, bool with_run) -{ - return GetInventory( - ctx, [this](HesaiInventory & result) { std::cout << result << std::endl; }, with_run); -} -Status HesaiHwInterface::GetInventory( - std::function callback, bool with_run) -{ - if (with_run) { - if (tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } - } - return GetInventory(tcp_driver_, callback, with_run); -} -Status HesaiHwInterface::GetInventory(bool with_run) -{ - return GetInventory( - [this](HesaiInventory & result) { std::cout << result << std::endl; }, with_run); -} - -Status HesaiHwInterface::GetConfig( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function callback, bool with_run) -{ - std::vector buf_vec; - int len = 0; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_GET_CONFIG_INFO); // Cmd PTC_COMMAND_GET_CONFIG_INFO - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max, "GetConfig")) { - return GetConfig(target_tcp_driver, callback, with_run); - } - PrintDebug("GetConfig: start"); - - target_tcp_driver->asyncSendReceiveHeaderPayload( - buf_vec, - [this](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - }, - [this, target_tcp_driver, callback](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - - std::cout << "GetConfig getHeader: "; - for (const auto & b : target_tcp_driver->getHeader()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - std::cout << "GetConfig getPayload: "; - for (const auto & b : target_tcp_driver->getPayload()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - - auto response = target_tcp_driver->getPayload(); - HesaiConfig hesai_config; - if (8 < response.size()) { - int payload_pos = 8; - hesai_config.ipaddr[0] = static_cast(response[payload_pos++]); - hesai_config.ipaddr[1] = static_cast(response[payload_pos++]); - hesai_config.ipaddr[2] = static_cast(response[payload_pos++]); - hesai_config.ipaddr[3] = static_cast(response[payload_pos++]); - hesai_config.mask[0] = static_cast(response[payload_pos++]); - hesai_config.mask[1] = static_cast(response[payload_pos++]); - hesai_config.mask[2] = static_cast(response[payload_pos++]); - hesai_config.mask[3] = static_cast(response[payload_pos++]); - hesai_config.gateway[0] = static_cast(response[payload_pos++]); - hesai_config.gateway[1] = static_cast(response[payload_pos++]); - hesai_config.gateway[2] = static_cast(response[payload_pos++]); - hesai_config.gateway[3] = static_cast(response[payload_pos++]); - hesai_config.dest_ipaddr[0] = static_cast(response[payload_pos++]); - hesai_config.dest_ipaddr[1] = static_cast(response[payload_pos++]); - hesai_config.dest_ipaddr[2] = static_cast(response[payload_pos++]); - hesai_config.dest_ipaddr[3] = static_cast(response[payload_pos++]); - hesai_config.dest_LiDAR_udp_port = response[payload_pos++] << 8; - hesai_config.dest_LiDAR_udp_port = - hesai_config.dest_LiDAR_udp_port | response[payload_pos++]; - hesai_config.dest_gps_udp_port = response[payload_pos++] << 8; - hesai_config.dest_gps_udp_port = hesai_config.dest_gps_udp_port | response[payload_pos++]; - hesai_config.spin_rate = response[payload_pos++] << 8; - hesai_config.spin_rate = hesai_config.spin_rate | response[payload_pos++]; - hesai_config.sync = static_cast(response[payload_pos++]); - hesai_config.sync_angle = response[payload_pos++] << 8; - hesai_config.sync_angle = hesai_config.sync_angle | response[payload_pos++]; - hesai_config.start_angle = response[payload_pos++] << 8; - hesai_config.start_angle = hesai_config.start_angle | response[payload_pos++]; - hesai_config.stop_angle = response[payload_pos++] << 8; - hesai_config.stop_angle = hesai_config.stop_angle | response[payload_pos++]; - hesai_config.clock_source = static_cast(response[payload_pos++]); - hesai_config.udp_seq = static_cast(response[payload_pos++]); - hesai_config.trigger_method = static_cast(response[payload_pos++]); - hesai_config.return_mode = static_cast(response[payload_pos++]); - hesai_config.standby_mode = static_cast(response[payload_pos++]); - hesai_config.motor_status = static_cast(response[payload_pos++]); - hesai_config.vlan_flag = static_cast(response[payload_pos++]); - hesai_config.vlan_id = response[payload_pos++] << 8; - hesai_config.vlan_id = hesai_config.vlan_id | response[payload_pos++]; - hesai_config.clock_data_fmt = static_cast(response[payload_pos++]); - hesai_config.noise_filtering = static_cast(response[payload_pos++]); - hesai_config.reflectivity_mapping = static_cast(response[payload_pos++]); - hesai_config.reserved[0] = static_cast(response[payload_pos++]); - hesai_config.reserved[1] = static_cast(response[payload_pos++]); - hesai_config.reserved[2] = static_cast(response[payload_pos++]); - hesai_config.reserved[3] = static_cast(response[payload_pos++]); - hesai_config.reserved[4] = static_cast(response[payload_pos++]); - hesai_config.reserved[5] = static_cast(response[payload_pos++]); - - callback(hesai_config); - } - }, - [this]() { CheckUnlock(tm_, "GetConfig"); }); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::GetConfig: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): GetConfig" << std::endl; -#endif - } - - return Status::WAITING_FOR_SENSOR_RESPONSE; -} -Status HesaiHwInterface::GetConfig( - std::shared_ptr ctx, std::function callback, - bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return GetConfig(tcp_driver_local, callback, with_run); -} -Status HesaiHwInterface::GetConfig(std::shared_ptr ctx, bool with_run) -{ - return GetConfig( - ctx, [this](HesaiConfig & result) { std::cout << result << std::endl; }, with_run); -} -Status HesaiHwInterface::GetConfig( - std::function callback, bool with_run) -{ - if (with_run) { - if (tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } - } - return GetConfig(tcp_driver_, callback, with_run); -} -Status HesaiHwInterface::GetConfig(bool with_run) -{ - return GetConfig([this](HesaiConfig & result) { std::cout << result << std::endl; }, with_run); -} - -Status HesaiHwInterface::GetLidarStatus( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function callback, bool with_run) -{ - std::vector buf_vec; - int len = 0; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_GET_LIDAR_STATUS); // Cmd PTC_COMMAND_GET_LIDAR_STATUS - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - - if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max, "GetLidarStatus")) { - return GetLidarStatus(target_tcp_driver, callback, with_run); - } - PrintDebug("GetLidarStatus: start"); - - target_tcp_driver->asyncSendReceiveHeaderPayload( - buf_vec, - [this](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - }, - [this, target_tcp_driver, callback](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - - std::cout << "GetLidarStatus getHeader: "; - for (const auto & b : target_tcp_driver->getHeader()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - std::cout << "GetLidarStatus getPayload: "; - for (const auto & b : target_tcp_driver->getPayload()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - - auto response = target_tcp_driver->getPayload(); - HesaiLidarStatus hesai_status; - if (8 < response.size()) { - int payload_pos = 8; - hesai_status.system_uptime = response[payload_pos++] << 24; - hesai_status.system_uptime = hesai_status.system_uptime | response[payload_pos++] << 16; - hesai_status.system_uptime = hesai_status.system_uptime | response[payload_pos++] << 8; - hesai_status.system_uptime = hesai_status.system_uptime | response[payload_pos++]; - hesai_status.motor_speed = response[payload_pos++] << 8; - hesai_status.motor_speed = hesai_status.motor_speed | response[payload_pos++]; - for (size_t i = 0; i < hesai_status.temperature.size(); i++) { - hesai_status.temperature[i] = response[payload_pos++] << 24; - hesai_status.temperature[i] = hesai_status.temperature[i] | response[payload_pos++] << 16; - hesai_status.temperature[i] = hesai_status.temperature[i] | response[payload_pos++] << 8; - hesai_status.temperature[i] = hesai_status.temperature[i] | response[payload_pos++]; - } - hesai_status.gps_pps_lock = static_cast(response[payload_pos++]); - hesai_status.gps_gprmc_status = static_cast(response[payload_pos++]); - hesai_status.startup_times = response[payload_pos++] << 24; - hesai_status.startup_times = hesai_status.startup_times | response[payload_pos++] << 16; - hesai_status.startup_times = hesai_status.startup_times | response[payload_pos++] << 8; - hesai_status.startup_times = hesai_status.startup_times | response[payload_pos++]; - hesai_status.total_operation_time = response[payload_pos++] << 24; - hesai_status.total_operation_time = - hesai_status.total_operation_time | response[payload_pos++] << 16; - hesai_status.total_operation_time = - hesai_status.total_operation_time | response[payload_pos++] << 8; - hesai_status.total_operation_time = - hesai_status.total_operation_time | response[payload_pos++]; - hesai_status.ptp_clock_status = static_cast(response[payload_pos++]); - for (size_t i = 0; i < hesai_status.reserved.size(); i++) { - hesai_status.reserved[i] = static_cast(response[payload_pos++]); - } - - callback(hesai_status); - } - }, - [this]() { CheckUnlock(tm_, "GetLidarStatus"); }); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::GetLidarStatus: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): GetLidarStatus" << std::endl; -#endif - } - - return Status::WAITING_FOR_SENSOR_RESPONSE; -} -Status HesaiHwInterface::GetLidarStatus( - std::shared_ptr ctx, - std::function callback, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return GetLidarStatus(tcp_driver_local, callback, with_run); -} -Status HesaiHwInterface::GetLidarStatus(std::shared_ptr ctx, bool with_run) -{ - return GetLidarStatus( - ctx, [this](HesaiLidarStatus & result) { std::cout << result << std::endl; }, with_run); -} -Status HesaiHwInterface::GetLidarStatus( - std::function callback, bool with_run) -{ -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "GetLidarStatus tcp_driver_->GetIOContext()->stopped()=" - << tcp_driver_->GetIOContext()->stopped() << std::endl; -#endif - if (with_run) { - if (tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } - } - return GetLidarStatus(tcp_driver_, callback, with_run); -} -Status HesaiHwInterface::GetLidarStatus(bool with_run) -{ - return GetLidarStatus( - [this](HesaiLidarStatus & result) { std::cout << result << std::endl; }, with_run); -} - -Status HesaiHwInterface::SetSpinRate( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, uint16_t rpm, bool with_run) -{ - std::vector buf_vec; - int len = 2; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_SET_SPIN_RATE); // Cmd PTC_COMMAND_SET_SPIN_RATE - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - - buf_vec.emplace_back((rpm >> 8) & 0xff); - buf_vec.emplace_back((rpm >> 0) & 0xff); - - if (!CheckLock(tms_, tms_fail_cnt, tms_fail_cnt_max, "SetSpinRate")) { - return SetSpinRate(target_tcp_driver, rpm, with_run); - } - PrintDebug("SetSpinRate: start"); - - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tms_, "SetSpinRate"); }); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::SetSpinRate: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): SetSpinRate" << std::endl; -#endif - } - - return Status::WAITING_FOR_SENSOR_RESPONSE; -} -Status HesaiHwInterface::SetSpinRate( - std::shared_ptr ctx, uint16_t rpm, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return SetSpinRate(tcp_driver_local, rpm, with_run); -} -Status HesaiHwInterface::SetSpinRate(uint16_t rpm, bool with_run) -{ - if (with_run) { - if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { - tcp_driver_s_->GetIOContext()->restart(); - } - } - return SetSpinRate(tcp_driver_s_, rpm, with_run); -} - -Status HesaiHwInterface::SetSyncAngle( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int sync_angle, int angle, - bool with_run) -{ - std::vector buf_vec; - int len = 3; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_SET_SYNC_ANGLE); // Cmd PTC_COMMAND_SET_SYNC_ANGLE - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - - buf_vec.emplace_back((sync_angle >> 0) & 0xff); - buf_vec.emplace_back((angle >> 8) & 0xff); - buf_vec.emplace_back((angle >> 0) & 0xff); - - if (!CheckLock(tms_, tms_fail_cnt, tms_fail_cnt_max, "SetSyncAngle")) { - return SetSyncAngle(target_tcp_driver, sync_angle, angle, with_run); - } - PrintDebug("SetSyncAngle: start"); - - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tms_, "SetSyncAngle"); }); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::SetSyncAngle: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): SetSyncAngle" << std::endl; -#endif - } - - return Status::WAITING_FOR_SENSOR_RESPONSE; -} -Status HesaiHwInterface::SetSyncAngle( - std::shared_ptr ctx, int sync_angle, int angle, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return SetSyncAngle(tcp_driver_local, sync_angle, angle, with_run); -} -Status HesaiHwInterface::SetSyncAngle(int sync_angle, int angle, bool with_run) -{ - if (with_run) { - if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { - tcp_driver_s_->GetIOContext()->restart(); - } - } - return SetSyncAngle(tcp_driver_s_, sync_angle, angle, with_run); -} - -Status HesaiHwInterface::SetTriggerMethod( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int trigger_method, - bool with_run) -{ - std::vector buf_vec; - int len = 1; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_SET_TRIGGER_METHOD); // Cmd PTC_COMMAND_SET_TRIGGER_METHOD - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - - buf_vec.emplace_back((trigger_method >> 0) & 0xff); - - if (!CheckLock(tms_, tms_fail_cnt, tms_fail_cnt_max, "SetTriggerMethod")) { - return SetTriggerMethod(target_tcp_driver, trigger_method, with_run); - } - PrintDebug("SetTriggerMethod: start"); - - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tms_, "SetTriggerMethod"); }); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::SetTriggerMethod: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): SetTriggerMethod" << std::endl; -#endif - } - - return Status::WAITING_FOR_SENSOR_RESPONSE; -} -Status HesaiHwInterface::SetTriggerMethod( - std::shared_ptr ctx, int trigger_method, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return SetTriggerMethod(tcp_driver_local, trigger_method, with_run); -} -Status HesaiHwInterface::SetTriggerMethod(int trigger_method, bool with_run) -{ - if (with_run) { - if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { - tcp_driver_s_->GetIOContext()->restart(); - } - } - return SetTriggerMethod(tcp_driver_s_, trigger_method, with_run); -} - -Status HesaiHwInterface::SetStandbyMode( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int standby_mode, - bool with_run) -{ - std::vector buf_vec; - int len = 1; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_SET_STANDBY_MODE); // Cmd PTC_COMMAND_SET_STANDBY_MODE - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - - buf_vec.emplace_back((standby_mode >> 0) & 0xff); - if (!CheckLock(tms_, tms_fail_cnt, tms_fail_cnt_max, "SetStandbyMode")) { - return SetStandbyMode(target_tcp_driver, standby_mode, with_run); - } - std::cout << "start: SetStandbyMode" << std::endl; - - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tms_, "SetStandbyMode"); }); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::SetStandbyMode: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): SetStandbyMode" << std::endl; -#endif - } - - return Status::WAITING_FOR_SENSOR_RESPONSE; -} -Status HesaiHwInterface::SetStandbyMode( - std::shared_ptr ctx, int standby_mode, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return SetStandbyMode(tcp_driver_local, standby_mode, with_run); -} -Status HesaiHwInterface::SetStandbyMode(int standby_mode, bool with_run) -{ - if (with_run) { - if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { - tcp_driver_s_->GetIOContext()->restart(); - } - } - return SetStandbyMode(tcp_driver_s_, standby_mode, with_run); -} - -Status HesaiHwInterface::SetReturnMode( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int return_mode, - bool with_run) -{ - std::vector buf_vec; - int len = 1; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_SET_RETURN_MODE); // Cmd PTC_COMMAND_SET_RETURN_MODE - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - - buf_vec.emplace_back((return_mode >> 0) & 0xff); - PrintDebug("SetReturnMode: start" + std::to_string(return_mode)); - if (!CheckLock(tms_, tms_fail_cnt, tms_fail_cnt_max, "SetReturnMode")) { - return SetReturnMode(target_tcp_driver, return_mode, with_run); - } - PrintDebug("SetReturnMode: asyncSend"); - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tms_, "SetReturnMode"); }); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::SetReturnMode: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): SetReturnMode" << std::endl; -#endif - } - PrintDebug("SetReturnMode: done"); - return Status::WAITING_FOR_SENSOR_RESPONSE; -} -Status HesaiHwInterface::SetReturnMode( - std::shared_ptr ctx, int return_mode, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return SetReturnMode(tcp_driver_local, return_mode, with_run); -} -Status HesaiHwInterface::SetReturnMode(int return_mode, bool with_run) -{ - //* - if (with_run) { - if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { - tcp_driver_s_->GetIOContext()->restart(); - } - } - //*/ - return SetReturnMode(tcp_driver_s_, return_mode, with_run); -} - -Status HesaiHwInterface::SetDestinationIp( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int dest_ip_1, int dest_ip_2, - int dest_ip_3, int dest_ip_4, int port, int gps_port, bool with_run) -{ - std::vector buf_vec; - int len = 8; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_SET_DESTINATION_IP); // Cmd PTC_COMMAND_SET_DESTINATION_IP - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - - buf_vec.emplace_back((dest_ip_1 >> 0) & 0xff); - buf_vec.emplace_back((dest_ip_2 >> 0) & 0xff); - buf_vec.emplace_back((dest_ip_3 >> 0) & 0xff); - buf_vec.emplace_back((dest_ip_4 >> 0) & 0xff); - buf_vec.emplace_back((port >> 8) & 0xff); - buf_vec.emplace_back((port >> 0) & 0xff); - buf_vec.emplace_back((gps_port >> 8) & 0xff); - buf_vec.emplace_back((gps_port >> 0) & 0xff); - - if (!CheckLock(tms_, tms_fail_cnt, tms_fail_cnt_max, "SetDestinationIp")) { - return SetDestinationIp( - target_tcp_driver, dest_ip_1, dest_ip_2, dest_ip_3, dest_ip_4, port, gps_port, with_run); - } - PrintDebug("SetDestinationIp: start"); - - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tms_, "SetDestinationIp"); }); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::SetDestinationIp: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): SetDestinationIp" << std::endl; -#endif - } - - return Status::WAITING_FOR_SENSOR_RESPONSE; -} -Status HesaiHwInterface::SetDestinationIp( - std::shared_ptr ctx, int dest_ip_1, int dest_ip_2, int dest_ip_3, - int dest_ip_4, int port, int gps_port, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return SetDestinationIp( - tcp_driver_local, dest_ip_1, dest_ip_2, dest_ip_3, dest_ip_4, port, gps_port, with_run); -} -Status HesaiHwInterface::SetDestinationIp( - int dest_ip_1, int dest_ip_2, int dest_ip_3, int dest_ip_4, int port, int gps_port, bool with_run) -{ - if (with_run) { - if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { - tcp_driver_s_->GetIOContext()->restart(); - } - } - return SetDestinationIp( - tcp_driver_s_, dest_ip_1, dest_ip_2, dest_ip_3, dest_ip_4, port, gps_port, with_run); -} - -Status HesaiHwInterface::SetControlPort( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int ip_1, int ip_2, int ip_3, - int ip_4, int mask_1, int mask_2, int mask_3, int mask_4, int gateway_1, int gateway_2, - int gateway_3, int gateway_4, int vlan_flg, int vlan_id, bool with_run) -{ - std::vector buf_vec; - int len = 15; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_SET_CONTROL_PORT); // Cmd PTC_COMMAND_SET_CONTROL_PORT - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - - buf_vec.emplace_back((ip_1 >> 0) & 0xff); - buf_vec.emplace_back((ip_2 >> 0) & 0xff); - buf_vec.emplace_back((ip_3 >> 0) & 0xff); - buf_vec.emplace_back((ip_4 >> 0) & 0xff); - buf_vec.emplace_back((mask_1 >> 0) & 0xff); - buf_vec.emplace_back((mask_2 >> 0) & 0xff); - buf_vec.emplace_back((mask_3 >> 0) & 0xff); - buf_vec.emplace_back((mask_4 >> 0) & 0xff); - buf_vec.emplace_back((gateway_1 >> 0) & 0xff); - buf_vec.emplace_back((gateway_2 >> 0) & 0xff); - buf_vec.emplace_back((gateway_3 >> 0) & 0xff); - buf_vec.emplace_back((gateway_4 >> 0) & 0xff); - buf_vec.emplace_back((vlan_flg >> 0) & 0xff); - buf_vec.emplace_back((vlan_id >> 8) & 0xff); - buf_vec.emplace_back((vlan_id >> 0) & 0xff); - - if (!CheckLock(tms_, tms_fail_cnt, tms_fail_cnt_max, "SetControlPort")) { - return SetControlPort( - target_tcp_driver, ip_1, ip_2, ip_3, ip_4, mask_1, mask_2, mask_3, mask_4, gateway_1, - gateway_2, gateway_3, gateway_4, vlan_flg, vlan_id, with_run); - } - PrintDebug("SetControlPort: start"); - - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tms_, "SetControlPort"); }); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::SetControlPort: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): SetControlPort" << std::endl; -#endif - } - - return Status::WAITING_FOR_SENSOR_RESPONSE; -} -Status HesaiHwInterface::SetControlPort( - std::shared_ptr ctx, int ip_1, int ip_2, int ip_3, int ip_4, int mask_1, - int mask_2, int mask_3, int mask_4, int gateway_1, int gateway_2, int gateway_3, int gateway_4, - int vlan_flg, int vlan_id, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return SetControlPort( - tcp_driver_local, ip_1, ip_2, ip_3, ip_4, mask_1, mask_2, mask_3, mask_4, gateway_1, gateway_2, - gateway_3, gateway_4, vlan_flg, vlan_id, with_run); -} -Status HesaiHwInterface::SetControlPort( - int ip_1, int ip_2, int ip_3, int ip_4, int mask_1, int mask_2, int mask_3, int mask_4, - int gateway_1, int gateway_2, int gateway_3, int gateway_4, int vlan_flg, int vlan_id, - bool with_run) -{ - if (with_run) { - if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { - tcp_driver_s_->GetIOContext()->restart(); - } - } - return SetControlPort( - tcp_driver_s_, ip_1, ip_2, ip_3, ip_4, mask_1, mask_2, mask_3, mask_4, gateway_1, gateway_2, - gateway_3, gateway_4, vlan_flg, vlan_id, with_run); -} - -Status HesaiHwInterface::SetLidarRange( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int method, - std::vector data, bool with_run) -{ - std::vector buf_vec; - int len = 1 + data.size(); - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_SET_LIDAR_RANGE); // Cmd PTC_COMMAND_SET_LIDAR_RANGE - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - - // 0 - for all channels : 5-1 bytes - // 1 - for each channel : 323-1 bytes - // 2 - multi-section FOV : 1347-1 bytes - buf_vec.emplace_back((method >> 0) & 0xff); - for (int d : data) { - buf_vec.emplace_back(d); - } - - if (!CheckLock(tms_, tms_fail_cnt, tms_fail_cnt_max, "SetLidarRange")) { - return SetLidarRange(target_tcp_driver, method, data, with_run); - } - PrintDebug("SetLidarRange: start"); - - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tms_, "SetLidarRange"); }); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::SetLidarRange: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): SetLidarRange" << std::endl; -#endif - } - - return Status::WAITING_FOR_SENSOR_RESPONSE; -} -Status HesaiHwInterface::SetLidarRange( - std::shared_ptr ctx, int method, std::vector data, - bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return SetLidarRange(tcp_driver_local, method, data, with_run); -} -Status HesaiHwInterface::SetLidarRange(int method, std::vector data, bool with_run) -{ - if (with_run) { - if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { - tcp_driver_s_->GetIOContext()->restart(); - } - } - return SetLidarRange(tcp_driver_s_, method, data, with_run); -} - -Status HesaiHwInterface::SetLidarRange( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int start, int end, - bool with_run) -{ - std::vector buf_vec; - int len = 5; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_SET_LIDAR_RANGE); // Cmd PTC_COMMAND_SET_LIDAR_RANGE - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - - // 0 - for all channels : 5-1 bytes - // 1 - for each channel : 323-1 bytes - // 2 - multi-section FOV : 1347-1 bytes - int method = 0; - buf_vec.emplace_back((method >> 0) & 0xff); - buf_vec.emplace_back((start >> 8) & 0xff); - buf_vec.emplace_back((start >> 0) & 0xff); - buf_vec.emplace_back((end >> 8) & 0xff); - buf_vec.emplace_back((end >> 0) & 0xff); - - if (!CheckLock(tms_, tms_fail_cnt, tms_fail_cnt_max, "SetLidarRange(All)")) { - return SetLidarRange(target_tcp_driver, start, end, with_run); - } - PrintDebug("SetLidarRange(All): start"); - - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tms_, "SetLidarRange(All)"); }); - if (with_run) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "start ctx->run(): SetLidarRange(All)" << std::endl; -#endif - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::SetLidarRange(All): " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): SetLidarRange(All)" << std::endl; -#endif - } - - return Status::WAITING_FOR_SENSOR_RESPONSE; -} -Status HesaiHwInterface::SetLidarRange( - std::shared_ptr ctx, int start, int end, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return SetLidarRange(tcp_driver_local, start, end, with_run); -} -Status HesaiHwInterface::SetLidarRange(int start, int end, bool with_run) -{ - if (with_run) { - if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { - tcp_driver_s_->GetIOContext()->restart(); - } - } - return SetLidarRange(tcp_driver_s_, start, end, with_run); -} - -Status HesaiHwInterface::GetLidarRange( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function callback, bool with_run) -{ - std::vector buf_vec; - int len = 0; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_GET_LIDAR_RANGE); // Cmd PTC_COMMAND_GET_LIDAR_RANGE - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max, "GetLidarRange")) { - return GetLidarRange(target_tcp_driver, callback, with_run); - } - PrintDebug("SetLidarRange: start"); +} - target_tcp_driver->asyncSendReceiveHeaderPayload( - buf_vec, - [this](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - }, - [this, target_tcp_driver, callback](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; +std::vector HesaiHwInterface::GetLidarCalibrationBytes() +{ + auto response_ptr = SendReceive(PTC_COMMAND_GET_LIDAR_CALIBRATION); + return std::vector(*response_ptr); +} + +std::string HesaiHwInterface::GetLidarCalibrationString() +{ + auto response_ptr = SendReceive(PTC_COMMAND_GET_LIDAR_CALIBRATION); + std::string calib_string(response_ptr->begin(), response_ptr->end()); + return calib_string; +} + +HesaiPtpDiagStatus HesaiHwInterface::GetPtpDiagStatus() +{ + auto response_ptr = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_STATUS}); + auto & response = *response_ptr; + + HesaiPtpDiagStatus hesai_ptp_diag_status{}; + int payload_pos = 0; + hesai_ptp_diag_status.master_offset = static_cast(response[payload_pos++]) << 56; + hesai_ptp_diag_status.master_offset = + hesai_ptp_diag_status.master_offset | static_cast(response[payload_pos++]) << 48; + hesai_ptp_diag_status.master_offset = + hesai_ptp_diag_status.master_offset | static_cast(response[payload_pos++]) << 40; + hesai_ptp_diag_status.master_offset = + hesai_ptp_diag_status.master_offset | static_cast(response[payload_pos++]) << 32; + hesai_ptp_diag_status.master_offset = + hesai_ptp_diag_status.master_offset | static_cast(response[payload_pos++]) << 24; + hesai_ptp_diag_status.master_offset = + hesai_ptp_diag_status.master_offset | static_cast(response[payload_pos++]) << 16; + hesai_ptp_diag_status.master_offset = + hesai_ptp_diag_status.master_offset | static_cast(response[payload_pos++]) << 8; + hesai_ptp_diag_status.master_offset = + hesai_ptp_diag_status.master_offset | static_cast(response[payload_pos++]); + hesai_ptp_diag_status.ptp_state = response[payload_pos++] << 24; + hesai_ptp_diag_status.ptp_state = hesai_ptp_diag_status.ptp_state | response[payload_pos++] << 16; + hesai_ptp_diag_status.ptp_state = hesai_ptp_diag_status.ptp_state | response[payload_pos++] << 8; + hesai_ptp_diag_status.ptp_state = hesai_ptp_diag_status.ptp_state | response[payload_pos++]; + hesai_ptp_diag_status.elapsed_millisec = response[payload_pos++] << 24; + hesai_ptp_diag_status.elapsed_millisec = + hesai_ptp_diag_status.elapsed_millisec | response[payload_pos++] << 16; + hesai_ptp_diag_status.elapsed_millisec = + hesai_ptp_diag_status.elapsed_millisec | response[payload_pos++] << 8; + hesai_ptp_diag_status.elapsed_millisec = + hesai_ptp_diag_status.elapsed_millisec | response[payload_pos++]; - std::cout << "GetLidarRange getHeader: "; - for (const auto & b : target_tcp_driver->getHeader()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - std::cout << "GetLidarRange getPayload: "; - for (const auto & b : target_tcp_driver->getPayload()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); + std::stringstream ss; + ss << "HesaiHwInterface::GetPtpDiagStatus: " << hesai_ptp_diag_status; + PrintInfo(ss.str()); + + return hesai_ptp_diag_status; +} + +HesaiPtpDiagPort HesaiHwInterface::GetPtpDiagPort() +{ + auto response_ptr = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_PORT_DATA_SET}); + auto & response = *response_ptr; + + HesaiPtpDiagPort hesai_ptp_diag_port; + int payload_pos = 0; + + for (size_t i = 0; i < hesai_ptp_diag_port.portIdentity.size(); i++) { + hesai_ptp_diag_port.portIdentity[i] = response[payload_pos++]; + } + hesai_ptp_diag_port.portState = static_cast(response[payload_pos++]); + hesai_ptp_diag_port.logMinDelayReqInterval = static_cast(response[payload_pos++]); + hesai_ptp_diag_port.peerMeanPathDelay = static_cast(response[payload_pos++]) << 56; + hesai_ptp_diag_port.peerMeanPathDelay = + hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) << 48; + hesai_ptp_diag_port.peerMeanPathDelay = + hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) << 40; + hesai_ptp_diag_port.peerMeanPathDelay = + hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) << 32; + hesai_ptp_diag_port.peerMeanPathDelay = + hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) << 24; + hesai_ptp_diag_port.peerMeanPathDelay = + hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) << 16; + hesai_ptp_diag_port.peerMeanPathDelay = + hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) << 8; + hesai_ptp_diag_port.peerMeanPathDelay = + hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]); + hesai_ptp_diag_port.logAnnounceInterval = static_cast(response[payload_pos++]); + hesai_ptp_diag_port.announceReceiptTimeout = static_cast(response[payload_pos++]); + hesai_ptp_diag_port.logSyncInterval = static_cast(response[payload_pos++]); + hesai_ptp_diag_port.delayMechanism = static_cast(response[payload_pos++]); + hesai_ptp_diag_port.logMinPdelayReqInterval = static_cast(response[payload_pos++]); + hesai_ptp_diag_port.versionNumber = static_cast(response[payload_pos++]); - auto response = target_tcp_driver->getPayload(); - if (8 < response.size()) { - int payload_pos = 8; - int method = static_cast(response[payload_pos++]); -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "GetLidarRange method: " << method << std::endl; -#endif - if (method == 0) // for all channels - { - HesaiLidarRangeAll hesai_range_all; - hesai_range_all.method = method; - hesai_range_all.start = response[payload_pos++] << 8; - hesai_range_all.start = hesai_range_all.start | response[payload_pos++]; - hesai_range_all.end = response[payload_pos++] << 8; - hesai_range_all.end = hesai_range_all.end | response[payload_pos++]; - callback(hesai_range_all); - } else if (method == 1) // for each channel - { - HesaiLidarRangeAll hesai_range_all; - hesai_range_all.method = method; - callback(hesai_range_all); - } else if (method == 2) // multi-section FOV - { - HesaiLidarRangeAll hesai_range_all; - hesai_range_all.method = method; - callback(hesai_range_all); - } - } - }, - [this]() { CheckUnlock(tm_, "GetLidarRange"); }); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::GetLidarRange: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): GetLidarRange" << std::endl; -#endif - } + std::stringstream ss; + ss << "HesaiHwInterface::GetPtpDiagPort: " << hesai_ptp_diag_port; + PrintInfo(ss.str()); + + return hesai_ptp_diag_port; +} + +HesaiPtpDiagTime HesaiHwInterface::GetPtpDiagTime() +{ + auto response_ptr = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_TIME_STATUS_NP}); + auto & response = *response_ptr; + + HesaiPtpDiagTime hesai_ptp_diag_time; + int payload_pos = 0; + hesai_ptp_diag_time.master_offset = static_cast(response[payload_pos++]) << 56; + hesai_ptp_diag_time.master_offset = + hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 48; + hesai_ptp_diag_time.master_offset = + hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 40; + hesai_ptp_diag_time.master_offset = + hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 32; + hesai_ptp_diag_time.master_offset = + hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 24; + hesai_ptp_diag_time.master_offset = + hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 16; + hesai_ptp_diag_time.master_offset = + hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 8; + hesai_ptp_diag_time.master_offset = + hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]); + hesai_ptp_diag_time.ingress_time = static_cast(response[payload_pos++]) << 56; + hesai_ptp_diag_time.ingress_time = + hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 48; + hesai_ptp_diag_time.ingress_time = + hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 40; + hesai_ptp_diag_time.ingress_time = + hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 32; + hesai_ptp_diag_time.ingress_time = + hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 24; + hesai_ptp_diag_time.ingress_time = + hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 16; + hesai_ptp_diag_time.ingress_time = + hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 8; + hesai_ptp_diag_time.ingress_time = + hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]); + hesai_ptp_diag_time.cumulativeScaledRateOffset = response[payload_pos++] << 24; + hesai_ptp_diag_time.cumulativeScaledRateOffset = + hesai_ptp_diag_time.cumulativeScaledRateOffset | response[payload_pos++] << 16; + hesai_ptp_diag_time.cumulativeScaledRateOffset = + hesai_ptp_diag_time.cumulativeScaledRateOffset | response[payload_pos++] << 8; + hesai_ptp_diag_time.cumulativeScaledRateOffset = + hesai_ptp_diag_time.cumulativeScaledRateOffset | response[payload_pos++]; + hesai_ptp_diag_time.scaledLastGmPhaseChange = response[payload_pos++] << 24; + hesai_ptp_diag_time.scaledLastGmPhaseChange = + hesai_ptp_diag_time.scaledLastGmPhaseChange | response[payload_pos++] << 16; + hesai_ptp_diag_time.scaledLastGmPhaseChange = + hesai_ptp_diag_time.scaledLastGmPhaseChange | response[payload_pos++] << 8; + hesai_ptp_diag_time.scaledLastGmPhaseChange = + hesai_ptp_diag_time.scaledLastGmPhaseChange | response[payload_pos++]; + hesai_ptp_diag_time.gmTimeBaseIndicator = response[payload_pos++] << 8; + hesai_ptp_diag_time.gmTimeBaseIndicator = + hesai_ptp_diag_time.gmTimeBaseIndicator | response[payload_pos++]; + for (size_t i = 0; i < hesai_ptp_diag_time.lastGmPhaseChange.size(); i++) { + hesai_ptp_diag_time.lastGmPhaseChange[i] = response[payload_pos++]; + } + hesai_ptp_diag_time.gmPresent = response[payload_pos++] << 24; + hesai_ptp_diag_time.gmPresent = hesai_ptp_diag_time.gmPresent | response[payload_pos++] << 16; + hesai_ptp_diag_time.gmPresent = hesai_ptp_diag_time.gmPresent | response[payload_pos++] << 8; + hesai_ptp_diag_time.gmPresent = hesai_ptp_diag_time.gmPresent | response[payload_pos++]; + hesai_ptp_diag_time.gmIdentity = static_cast(response[payload_pos++]) << 56; + hesai_ptp_diag_time.gmIdentity = + hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 48; + hesai_ptp_diag_time.gmIdentity = + hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 40; + hesai_ptp_diag_time.gmIdentity = + hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 32; + hesai_ptp_diag_time.gmIdentity = + hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 24; + hesai_ptp_diag_time.gmIdentity = + hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 16; + hesai_ptp_diag_time.gmIdentity = + hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 8; + hesai_ptp_diag_time.gmIdentity = + hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]); - return Status::WAITING_FOR_SENSOR_RESPONSE; + std::stringstream ss; + ss << "HesaiHwInterface::GetPtpDiagTime: " << hesai_ptp_diag_time; + PrintInfo(ss.str()); + + return hesai_ptp_diag_time; } -Status HesaiHwInterface::GetLidarRange( - std::shared_ptr ctx, - std::function callback, bool with_run) + +HesaiPtpDiagGrandmaster HesaiHwInterface::GetPtpDiagGrandmaster() { - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return GetLidarRange(tcp_driver_local, callback, with_run); + auto response_ptr = + SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_GRANDMASTER_SETTINGS_NP}); + auto & response = *response_ptr; + + HesaiPtpDiagGrandmaster hesai_ptp_diag_grandmaster; + int payload_pos = 0; + + hesai_ptp_diag_grandmaster.clockQuality = response[payload_pos++] << 24; + hesai_ptp_diag_grandmaster.clockQuality = + hesai_ptp_diag_grandmaster.clockQuality | response[payload_pos++] << 16; + hesai_ptp_diag_grandmaster.clockQuality = + hesai_ptp_diag_grandmaster.clockQuality | response[payload_pos++] << 8; + hesai_ptp_diag_grandmaster.clockQuality = + hesai_ptp_diag_grandmaster.clockQuality | response[payload_pos++]; + hesai_ptp_diag_grandmaster.utc_offset = response[payload_pos++] << 8; + hesai_ptp_diag_grandmaster.utc_offset = + hesai_ptp_diag_grandmaster.utc_offset | response[payload_pos++]; + hesai_ptp_diag_grandmaster.time_flags = static_cast(response[payload_pos++]); + hesai_ptp_diag_grandmaster.time_source = static_cast(response[payload_pos++]); + + std::stringstream ss; + ss << "HesaiHwInterface::GetPtpDiagGrandmaster: " << hesai_ptp_diag_grandmaster; + PrintInfo(ss.str()); + + return hesai_ptp_diag_grandmaster; +} + +HesaiInventory HesaiHwInterface::GetInventory() +{ + auto response_ptr = SendReceive(PTC_COMMAND_GET_INVENTORY_INFO); + auto & response = *response_ptr; + + HesaiInventory hesai_inventory; + int payload_pos = 0; + for (size_t i = 0; i < hesai_inventory.sn.size(); i++) { + hesai_inventory.sn[i] = response[payload_pos++]; + } + for (size_t i = 0; i < hesai_inventory.date_of_manufacture.size(); i++) { + hesai_inventory.date_of_manufacture[i] = response[payload_pos++]; + } + for (size_t i = 0; i < hesai_inventory.mac.size(); i++) { + hesai_inventory.mac[i] = response[payload_pos++]; + } + for (size_t i = 0; i < hesai_inventory.sw_ver.size(); i++) { + hesai_inventory.sw_ver[i] = response[payload_pos++]; + } + for (size_t i = 0; i < hesai_inventory.hw_ver.size(); i++) { + hesai_inventory.hw_ver[i] = response[payload_pos++]; + } + for (size_t i = 0; i < hesai_inventory.control_fw_ver.size(); i++) { + hesai_inventory.control_fw_ver[i] = response[payload_pos++]; + } + for (size_t i = 0; i < hesai_inventory.sensor_fw_ver.size(); i++) { + hesai_inventory.sensor_fw_ver[i] = response[payload_pos++]; + } + hesai_inventory.angle_offset = response[payload_pos++] << 8; + hesai_inventory.angle_offset = hesai_inventory.angle_offset | response[payload_pos++]; + hesai_inventory.model = static_cast(response[payload_pos++]); + hesai_inventory.motor_type = static_cast(response[payload_pos++]); + hesai_inventory.num_of_lines = static_cast(response[payload_pos++]); + for (size_t i = 0; i < hesai_inventory.reserved.size(); i++) { + hesai_inventory.reserved[i] = static_cast(response[payload_pos++]); + } + + return hesai_inventory; +} + +HesaiConfig HesaiHwInterface::GetConfig() +{ + auto response_ptr = SendReceive(PTC_COMMAND_GET_CONFIG_INFO); + auto & response = *response_ptr; + + HesaiConfig hesai_config{}; + int payload_pos = 0; + hesai_config.ipaddr[0] = static_cast(response[payload_pos++]); + hesai_config.ipaddr[1] = static_cast(response[payload_pos++]); + hesai_config.ipaddr[2] = static_cast(response[payload_pos++]); + hesai_config.ipaddr[3] = static_cast(response[payload_pos++]); + hesai_config.mask[0] = static_cast(response[payload_pos++]); + hesai_config.mask[1] = static_cast(response[payload_pos++]); + hesai_config.mask[2] = static_cast(response[payload_pos++]); + hesai_config.mask[3] = static_cast(response[payload_pos++]); + hesai_config.gateway[0] = static_cast(response[payload_pos++]); + hesai_config.gateway[1] = static_cast(response[payload_pos++]); + hesai_config.gateway[2] = static_cast(response[payload_pos++]); + hesai_config.gateway[3] = static_cast(response[payload_pos++]); + hesai_config.dest_ipaddr[0] = static_cast(response[payload_pos++]); + hesai_config.dest_ipaddr[1] = static_cast(response[payload_pos++]); + hesai_config.dest_ipaddr[2] = static_cast(response[payload_pos++]); + hesai_config.dest_ipaddr[3] = static_cast(response[payload_pos++]); + hesai_config.dest_LiDAR_udp_port = response[payload_pos++] << 8; + hesai_config.dest_LiDAR_udp_port = hesai_config.dest_LiDAR_udp_port | response[payload_pos++]; + hesai_config.dest_gps_udp_port = response[payload_pos++] << 8; + hesai_config.dest_gps_udp_port = hesai_config.dest_gps_udp_port | response[payload_pos++]; + hesai_config.spin_rate = response[payload_pos++] << 8; + hesai_config.spin_rate = hesai_config.spin_rate | response[payload_pos++]; + hesai_config.sync = static_cast(response[payload_pos++]); + hesai_config.sync_angle = response[payload_pos++] << 8; + hesai_config.sync_angle = hesai_config.sync_angle | response[payload_pos++]; + hesai_config.start_angle = response[payload_pos++] << 8; + hesai_config.start_angle = hesai_config.start_angle | response[payload_pos++]; + hesai_config.stop_angle = response[payload_pos++] << 8; + hesai_config.stop_angle = hesai_config.stop_angle | response[payload_pos++]; + hesai_config.clock_source = static_cast(response[payload_pos++]); + hesai_config.udp_seq = static_cast(response[payload_pos++]); + hesai_config.trigger_method = static_cast(response[payload_pos++]); + hesai_config.return_mode = static_cast(response[payload_pos++]); + hesai_config.standby_mode = static_cast(response[payload_pos++]); + hesai_config.motor_status = static_cast(response[payload_pos++]); + hesai_config.vlan_flag = static_cast(response[payload_pos++]); + hesai_config.vlan_id = response[payload_pos++] << 8; + hesai_config.vlan_id = hesai_config.vlan_id | response[payload_pos++]; + hesai_config.clock_data_fmt = static_cast(response[payload_pos++]); + hesai_config.noise_filtering = static_cast(response[payload_pos++]); + hesai_config.reflectivity_mapping = static_cast(response[payload_pos++]); + hesai_config.reserved[0] = static_cast(response[payload_pos++]); + hesai_config.reserved[1] = static_cast(response[payload_pos++]); + hesai_config.reserved[2] = static_cast(response[payload_pos++]); + hesai_config.reserved[3] = static_cast(response[payload_pos++]); + hesai_config.reserved[4] = static_cast(response[payload_pos++]); + hesai_config.reserved[5] = static_cast(response[payload_pos++]); + + std::cout << "Config: " << hesai_config << std::endl; + return hesai_config; +} + +HesaiLidarStatus HesaiHwInterface::GetLidarStatus() +{ + auto response_ptr = SendReceive(PTC_COMMAND_GET_LIDAR_STATUS); + auto & response = *response_ptr; + + HesaiLidarStatus hesai_status; + int payload_pos = 0; + hesai_status.system_uptime = response[payload_pos++] << 24; + hesai_status.system_uptime = hesai_status.system_uptime | response[payload_pos++] << 16; + hesai_status.system_uptime = hesai_status.system_uptime | response[payload_pos++] << 8; + hesai_status.system_uptime = hesai_status.system_uptime | response[payload_pos++]; + hesai_status.motor_speed = response[payload_pos++] << 8; + hesai_status.motor_speed = hesai_status.motor_speed | response[payload_pos++]; + for (size_t i = 0; i < hesai_status.temperature.size(); i++) { + hesai_status.temperature[i] = response[payload_pos++] << 24; + hesai_status.temperature[i] = hesai_status.temperature[i] | response[payload_pos++] << 16; + hesai_status.temperature[i] = hesai_status.temperature[i] | response[payload_pos++] << 8; + hesai_status.temperature[i] = hesai_status.temperature[i] | response[payload_pos++]; + } + hesai_status.gps_pps_lock = static_cast(response[payload_pos++]); + hesai_status.gps_gprmc_status = static_cast(response[payload_pos++]); + hesai_status.startup_times = response[payload_pos++] << 24; + hesai_status.startup_times = hesai_status.startup_times | response[payload_pos++] << 16; + hesai_status.startup_times = hesai_status.startup_times | response[payload_pos++] << 8; + hesai_status.startup_times = hesai_status.startup_times | response[payload_pos++]; + hesai_status.total_operation_time = response[payload_pos++] << 24; + hesai_status.total_operation_time = hesai_status.total_operation_time | response[payload_pos++] + << 16; + hesai_status.total_operation_time = hesai_status.total_operation_time | response[payload_pos++] + << 8; + hesai_status.total_operation_time = hesai_status.total_operation_time | response[payload_pos++]; + hesai_status.ptp_clock_status = static_cast(response[payload_pos++]); + for (size_t i = 0; i < hesai_status.reserved.size(); i++) { + hesai_status.reserved[i] = static_cast(response[payload_pos++]); + } + + return hesai_status; +} + +Status HesaiHwInterface::SetSpinRate(uint16_t rpm) +{ + std::vector request_payload; + request_payload.emplace_back((rpm >> 8) & 0xff); + request_payload.emplace_back(rpm & 0xff); + + SendReceive(PTC_COMMAND_SET_SPIN_RATE, request_payload); + return Status::OK; } -Status HesaiHwInterface::GetLidarRange(std::shared_ptr ctx, bool with_run) + +Status HesaiHwInterface::SetSyncAngle(int sync_angle, int angle) { - return GetLidarRange( - ctx, [this](HesaiLidarRangeAll & result) { std::cout << result << std::endl; }, with_run); + std::vector request_payload; + request_payload.emplace_back(sync_angle & 0xff); + request_payload.emplace_back((angle >> 8) & 0xff); + request_payload.emplace_back(angle & 0xff); + + SendReceive(PTC_COMMAND_SET_SYNC_ANGLE, request_payload); + return Status::OK; } -Status HesaiHwInterface::GetLidarRange( - std::function callback, bool with_run) + +Status HesaiHwInterface::SetTriggerMethod(int trigger_method) { - if (with_run) { - if (tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } - } - return GetLidarRange(tcp_driver_, callback, with_run); + std::vector request_payload; + request_payload.emplace_back(trigger_method & 0xff); + + SendReceive(PTC_COMMAND_SET_TRIGGER_METHOD, request_payload); + return Status::OK; } -Status HesaiHwInterface::GetLidarRange(bool with_run) + +Status HesaiHwInterface::SetStandbyMode(int standby_mode) { - return GetLidarRange( - [this](HesaiLidarRangeAll & result) { std::cout << result << std::endl; }, with_run); + std::vector request_payload; + request_payload.emplace_back(standby_mode & 0xff); + + SendReceive(PTC_COMMAND_SET_STANDBY_MODE, request_payload); + return Status::OK; } -Status HesaiHwInterface::SetClockSource( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int clock_source, - bool with_run) +Status HesaiHwInterface::SetReturnMode(int return_mode) { - std::vector buf_vec; - int len = 1; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_SET_CLOCK_SOURCE); - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); + std::vector request_payload; + request_payload.emplace_back(return_mode & 0xff); - buf_vec.emplace_back((clock_source >> 0) & 0xff); + SendReceive(PTC_COMMAND_SET_RETURN_MODE, request_payload); + return Status::OK; +} - if (!CheckLock(tms_, tms_fail_cnt, tms_fail_cnt_max, "SetClockSource")) { - return SetClockSource(target_tcp_driver, clock_source, with_run); - } - PrintDebug("SetClockSource: start"); +Status HesaiHwInterface::SetDestinationIp( + int dest_ip_1, int dest_ip_2, int dest_ip_3, int dest_ip_4, int port, int gps_port) +{ + std::vector request_payload; + request_payload.emplace_back(dest_ip_1 & 0xff); + request_payload.emplace_back(dest_ip_2 & 0xff); + request_payload.emplace_back(dest_ip_3 & 0xff); + request_payload.emplace_back(dest_ip_4 & 0xff); + request_payload.emplace_back((port >> 8) & 0xff); + request_payload.emplace_back(port & 0xff); + request_payload.emplace_back((gps_port >> 8) & 0xff); + request_payload.emplace_back(gps_port & 0xff); + + SendReceive(PTC_COMMAND_SET_DESTINATION_IP, request_payload); + return Status::OK; +} - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tms_, "SetClockSource"); }); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::SetClockSource: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): SetReturnMode" << std::endl; -#endif - } +Status HesaiHwInterface::SetControlPort( + int ip_1, int ip_2, int ip_3, int ip_4, int mask_1, int mask_2, int mask_3, int mask_4, + int gateway_1, int gateway_2, int gateway_3, int gateway_4, int vlan_flg, int vlan_id) +{ + std::vector request_payload; + request_payload.emplace_back(ip_1 & 0xff); + request_payload.emplace_back(ip_2 & 0xff); + request_payload.emplace_back(ip_3 & 0xff); + request_payload.emplace_back(ip_4 & 0xff); + request_payload.emplace_back(mask_1 & 0xff); + request_payload.emplace_back(mask_2 & 0xff); + request_payload.emplace_back(mask_3 & 0xff); + request_payload.emplace_back(mask_4 & 0xff); + request_payload.emplace_back(gateway_1 & 0xff); + request_payload.emplace_back(gateway_2 & 0xff); + request_payload.emplace_back(gateway_3 & 0xff); + request_payload.emplace_back(gateway_4 & 0xff); + request_payload.emplace_back(vlan_flg & 0xff); + request_payload.emplace_back((vlan_id >> 8) & 0xff); + request_payload.emplace_back(vlan_id & 0xff); + + SendReceive(PTC_COMMAND_SET_CONTROL_PORT, request_payload); + return Status::OK; +} - return Status::WAITING_FOR_SENSOR_RESPONSE; +Status HesaiHwInterface::SetLidarRange(int method, std::vector data) +{ + // 0 - for all channels : 5-1 bytes + // 1 - for each channel : 323-1 bytes + // 2 - multi-section FOV : 1347-1 bytes + std::vector request_payload; + request_payload.emplace_back(method & 0xff); + request_payload.insert(request_payload.end(), data.begin(), data.end()); + + SendReceive(PTC_COMMAND_SET_LIDAR_RANGE, request_payload); + return Status::OK; } -Status HesaiHwInterface::SetClockSource( - std::shared_ptr ctx, int clock_source, bool with_run) + +Status HesaiHwInterface::SetLidarRange(int start, int end) { - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return SetClockSource(tcp_driver_local, clock_source, with_run); + // 0 - for all channels : 5-1 bytes + // 1 - for each channel : 323-1 bytes + // 2 - multi-section FOV : 1347-1 bytes + std::vector request_payload; + int method = 0; + request_payload.emplace_back(method & 0xff); + request_payload.emplace_back((start >> 8) & 0xff); + request_payload.emplace_back(start & 0xff); + request_payload.emplace_back((end >> 8) & 0xff); + request_payload.emplace_back(end & 0xff); + + SendReceive(PTC_COMMAND_SET_LIDAR_RANGE, request_payload); + return Status::OK; } -Status HesaiHwInterface::SetClockSource(int clock_source, bool with_run) + +HesaiLidarRangeAll HesaiHwInterface::GetLidarRange() { - //* - if (with_run) { - if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { - tcp_driver_s_->GetIOContext()->restart(); - } + auto response_ptr = SendReceive(PTC_COMMAND_GET_LIDAR_RANGE); + auto & response = *response_ptr; + + HesaiLidarRangeAll hesai_range_all; + int payload_pos = 0; + int method = static_cast(response[payload_pos++]); + switch (method) { + case 0: // for all channels + hesai_range_all.method = method; + hesai_range_all.start = response[payload_pos++] << 8; + hesai_range_all.start = hesai_range_all.start | response[payload_pos++]; + hesai_range_all.end = response[payload_pos++] << 8; + hesai_range_all.end = hesai_range_all.end | response[payload_pos++]; + break; + case 1: // for each channel + hesai_range_all.method = method; + break; + case 2: // multi-section FOV + hesai_range_all.method = method; + break; } - //*/ - return SetClockSource(tcp_driver_s_, clock_source, with_run); + + return hesai_range_all; +} + +Status HesaiHwInterface::SetClockSource(int clock_source) +{ + std::vector request_payload; + request_payload.emplace_back(clock_source & 0xff); + + SendReceive(PTC_COMMAND_SET_CLOCK_SOURCE, request_payload); + return Status::OK; } Status HesaiHwInterface::SetPtpConfig( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int profile, int domain, - int network, int switch_type, int logAnnounceInterval, int logSyncInterval, - int logMinDelayReqInterval, bool with_run) + int profile, int domain, int network, int switch_type, int logAnnounceInterval, + int logSyncInterval, int logMinDelayReqInterval) { if (profile < 0 || profile > 3) { return Status::ERROR_1; } + // Handle the OT128 differently - it has TSN settings and defines the PTP profile // for automotive as 0x03 instead of 0x02 for other sensors. if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR128_E4X) { @@ -2331,420 +862,103 @@ Status HesaiHwInterface::SetPtpConfig( profile = 3; } - std::vector buf_vec; - int len = 3; - switch (profile) { - case 0: - len = 6; - break; - case 3: - len = 4; - break; - default: - len = 3; - } - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_SET_PTP_CONFIG); // Cmd PTC_COMMAND_SET_PTP_CONFIG - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - buf_vec.emplace_back((profile >> 0) & 0xff); - buf_vec.emplace_back((domain >> 0) & 0xff); - buf_vec.emplace_back((network >> 0) & 0xff); + std::vector request_payload; + request_payload.emplace_back(profile & 0xff); + request_payload.emplace_back(domain & 0xff); + request_payload.emplace_back(network & 0xff); if (profile == 0) { - buf_vec.emplace_back((logAnnounceInterval >> 0) & 0xff); - buf_vec.emplace_back((logSyncInterval >> 0) & 0xff); - buf_vec.emplace_back((logMinDelayReqInterval >> 0) & 0xff); - } - else if (profile == 3) { - buf_vec.emplace_back((switch_type >> 0) & 0xff); - } - - if (!CheckLock(tms_, tms_fail_cnt, tms_fail_cnt_max, "SetPtpConfig")) { - return SetPtpConfig( - target_tcp_driver, profile, domain, network, switch_type, logAnnounceInterval, - logSyncInterval, logMinDelayReqInterval, with_run); - } - PrintDebug("SetPtpConfig: start"); - - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tms_, "SetPtpConfig"); }); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::SetPtpConfig: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): SetPtpConfig" << std::endl; -#endif + request_payload.emplace_back(logAnnounceInterval & 0xff); + request_payload.emplace_back(logSyncInterval & 0xff); + request_payload.emplace_back(logMinDelayReqInterval & 0xff); + } else if (profile == 2 || profile == 3) { + request_payload.emplace_back(switch_type & 0xff); } - return Status::WAITING_FOR_SENSOR_RESPONSE; -} -Status HesaiHwInterface::SetPtpConfig( - std::shared_ptr ctx, int profile, int domain, int network, - int switch_type, int logAnnounceInterval, int logSyncInterval, - int logMinDelayReqInterval, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return SetPtpConfig( - tcp_driver_local, profile, domain, network, switch_type, logAnnounceInterval, - logSyncInterval, logMinDelayReqInterval, with_run); + SendReceive(PTC_COMMAND_SET_PTP_CONFIG, request_payload); + return Status::OK; } -Status HesaiHwInterface::SetPtpConfig( - int profile, int domain, int network, int switch_type, int logAnnounceInterval, - int logSyncInterval, int logMinDelayReqInterval, bool with_run) -{ - if (with_run) { - if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { - tcp_driver_s_->GetIOContext()->restart(); - } - } - return SetPtpConfig( - tcp_driver_s_, profile, domain, network, switch_type, logAnnounceInterval, - logSyncInterval, logMinDelayReqInterval, with_run); -} - -Status HesaiHwInterface::GetPtpConfig( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, bool with_run) -{ - std::vector buf_vec; - int len = 0; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_GET_PTP_CONFIG); // Cmd PTC_COMMAND_GET_PTP_CONFIG - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - - if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max, "GetPtpConfig")) { - return GetPtpConfig(target_tcp_driver, with_run); - } - PrintDebug("GetPtpConfig: start"); - - target_tcp_driver->asyncSendReceiveHeaderPayload( - buf_vec, - [this](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - }, - [this, target_tcp_driver](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - - std::cout << "GetPtpConfig getHeader: "; - for (const auto & b : target_tcp_driver->getHeader()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - std::cout << "GetPtpConfig getPayload: "; - for (const auto & b : target_tcp_driver->getPayload()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - - auto response = target_tcp_driver->getPayload(); - HesaiPtpConfig hesai_ptp_config{}; - if (8 < response.size()) { - int payload_pos = 8; - hesai_ptp_config.status = static_cast(response[payload_pos++]); - hesai_ptp_config.profile = static_cast(response[payload_pos++]); - hesai_ptp_config.domain = static_cast(response[payload_pos++]); - hesai_ptp_config.network = static_cast(response[payload_pos++]); - if (hesai_ptp_config.status == 0) { - hesai_ptp_config.logAnnounceInterval = static_cast(response[payload_pos++]); - hesai_ptp_config.logSyncInterval = static_cast(response[payload_pos++]); - hesai_ptp_config.logMinDelayReqInterval = static_cast(response[payload_pos++]); - } - - std::stringstream ss; - ss << "HesaiHwInterface::GetPtpConfig: " << hesai_ptp_config; - PrintInfo(ss.str()); - } - }, - [this]() { CheckUnlock(tm_, "GetPtpConfig"); }); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::GetPtpConfig: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): GetPtpConfig" << std::endl; -#endif - } - return Status::WAITING_FOR_SENSOR_RESPONSE; -} -Status HesaiHwInterface::GetPtpConfig(std::shared_ptr ctx, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return GetPtpConfig(tcp_driver_local, with_run); -} -Status HesaiHwInterface::GetPtpConfig(bool with_run) +HesaiPtpConfig HesaiHwInterface::GetPtpConfig() { - if (with_run) { - if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { - tcp_driver_s_->GetIOContext()->restart(); - } - } - return GetPtpConfig(tcp_driver_s_, with_run); -} + auto response_ptr = SendReceive(PTC_COMMAND_GET_PTP_CONFIG); + auto & response = *response_ptr; -Status HesaiHwInterface::SendReset( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, bool with_run) -{ - std::vector buf_vec; - int len = 0; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_RESET); // Cmd PTC_COMMAND_RESET - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); + HesaiPtpConfig hesai_ptp_config{}; - if (!CheckLock(tms_, tms_fail_cnt, tms_fail_cnt_max, "SendReset")) { - return SendReset(target_tcp_driver, with_run); + int payload_pos = 0; + hesai_ptp_config.status = static_cast(response[payload_pos++]); + hesai_ptp_config.profile = static_cast(response[payload_pos++]); + hesai_ptp_config.domain = static_cast(response[payload_pos++]); + hesai_ptp_config.network = static_cast(response[payload_pos++]); + if (hesai_ptp_config.status == 0) { + hesai_ptp_config.logAnnounceInterval = static_cast(response[payload_pos++]); + hesai_ptp_config.logSyncInterval = static_cast(response[payload_pos++]); + hesai_ptp_config.logMinDelayReqInterval = static_cast(response[payload_pos++]); } - PrintDebug("SendReset: start"); - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tms_, "SendReset"); }); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::SendReset: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): SendReset" << std::endl; -#endif - } + std::stringstream ss; + ss << "HesaiHwInterface::GetPtpConfig: " << hesai_ptp_config; + PrintInfo(ss.str()); - return Status::WAITING_FOR_SENSOR_RESPONSE; + return hesai_ptp_config; } -Status HesaiHwInterface::SendReset(std::shared_ptr ctx, bool with_run) + +Status HesaiHwInterface::SendReset() { - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return SendReset(tcp_driver_local, with_run); + SendReceive(PTC_COMMAND_RESET); + return Status::OK; } -Status HesaiHwInterface::SendReset(bool with_run) + +Status HesaiHwInterface::SetRotDir(int mode) { - //* - if (with_run) { - if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { - tcp_driver_s_->GetIOContext()->restart(); - } - } - //*/ - return SendReset(tcp_driver_s_, with_run); + std::vector request_payload; + request_payload.emplace_back(mode & 0xff); + + SendReceive(PTC_COMMAND_SET_ROTATE_DIRECTION, request_payload); + return Status::OK; } -Status HesaiHwInterface::SetRotDir( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int mode, bool with_run) +HesaiLidarMonitor HesaiHwInterface::GetLidarMonitor() { - std::vector buf_vec; - int len = 1; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_SET_ROTATE_DIRECTION); // Cmd PTC_COMMAND_SET_ROTATE_DIRECTION - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - - buf_vec.emplace_back((mode >> 0) & 0xff); + auto response_ptr = SendReceive(PTC_COMMAND_LIDAR_MONITOR); + auto & response = *response_ptr; - if (!CheckLock(tms_, tms_fail_cnt, tms_fail_cnt_max, "SetRotDir")) { - return SetRotDir(target_tcp_driver, mode, with_run); - } - PrintDebug("SetRotDir: start"); + HesaiLidarMonitor hesai_lidar_monitor; + int payload_pos = 0; + hesai_lidar_monitor.input_voltage = response[payload_pos++] << 24; + hesai_lidar_monitor.input_voltage = hesai_lidar_monitor.input_voltage | response[payload_pos++] + << 16; + hesai_lidar_monitor.input_voltage = hesai_lidar_monitor.input_voltage | response[payload_pos++] + << 8; + hesai_lidar_monitor.input_voltage = hesai_lidar_monitor.input_voltage | response[payload_pos++]; + hesai_lidar_monitor.input_current = response[payload_pos++] << 24; + hesai_lidar_monitor.input_current = hesai_lidar_monitor.input_current | response[payload_pos++] + << 16; + hesai_lidar_monitor.input_current = hesai_lidar_monitor.input_current | response[payload_pos++] + << 8; + hesai_lidar_monitor.input_current = hesai_lidar_monitor.input_current | response[payload_pos++]; + hesai_lidar_monitor.input_power = response[payload_pos++] << 24; + hesai_lidar_monitor.input_power = hesai_lidar_monitor.input_power | response[payload_pos++] << 16; + hesai_lidar_monitor.input_power = hesai_lidar_monitor.input_power | response[payload_pos++] << 8; + hesai_lidar_monitor.input_power = hesai_lidar_monitor.input_power | response[payload_pos++]; - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tms_, "SetRotDir"); }); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::SetRotDir: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): SetRotDir" << std::endl; -#endif + for (size_t i = 0; i < hesai_lidar_monitor.reserved.size(); i++) { + hesai_lidar_monitor.reserved[i] = static_cast(response[payload_pos++]); } - return Status::WAITING_FOR_SENSOR_RESPONSE; -} -Status HesaiHwInterface::SetRotDir( - std::shared_ptr ctx, int mode, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return SetRotDir(tcp_driver_local, mode, with_run); + return hesai_lidar_monitor; } -Status HesaiHwInterface::SetRotDir(int mode, bool with_run) -{ - if (with_run) { - if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { - tcp_driver_s_->GetIOContext()->restart(); - } - } - return SetRotDir(tcp_driver_s_, mode, with_run); -} - -Status HesaiHwInterface::GetLidarMonitor( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function callback, bool with_run) -{ - std::vector buf_vec; - int len = 0; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_LIDAR_MONITOR); // Cmd PTC_COMMAND_LIDAR_MONITOR - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - - if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max, "GetLidarMonitor")) { - return GetLidarMonitor(target_tcp_driver, callback, with_run); - } - PrintDebug("GetLidarMonitor: start"); - - target_tcp_driver->asyncSendReceiveHeaderPayload( - buf_vec, - [this](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - }, - [this, target_tcp_driver, callback](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - std::cout << "GetLidarMonitor getHeader: "; - for (const auto & b : target_tcp_driver->getHeader()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - std::cout << "GetLidarMonitor getPayload: "; - for (const auto & b : target_tcp_driver->getPayload()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - - auto response = target_tcp_driver->getPayload(); - HesaiLidarMonitor hesai_lidar_monitor; - if (8 < response.size()) { - int payload_pos = 8; - hesai_lidar_monitor.input_voltage = response[payload_pos++] << 24; - hesai_lidar_monitor.input_voltage = - hesai_lidar_monitor.input_voltage | response[payload_pos++] << 16; - hesai_lidar_monitor.input_voltage = - hesai_lidar_monitor.input_voltage | response[payload_pos++] << 8; - hesai_lidar_monitor.input_voltage = - hesai_lidar_monitor.input_voltage | response[payload_pos++]; - hesai_lidar_monitor.input_current = response[payload_pos++] << 24; - hesai_lidar_monitor.input_current = - hesai_lidar_monitor.input_current | response[payload_pos++] << 16; - hesai_lidar_monitor.input_current = - hesai_lidar_monitor.input_current | response[payload_pos++] << 8; - hesai_lidar_monitor.input_current = - hesai_lidar_monitor.input_current | response[payload_pos++]; - hesai_lidar_monitor.input_power = response[payload_pos++] << 24; - hesai_lidar_monitor.input_power = hesai_lidar_monitor.input_power | response[payload_pos++] - << 16; - hesai_lidar_monitor.input_power = hesai_lidar_monitor.input_power | response[payload_pos++] - << 8; - hesai_lidar_monitor.input_power = hesai_lidar_monitor.input_power | response[payload_pos++]; - - for (size_t i = 0; i < hesai_lidar_monitor.reserved.size(); i++) { - hesai_lidar_monitor.reserved[i] = static_cast(response[payload_pos++]); - } - callback(hesai_lidar_monitor); - } - }, - [this]() { CheckUnlock(tm_, "GetLidarMonitor"); }); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::GetLidarMonitor: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): GetLidarMonitor" << std::endl; -#endif - } - return Status::WAITING_FOR_SENSOR_RESPONSE; -} -Status HesaiHwInterface::GetLidarMonitor( - std::shared_ptr ctx, - std::function callback, bool with_run) +void HesaiHwInterface::IOContextRun() { - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return GetLidarMonitor(tcp_driver_local, callback, with_run); -} -Status HesaiHwInterface::GetLidarMonitor( - std::shared_ptr ctx, bool with_run) -{ - return GetLidarMonitor( - ctx, [this](HesaiLidarMonitor & result) { std::cout << result << std::endl; }, with_run); -} -Status HesaiHwInterface::GetLidarMonitor( - std::function callback, bool with_run) -{ - if (with_run) { - if (tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } - } - return GetLidarMonitor(tcp_driver_, callback, with_run); + m_owned_ctx->run(); } -Status HesaiHwInterface::GetLidarMonitor(bool with_run) + +std::shared_ptr HesaiHwInterface::GetIOContext() { - return GetLidarMonitor( - [this](HesaiLidarMonitor & result) { std::cout << result << std::endl; }, with_run); + return m_owned_ctx; } -void HesaiHwInterface::IOContextRun() { m_owned_ctx->run(); } - -std::shared_ptr HesaiHwInterface::GetIOContext() { return m_owned_ctx; } - HesaiStatus HesaiHwInterface::GetHttpClientDriverOnce( std::shared_ptr ctx, std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> & hcd) @@ -2773,7 +987,10 @@ HesaiStatus HesaiHwInterface::GetHttpClientDriverOnce( return st; } -void HesaiHwInterface::str_cb(const std::string & str) { PrintInfo(str); } +void HesaiHwInterface::str_cb(const std::string & str) +{ + PrintInfo(str); +} HesaiStatus HesaiHwInterface::SetSpinSpeedAsyncHttp( std::shared_ptr ctx, uint16_t rpm) @@ -2812,13 +1029,8 @@ HesaiStatus HesaiHwInterface::SetSpinSpeedAsyncHttp(uint16_t rpm) } HesaiStatus HesaiHwInterface::SetPtpConfigSyncHttp( - std::shared_ptr ctx, - int profile, - int domain, - int network, - int logAnnounceInterval, - int logSyncInterval, - int logMinDelayReqInterval) + std::shared_ptr ctx, int profile, int domain, int network, + int logAnnounceInterval, int logSyncInterval, int logMinDelayReqInterval) { std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; auto st = GetHttpClientDriverOnce(ctx, hcd); @@ -2826,53 +1038,47 @@ HesaiStatus HesaiHwInterface::SetPtpConfigSyncHttp( return st; } - auto response = hcd->get( - (boost::format("/pandar.cgi?action=set&object=lidar&key=ptp_configuration&value={" \ - "\"Profile\": %d," \ - "\"Domain\": %d," \ - "\"Network\": %d," \ - "\"LogAnnounceInterval\": %d," \ - "\"LogSyncInterval\": %d," \ - "\"LogMinDelayReqInterval\": %d," \ - "\"tsn_switch\": %d" \ - "}") - % profile % domain % network % logAnnounceInterval % logSyncInterval % logMinDelayReqInterval % 0 - ).str()); + auto response = + hcd->get((boost::format("/pandar.cgi?action=set&object=lidar&key=ptp_configuration&value={" + "\"Profile\": %d," + "\"Domain\": %d," + "\"Network\": %d," + "\"LogAnnounceInterval\": %d," + "\"LogSyncInterval\": %d," + "\"LogMinDelayReqInterval\": %d," + "\"tsn_switch\": %d" + "}") % + profile % domain % network % logAnnounceInterval % logSyncInterval % + logMinDelayReqInterval % 0) + .str()); ctx->run(); PrintInfo(response); return Status::OK; } -HesaiStatus HesaiHwInterface::SetPtpConfigSyncHttp(int profile, - int domain, - int network, - int logAnnounceInterval, - int logSyncInterval, - int logMinDelayReqInterval) +HesaiStatus HesaiHwInterface::SetPtpConfigSyncHttp( + int profile, int domain, int network, int logAnnounceInterval, int logSyncInterval, + int logMinDelayReqInterval) { - return SetPtpConfigSyncHttp(std::make_shared(), - profile, - domain, - network, - logAnnounceInterval, - logSyncInterval, - logMinDelayReqInterval); + return SetPtpConfigSyncHttp( + std::make_shared(), profile, domain, network, logAnnounceInterval, + logSyncInterval, logMinDelayReqInterval); } HesaiStatus HesaiHwInterface::SetSyncAngleSyncHttp( - std::shared_ptr ctx, - int enable, - int angle) + std::shared_ptr ctx, int enable, int angle) { std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; auto st = GetHttpClientDriverOnce(ctx, hcd); if (st != Status::OK) { return st; } - auto tmp_str = (boost::format("/pandar.cgi?action=set&object=lidar_sync&key=sync_angle&value={" \ - "\"sync\": %d," \ - "\"syncAngle\": %d" \ - "}") % enable % angle).str(); + auto tmp_str = (boost::format("/pandar.cgi?action=set&object=lidar_sync&key=sync_angle&value={" + "\"sync\": %d," + "\"syncAngle\": %d" + "}") % + enable % angle) + .str(); PrintInfo(tmp_str); auto response = hcd->get(tmp_str); ctx->run(); @@ -2882,9 +1088,7 @@ HesaiStatus HesaiHwInterface::SetSyncAngleSyncHttp( HesaiStatus HesaiHwInterface::SetSyncAngleSyncHttp(int enable, int angle) { - return SetSyncAngleSyncHttp(std::make_shared(), - enable, - angle); + return SetSyncAngleSyncHttp(std::make_shared(), enable, angle); } HesaiStatus HesaiHwInterface::GetLidarMonitorAsyncHttp( @@ -2924,7 +1128,8 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( #endif auto current_return_mode = nebula::drivers::ReturnModeFromIntHesai( hesai_config.return_mode, sensor_configuration->sensor_model); - auto wait_time = 100ms; // Avoids spamming the sensor, which leads to failure when configuring it. + // Avoids spamming the sensor, which leads to failure when configuring it. + auto wait_time = 100ms; if (sensor_configuration->return_mode != current_return_mode) { std::stringstream ss; ss << current_return_mode; @@ -2936,7 +1141,9 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( auto return_mode_int = nebula::drivers::IntFromReturnModeHesai( sensor_configuration->return_mode, sensor_configuration->sensor_model); if (return_mode_int < 0) { - PrintError("Invalid Return Mode for this sensor. Please check your settings. Falling back to Dual mode."); + PrintError( + "Invalid Return Mode for this sensor. Please check your settings. Falling back to Dual " + "mode."); return_mode_int = 2; } SetReturnMode(return_mode_int); @@ -2954,7 +1161,8 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( if (UseHttpSetSpinRate()) { SetSpinSpeedAsyncHttp(sensor_configuration->rotation_speed); } else { - PrintInfo("Setting up spin rate via TCP." + std::to_string(sensor_configuration->rotation_speed) ); + PrintInfo( + "Setting up spin rate via TCP." + std::to_string(sensor_configuration->rotation_speed)); std::thread t( [this, sensor_configuration] { SetSpinRate(sensor_configuration->rotation_speed); }); t.join(); @@ -3014,19 +1222,18 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( PrintInfo("current lidar sync: " + std::to_string(hesai_config.sync)); PrintInfo("current lidar sync_angle: " + std::to_string(sync_angle)); PrintInfo("current configuration scan_phase: " + std::to_string(scan_phase)); - std::thread t([this, sync_flg, scan_phase] { - SetSyncAngle(sync_flg, scan_phase); - }); + std::thread t([this, sync_flg, scan_phase] { SetSyncAngle(sync_flg, scan_phase); }); t.join(); std::this_thread::sleep_for(wait_time); } std::thread t([this, sensor_configuration] { - if(sensor_configuration->sensor_model == SensorModel::HESAI_PANDAR40P - || sensor_configuration->sensor_model == SensorModel::HESAI_PANDAR64 - || sensor_configuration->sensor_model == SensorModel::HESAI_PANDARQT64 - || sensor_configuration->sensor_model == SensorModel::HESAI_PANDARXT32 - || sensor_configuration->sensor_model == SensorModel::HESAI_PANDARXT32M) { + if ( + sensor_configuration->sensor_model == SensorModel::HESAI_PANDAR40P || + sensor_configuration->sensor_model == SensorModel::HESAI_PANDAR64 || + sensor_configuration->sensor_model == SensorModel::HESAI_PANDARQT64 || + sensor_configuration->sensor_model == SensorModel::HESAI_PANDARXT32 || + sensor_configuration->sensor_model == SensorModel::HESAI_PANDARXT32M) { PrintInfo("Trying to set Clock source to PTP"); SetClockSource(HESAI_LIDAR_PTP_CLOCK_SOURCE); } @@ -3036,40 +1243,39 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( << ", Transport: " << sensor_configuration->ptp_transport_type << ", Switch Type: " << sensor_configuration->ptp_switch_type << " via TCP"; PrintInfo(tmp_ostringstream.str()); - SetPtpConfig(static_cast(sensor_configuration->ptp_profile), - sensor_configuration->ptp_domain, - static_cast(sensor_configuration->ptp_transport_type), - static_cast(sensor_configuration->ptp_switch_type), - PTP_LOG_ANNOUNCE_INTERVAL, - PTP_SYNC_INTERVAL, - PTP_LOG_MIN_DELAY_INTERVAL - ); + SetPtpConfig( + static_cast(sensor_configuration->ptp_profile), sensor_configuration->ptp_domain, + static_cast(sensor_configuration->ptp_transport_type), + static_cast(sensor_configuration->ptp_switch_type), PTP_LOG_ANNOUNCE_INTERVAL, + PTP_SYNC_INTERVAL, PTP_LOG_MIN_DELAY_INTERVAL); + PrintDebug("Setting properties done"); }); + PrintDebug("Waiting for thread to finish"); + t.join(); + PrintDebug("Thread finished"); + std::this_thread::sleep_for(wait_time); - } - else { //AT128 only supports PTP setup via HTTP + } else { // AT128 only supports PTP setup via HTTP PrintInfo("Trying to set SyncAngle via HTTP"); - SetSyncAngleSyncHttp(1, - static_cast(sensor_configuration->scan_phase)); + SetSyncAngleSyncHttp(1, static_cast(sensor_configuration->scan_phase)); std::ostringstream tmp_ostringstream; tmp_ostringstream << "Trying to set PTP Config: " << sensor_configuration->ptp_profile - << ", Domain: " << sensor_configuration->ptp_domain - << ", Transport: " << sensor_configuration->ptp_transport_type << " via HTTP"; + << ", Domain: " << sensor_configuration->ptp_domain + << ", Transport: " << sensor_configuration->ptp_transport_type << " via HTTP"; PrintInfo(tmp_ostringstream.str()); - SetPtpConfigSyncHttp(static_cast(sensor_configuration->ptp_profile), - sensor_configuration->ptp_domain, - static_cast(sensor_configuration->ptp_transport_type), - PTP_LOG_ANNOUNCE_INTERVAL, - PTP_SYNC_INTERVAL, - PTP_LOG_MIN_DELAY_INTERVAL); - + SetPtpConfigSyncHttp( + static_cast(sensor_configuration->ptp_profile), sensor_configuration->ptp_domain, + static_cast(sensor_configuration->ptp_transport_type), PTP_LOG_ANNOUNCE_INTERVAL, + PTP_SYNC_INTERVAL, PTP_LOG_MIN_DELAY_INTERVAL); } #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << "End CheckAndSetConfig(HesaiConfig)!!" << std::endl; #endif - return Status::WAITING_FOR_SENSOR_RESPONSE; + PrintDebug("GetAndCheckConfig(HesaiConfig) finished"); + + return Status::OK; } HesaiStatus HesaiHwInterface::CheckAndSetConfig( @@ -3130,78 +1336,30 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig() #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << "Start CheckAndSetConfig!!" << std::endl; #endif - if (true) { - std::thread t([this] { - GetConfig( // ctx, - [this](HesaiConfig & result) { - std::stringstream ss; - ss << result; - PrintInfo(ss.str()); - CheckAndSetConfig( - std::static_pointer_cast(sensor_configuration_), result); - }); - }); - t.join(); - - std::thread t2([this] { - GetLidarRange( // ctx, - [this](HesaiLidarRangeAll & result) { - std::stringstream ss; - ss << result; - PrintInfo(ss.str()); - CheckAndSetConfig( - std::static_pointer_cast(sensor_configuration_), result); - }); - }); - t2.join(); - } else if (false) { - GetConfig([this](HesaiConfig & result) { - std::cout << result << std::endl; - CheckAndSetConfig( - std::static_pointer_cast(sensor_configuration_), result); - }); - GetLidarRange([this](HesaiLidarRangeAll & result) { - std::cout << result << std::endl; - CheckAndSetConfig( - std::static_pointer_cast(sensor_configuration_), result); - }); - std::cout << "make thread t2" << std::endl; - std::thread t2([this] { - tcp_driver_->GetIOContext()->restart(); - tcp_driver_->run(); - }); - std::cout << "made thread t2" << std::endl; - t2.join(); - std::cout << "joined thread t2" << std::endl; + std::thread t([this] { + auto result = GetConfig(); - } else { - bool stopped = tcp_driver_->GetIOContext()->stopped(); - std::cout << "stopped: " << stopped << std::endl; - if (stopped) tcp_driver_->GetIOContext()->restart(); - GetConfig( - [this](HesaiConfig & result) { - std::cout << result << std::endl; - CheckAndSetConfig( - std::static_pointer_cast(sensor_configuration_), result); - }, - false); - if (stopped) tcp_driver_->run(); - stopped = tcp_driver_->GetIOContext()->stopped(); - std::cout << "stopped2: " << stopped << std::endl; - if (stopped) tcp_driver_->GetIOContext()->restart(); - GetLidarRange( - [this](HesaiLidarRangeAll & result) { - std::cout << result << std::endl; - CheckAndSetConfig( - std::static_pointer_cast(sensor_configuration_), result); - }, - false); - if (stopped) tcp_driver_->run(); - } + std::stringstream ss; + ss << result; + PrintInfo(ss.str()); + CheckAndSetConfig( + std::static_pointer_cast(sensor_configuration_), result); + }); + t.join(); + + std::thread t2([this] { + auto result = GetLidarRange(); + std::stringstream ss; + ss << result; + PrintInfo(ss.str()); + CheckAndSetConfig( + std::static_pointer_cast(sensor_configuration_), result); + }); + t2.join(); #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << "End CheckAndSetConfig!!" << std::endl; #endif - return Status::WAITING_FOR_SENSOR_RESPONSE; + return Status::OK; } /* @@ -3216,17 +1374,17 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig() 32: QT128C2X 38: ? 40: AT128? +42: OT128 48: ? */ int HesaiHwInterface::NebulaModelToHesaiModelNo(nebula::drivers::SensorModel model) { - switch (model) { case SensorModel::HESAI_PANDAR40P: return 0; case SensorModel::HESAI_PANDAR64: return 2; - case SensorModel::HESAI_PANDARQT64://check required + case SensorModel::HESAI_PANDARQT64: // check required return 15; case SensorModel::HESAI_PANDAR40M: return 17; @@ -3236,9 +1394,9 @@ int HesaiHwInterface::NebulaModelToHesaiModelNo(nebula::drivers::SensorModel mod return 32; case SensorModel::HESAI_PANDARXT32M: return 38; - case SensorModel::HESAI_PANDAR128_E3X://check required + case SensorModel::HESAI_PANDAR128_E3X: // check required return 40; - case SensorModel::HESAI_PANDAR128_E4X://OT128 + case SensorModel::HESAI_PANDAR128_E4X: // OT128 return 42; case SensorModel::HESAI_PANDARAT128: return 48; @@ -3247,8 +1405,14 @@ int HesaiHwInterface::NebulaModelToHesaiModelNo(nebula::drivers::SensorModel mod return -1; } } -void HesaiHwInterface::SetTargetModel(int model) { target_model_no = model; } -void HesaiHwInterface::SetTargetModel(nebula::drivers::SensorModel model) { target_model_no = NebulaModelToHesaiModelNo(model); } +void HesaiHwInterface::SetTargetModel(int model) +{ + target_model_no = model; +} +void HesaiHwInterface::SetTargetModel(nebula::drivers::SensorModel model) +{ + target_model_no = NebulaModelToHesaiModelNo(model); +} bool HesaiHwInterface::UseHttpSetSpinRate(int model) { @@ -3291,7 +1455,10 @@ bool HesaiHwInterface::UseHttpSetSpinRate(int model) break; } } -bool HesaiHwInterface::UseHttpSetSpinRate() { return UseHttpSetSpinRate(target_model_no); } +bool HesaiHwInterface::UseHttpSetSpinRate() +{ + return UseHttpSetSpinRate(target_model_no); +} bool HesaiHwInterface::UseHttpGetLidarMonitor(int model) { switch (model) { @@ -3333,66 +1500,9 @@ bool HesaiHwInterface::UseHttpGetLidarMonitor(int model) break; } } -bool HesaiHwInterface::UseHttpGetLidarMonitor() { return UseHttpGetLidarMonitor(target_model_no); } - -bool HesaiHwInterface::CheckLock( - std::timed_mutex & tm, int & fail_cnt, const int & fail_cnt_max, std::string name) -{ - if (wl) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE -// std::chrono::time_point start_time = std::chrono::steady_clock::now(); - std::chrono::system_clock::time_point start_time = std::chrono::system_clock::now(); - std::time_t stt = std::chrono::system_clock::to_time_t(start_time); - const std::tm* system_local_time = std::localtime(&stt); - std::cout << "try_lock_for: start at " << std::put_time(system_local_time, "%c") << std::endl; -/* - std::chrono::system_clock::time_point test_time = std::chrono::system_clock::now(); - std::time_t tst = std::chrono::system_clock::to_time_t(test_time); - const std::tm* local_time = std::localtime(&tst); - std::cerr << "try_lock_for: test at " << std::put_time(local_time, "%c") << std::endl; - auto dur_test = test_time - start_time; - std::cerr << "test: " << name << " : " << std::chrono::duration_cast(dur_test).count() << std::endl; -*/ -#endif - if (!tm.try_lock_for(std::chrono::milliseconds(timeout_))) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "if (!tm.try_lock_for(std::chrono::milliseconds(" << timeout_ << "))) {" << std::endl; - std::chrono::system_clock::time_point end_time = std::chrono::system_clock::now(); - std::time_t edt = std::chrono::system_clock::to_time_t(end_time); - const std::tm* end_local_time = std::localtime(&edt); - std::cerr << "try_lock_for: end at " << std::put_time(end_local_time, "%c") << std::endl; - auto dur = end_time - start_time; - std::cerr << "timeout: " << name << " : " << std::chrono::duration_cast(dur).count() << std::endl; -#endif - PrintDebug("timeout: " + name); - fail_cnt++; - if (fail_cnt_max < fail_cnt) { - tm.unlock(); - if (tcp_driver_ && tcp_driver_->isOpen()) { - tcp_driver_->closeSync(); - tcp_driver_->open(); - } - if (tcp_driver_s_ && tcp_driver_s_->isOpen()) { - tcp_driver_s_->closeSync(); - tcp_driver_s_->open(); - } - } - else { - return true; - } - return false; - } - fail_cnt = 0; - } - return true; -} - -void HesaiHwInterface::CheckUnlock(std::timed_mutex & tm, std::string name) +bool HesaiHwInterface::UseHttpGetLidarMonitor() { - if (wl) { - tm.unlock(); - PrintDebug(name + ": finished"); - } + return UseHttpGetLidarMonitor(target_model_no); } void HesaiHwInterface::SetLogger(std::shared_ptr logger) diff --git a/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp index 0152fa772..6f27d547e 100644 --- a/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp @@ -305,30 +305,26 @@ Status HesaiDriverRosWrapper::GetParameters( << 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(false) == Status::OK) { - hw_interface_.GetLidarCalibrationFromSensor( - [this, &calibration_configuration, &calibration_file_path_from_sensor]( - const std::string &str) { - 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"); - } - }, - true); + 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; } @@ -398,34 +394,32 @@ Status HesaiDriverRosWrapper::GetParameters( 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(false) == Status::OK) { - RCLCPP_INFO_STREAM( - this->get_logger(), "Trying to acquire calibration data from sensor"); - hw_interface_.GetLidarCalibrationFromSensor( - [this, &correction_configuration, &correction_file_path_from_sensor, &run_local](const std::vector & received_bytes) { - 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; - } - }); + 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; diff --git a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp index 8a6060d21..381a214f0 100644 --- a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp @@ -22,7 +22,7 @@ HesaiHwInterfaceRosWrapper::HesaiHwInterfaceRosWrapper(const rclcpp::NodeOptions hw_interface_.SetSensorConfiguration( std::static_pointer_cast(sensor_cfg_ptr)); #if not defined(TEST_PCAP) - Status rt = hw_interface_.InitializeTcpDriver(this->setup_sensor); + Status rt = hw_interface_.InitializeTcpDriver(); if(this->retry_hw_) { int cnt = 0; @@ -32,7 +32,7 @@ HesaiHwInterfaceRosWrapper::HesaiHwInterfaceRosWrapper(const rclcpp::NodeOptions 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(this->setup_sensor); + rt = hw_interface_.InitializeTcpDriver(); } } @@ -40,11 +40,9 @@ HesaiHwInterfaceRosWrapper::HesaiHwInterfaceRosWrapper(const rclcpp::NodeOptions try{ std::vector thread_pool{}; thread_pool.emplace_back([this] { - hw_interface_.GetInventory( // ios, - [this](HesaiInventory & result) { - RCLCPP_INFO_STREAM(get_logger(), result); - hw_interface_.SetTargetModel(result.model); - }); + auto result = hw_interface_.GetInventory(); + RCLCPP_INFO_STREAM(get_logger(), result); + hw_interface_.SetTargetModel(result.model); }); for (std::thread & th : thread_pool) { th.join(); @@ -144,6 +142,7 @@ HesaiHwInterfaceRosWrapper::HesaiHwInterfaceRosWrapper(const rclcpp::NodeOptions } #endif + RCLCPP_DEBUG(this->get_logger(), "Starting stream"); StreamStart(); } diff --git a/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp index 82be03022..239f6b8b0 100644 --- a/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp @@ -61,25 +61,23 @@ HesaiHwMonitorRosWrapper::HesaiHwMonitorRosWrapper(const rclcpp::NodeOptions & o hw_interface_.SetLogger(std::make_shared(this->get_logger())); hw_interface_.SetSensorConfiguration( std::static_pointer_cast(sensor_cfg_ptr)); - while(hw_interface_.InitializeTcpDriver(false) == Status::ERROR_1) + 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] { - hw_interface_.GetInventory( // ios, - [this](HesaiInventory & result) { - 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(); - }); + 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(); @@ -413,12 +411,10 @@ void HesaiHwMonitorRosWrapper::OnHesaiStatusTimer() RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiStatusTimer" << std::endl); try { auto ios = std::make_shared(); - hw_interface_.GetLidarStatus( // ios, - [this](HesaiLidarStatus & result) { - std::scoped_lock lock(mtx_status); - current_status_time.reset(new rclcpp::Time(this->get_clock()->now())); - current_status.reset(new HesaiLidarStatus(result)); - }); + 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)"), @@ -459,11 +455,10 @@ void HesaiHwMonitorRosWrapper::OnHesaiLidarMonitorTimer() RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiLidarMonitorTimer"); try { auto ios = std::make_shared(); - hw_interface_.GetLidarMonitor([this](HesaiLidarMonitor & result) { - std::scoped_lock lock(mtx_lidar_monitor); - current_lidar_monitor_time.reset(new rclcpp::Time(this->get_clock()->now())); - current_monitor.reset(new HesaiLidarMonitor(result)); - }); + 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)"),