From 8275762691e147a484f763975627c9d0821fbee2 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 26 Nov 2024 20:26:40 +0900 Subject: [PATCH 1/2] refactor(hesai_hw_interface): case style fixes Signed-off-by: Max SCHMELLER --- .../hesai_hw_interface.hpp | 237 ++++++----- .../hesai_hw_interface.cpp | 382 +++++++++--------- nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 10 +- nebula_ros/src/hesai/hw_interface_wrapper.cpp | 16 +- nebula_ros/src/hesai/hw_monitor_wrapper.cpp | 14 +- 5 files changed, 324 insertions(+), 335 deletions(-) diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp index dc5103ab..9d25e2d7 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 @@ -50,77 +50,66 @@ namespace nebula::drivers { -const int PandarTcpCommandPort = 9347; -const uint8_t PTC_COMMAND_DUMMY_BYTE = 0x00; -const uint8_t PTC_COMMAND_HEADER_HIGH = 0x47; -const uint8_t PTC_COMMAND_HEADER_LOW = 0x74; -const uint8_t PTC_COMMAND_GET_LIDAR_CALIBRATION = 0x05; -const uint8_t PTC_COMMAND_PTP_DIAGNOSTICS = 0x06; -const uint8_t PTC_COMMAND_PTP_STATUS = 0x01; -const uint8_t PTC_COMMAND_PTP_PORT_DATA_SET = 0x02; -const uint8_t PTC_COMMAND_PTP_TIME_STATUS_NP = 0x03; -const uint8_t PTC_COMMAND_PTP_GRANDMASTER_SETTINGS_NP = 0x04; -const uint8_t PTC_COMMAND_GET_INVENTORY_INFO = 0x07; -const uint8_t PTC_COMMAND_GET_CONFIG_INFO = 0x08; -const uint8_t PTC_COMMAND_GET_LIDAR_STATUS = 0x09; -const uint8_t PTC_COMMAND_SET_SPIN_RATE = 0x17; -const uint8_t PTC_COMMAND_SET_SYNC_ANGLE = 0x18; -const uint8_t PTC_COMMAND_SET_TRIGGER_METHOD = 0x1b; -const uint8_t PTC_COMMAND_SET_STANDBY_MODE = 0x1c; -const uint8_t PTC_COMMAND_SET_RETURN_MODE = 0x1e; -const uint8_t PTC_COMMAND_SET_CLOCK_SOURCE = 0x1f; -const uint8_t PTC_COMMAND_SET_DESTINATION_IP = 0x20; -const uint8_t PTC_COMMAND_SET_CONTROL_PORT = 0x21; -const uint8_t PTC_COMMAND_SET_LIDAR_RANGE = 0x22; -const uint8_t PTC_COMMAND_GET_LIDAR_RANGE = 0x23; -const uint8_t PTC_COMMAND_SET_PTP_CONFIG = 0x24; -const uint8_t PTC_COMMAND_GET_PTP_CONFIG = 0x26; -const uint8_t PTC_COMMAND_RESET = 0x25; -const uint8_t PTC_COMMAND_SET_ROTATE_DIRECTION = 0x2a; -const uint8_t PTC_COMMAND_LIDAR_MONITOR = 0x27; - -const uint8_t PTC_ERROR_CODE_NO_ERROR = 0x00; -const uint8_t PTC_ERROR_CODE_INVALID_INPUT_PARAM = 0x01; -const uint8_t PTC_ERROR_CODE_SERVER_CONN_FAILED = 0x02; -const uint8_t PTC_ERROR_CODE_INVALID_DATA = 0x03; -const uint8_t PTC_ERROR_CODE_OUT_OF_MEMORY = 0x04; -const uint8_t PTC_ERROR_CODE_UNSUPPORTED_CMD = 0x05; -const uint8_t PTC_ERROR_CODE_FPGA_COMM_FAILED = 0x06; -const uint8_t PTC_ERROR_CODE_OTHER = 0x07; - -const uint8_t TCP_ERROR_UNRELATED_RESPONSE = 1; -const uint8_t TCP_ERROR_UNEXPECTED_PAYLOAD = 2; -const uint8_t TCP_ERROR_TIMEOUT = 4; -const uint8_t TCP_ERROR_INCOMPLETE_RESPONSE = 8; - -const uint16_t PANDARQT64_PACKET_SIZE = 1072; -const uint16_t PANDARQT128_PACKET_SIZE = 1127; -const uint16_t PANDARXT32_PACKET_SIZE = 1080; -const uint16_t PANDARXT32M_PACKET_SIZE = 820; -const uint16_t PANDARAT128_PACKET_SIZE = 1118; -const uint16_t PANDAR64_PACKET_SIZE = 1194; -const uint16_t PANDAR64_EXTENDED_PACKET_SIZE = 1198; -const uint16_t PANDAR40_PACKET_SIZE = 1262; -const uint16_t PANDAR40P_EXTENDED_PACKET_SIZE = 1266; -const uint16_t PANDAR128_E4X_PACKET_SIZE = 861; -const uint16_t PANDAR128_E4X_EXTENDED_PACKET_SIZE = 1117; -const uint16_t MTU_SIZE = 1500; +const int g_pandar_tcp_command_port = 9347; +const uint8_t g_ptc_command_dummy_byte = 0x00; +const uint8_t g_ptc_command_header_high = 0x47; +const uint8_t g_ptc_command_header_low = 0x74; +const uint8_t g_ptc_command_get_lidar_calibration = 0x05; +const uint8_t g_ptc_command_ptp_diagnostics = 0x06; +const uint8_t g_ptc_command_ptp_status = 0x01; +const uint8_t g_ptc_command_ptp_port_data_set = 0x02; +const uint8_t g_ptc_command_ptp_time_status_np = 0x03; +const uint8_t g_ptc_command_ptp_grandmaster_settings_np = 0x04; +const uint8_t g_ptc_command_get_inventory_info = 0x07; +const uint8_t g_ptc_command_get_config_info = 0x08; +const uint8_t g_ptc_command_get_lidar_status = 0x09; +const uint8_t g_ptc_command_set_spin_rate = 0x17; +const uint8_t g_ptc_command_set_sync_angle = 0x18; +const uint8_t g_ptc_command_set_trigger_method = 0x1b; +const uint8_t g_ptc_command_set_standby_mode = 0x1c; +const uint8_t g_ptc_command_set_return_mode = 0x1e; +const uint8_t g_ptc_command_set_clock_source = 0x1f; +const uint8_t g_ptc_command_set_destination_ip = 0x20; +const uint8_t g_ptc_command_set_control_port = 0x21; +const uint8_t g_ptc_command_set_lidar_range = 0x22; +const uint8_t g_ptc_command_get_lidar_range = 0x23; +const uint8_t g_ptc_command_set_ptp_config = 0x24; +const uint8_t g_ptc_command_get_ptp_config = 0x26; +const uint8_t g_ptc_command_reset = 0x25; +const uint8_t g_ptc_command_set_rotate_direction = 0x2a; +const uint8_t g_ptc_command_lidar_monitor = 0x27; + +const uint8_t g_ptc_error_code_no_error = 0x00; +const uint8_t g_ptc_error_code_invalid_input_param = 0x01; +const uint8_t g_ptc_error_code_server_conn_failed = 0x02; +const uint8_t g_ptc_error_code_invalid_data = 0x03; +const uint8_t g_ptc_error_code_out_of_memory = 0x04; +const uint8_t g_ptc_error_code_unsupported_cmd = 0x05; +const uint8_t g_ptc_error_code_fpga_comm_failed = 0x06; +const uint8_t g_ptc_error_code_other = 0x07; + +const uint8_t g_tcp_error_unrelated_response = 1; +const uint8_t g_tcp_error_unexpected_payload = 2; +const uint8_t g_tcp_error_timeout = 4; +const uint8_t g_tcp_error_incomplete_response = 8; + +const uint16_t g_mtu_size = 1500; /// @brief The kernel buffer size in bytes to use for receiving UDP packets. If the buffer is too /// small to bridge scheduling and processing delays, packets will be dropped. This corresponds to /// the net.core.rmem_default setting in Linux. The current value is hardcoded to accommodate one /// pointcloud worth of OT128 packets (currently the highest data rate sensor supported). -const size_t UDP_SOCKET_BUFFER_SIZE = MTU_SIZE * 3600; +const size_t g_udp_socket_buffer_size = g_mtu_size * 3600; // Time interval between Announce messages, in units of log seconds (default: 1) -const int PTP_LOG_ANNOUNCE_INTERVAL = 1; +const int g_ptp_log_announce_interval = 1; // Time interval between Sync messages, in units of log seconds (default: 1) -const int PTP_SYNC_INTERVAL = 1; +const int g_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 g_ptp_log_min_delay_interval = 0; -const int HESAI_LIDAR_GPS_CLOCK_SOURCE = 0; -const int HESAI_LIDAR_PTP_CLOCK_SOURCE = 1; +const int g_hesai_lidar_gps_clock_source = 0; +const int g_hesai_lidar_ptp_clock_source = 1; /// @brief Hardware interface of hesai driver class HesaiHwInterface @@ -138,7 +127,7 @@ class HesaiHwInterface std::shared_ptr logger_; std::unique_ptr<::drivers::common::IoContext> cloud_io_context_; - std::shared_ptr m_owned_ctx; + std::shared_ptr m_owned_ctx_; std::unique_ptr<::drivers::udp_driver::UdpDriver> cloud_udp_driver_; std::shared_ptr<::drivers::tcp_driver::TcpDriver> tcp_driver_; std::shared_ptr sensor_configuration_; @@ -147,20 +136,20 @@ class HesaiHwInterface std::mutex mtx_inflight_tcp_request_; - int target_model_no; + int target_model_no_; /// @brief Get a one-off HTTP client to communicate with the hardware /// @param ctx IO Context /// @param hcd Got http client driver /// @return Resulting status - HesaiStatus GetHttpClientDriverOnce( + HesaiStatus get_http_client_driver_once( std::shared_ptr ctx, std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> & hcd); /// @brief Get a one-off HTTP client to communicate with the hardware (without specifying /// io_context) /// @param hcd Got http client driver /// @return Resulting status - HesaiStatus GetHttpClientDriverOnce( + HesaiStatus get_http_client_driver_once( std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> & hcd); /// @brief A callback that receives a string (just prints) /// @param str Received string @@ -170,20 +159,20 @@ class HesaiHwInterface /// @param error_code The error code, containing the sensor's error code (if any), along with /// flags such as TCP_ERROR_UNRELATED_RESPONSE etc. /// @return A string description of all errors in this code - std::string PrettyPrintPTCError(ptc_error_t error_code); + std::string pretty_print_ptc_error(ptc_error_t error_code); /// @brief Checks if the data size matches that of the struct to be parsed, and parses the struct. /// If data is too small, a std::runtime_error is thrown. If data is too large, a warning is /// printed and the struct is parsed with the first sizeof(T) bytes. template - T CheckSizeAndParse(const std::vector & data); + T check_size_and_parse(const std::vector & data); /// @brief Send a PTC request with an optional payload, and return the full response payload. /// Blocking. /// @param command_id PTC command number. /// @param payload Payload bytes of the PTC command. Not including the 8-byte PTC header. /// @return The returned payload, if successful, or nullptr. - ptc_cmd_result_t SendReceive(const uint8_t command_id, const std::vector & payload = {}); + ptc_cmd_result_t send_receive(const uint8_t command_id, const std::vector & payload = {}); static std::pair unwrap_http_response(const std::string & response); @@ -195,89 +184,89 @@ class HesaiHwInterface /// @brief Initializing tcp_driver for TCP communication /// @param setup_sensor Whether to also initialize tcp_driver for sensor configuration /// @return Resulting status - Status InitializeTcpDriver(); + Status initialize_tcp_driver(); /// @brief Closes the TcpDriver and related resources /// @return Status result - Status FinalizeTcpDriver(); + Status finalize_tcp_driver(); /// @brief Parsing json string to property_tree /// @param str JSON string /// @return Parsed property_tree - boost::property_tree::ptree ParseJson(const std::string & str); + boost::property_tree::ptree parse_json(const std::string & str); /// @brief Callback function to receive the Cloud Packet data from the UDP Driver /// @param buffer Buffer containing the data received from the UDP socket - void ReceiveSensorPacketCallback(std::vector & buffer); + void receive_sensor_packet_callback(std::vector & buffer); /// @brief Starting the interface that handles UDP streams /// @return Resulting status - Status SensorInterfaceStart(); + Status sensor_interface_start(); /// @brief Function for stopping the interface that handles UDP streams /// @return Resulting status - Status SensorInterfaceStop(); + Status sensor_interface_stop(); /// @brief Printing sensor configuration /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status - Status GetSensorConfiguration(const SensorConfigurationBase & sensor_configuration); + Status get_sensor_configuration(const SensorConfigurationBase & sensor_configuration); /// @brief Printing calibration configuration /// @param calibration_configuration CalibrationConfiguration for the checking /// @return Resulting status - Status GetCalibrationConfiguration(CalibrationConfigurationBase & calibration_configuration); + Status get_calibration_configuration(CalibrationConfigurationBase & calibration_configuration); /// @brief Setting sensor configuration /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status - Status SetSensorConfiguration( + Status set_sensor_configuration( std::shared_ptr sensor_configuration); /// @brief Registering callback for PandarScan /// @param scan_callback Callback function /// @return Resulting status - Status RegisterScanCallback(std::function &)> scan_callback); + Status register_scan_callback(std::function &)> scan_callback); /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION /// @return Resulting status - std::string GetLidarCalibrationString(); + std::string get_lidar_calibration_string(); /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION /// @return Resulting status - std::vector GetLidarCalibrationBytes(); + std::vector get_lidar_calibration_bytes(); /// @brief Getting data with PTC_COMMAND_PTP_DIAGNOSTICS (PTP STATUS) /// @return Resulting status - HesaiPtpDiagStatus GetPtpDiagStatus(); + HesaiPtpDiagStatus get_ptp_diag_status(); /// @brief Getting data with PTC_COMMAND_PTP_DIAGNOSTICS (PTP TLV PORT_DATA_SET) /// @return Resulting status - HesaiPtpDiagPort GetPtpDiagPort(); + HesaiPtpDiagPort get_ptp_diag_port(); /// @brief Getting data with PTC_COMMAND_PTP_DIAGNOSTICS (PTP TLV TIME_STATUS_NP) /// @return Resulting status - HesaiPtpDiagTime GetPtpDiagTime(); + HesaiPtpDiagTime get_ptp_diag_time(); /// @brief Getting data with PTC_COMMAND_PTP_DIAGNOSTICS (PTP TLV GRANDMASTER_SETTINGS_NP) /// @return Resulting status - HesaiPtpDiagGrandmaster GetPtpDiagGrandmaster(); + HesaiPtpDiagGrandmaster get_ptp_diag_grandmaster(); /// @brief Getting data with PTC_COMMAND_GET_INVENTORY_INFO /// @return Resulting status - std::shared_ptr GetInventory(); + std::shared_ptr get_inventory(); /// @brief Getting data with PTC_COMMAND_GET_CONFIG_INFO /// @return Resulting status - std::shared_ptr GetConfig(); + std::shared_ptr get_config(); /// @brief Getting data with PTC_COMMAND_GET_LIDAR_STATUS /// @return Resulting status - std::shared_ptr GetLidarStatus(); + std::shared_ptr get_lidar_status(); /// @brief Setting value with PTC_COMMAND_SET_SPIN_RATE /// @param rpm Spin rate /// @return Resulting status - Status SetSpinRate(uint16_t rpm); + Status set_spin_rate(uint16_t rpm); /// @brief Setting value with PTC_COMMAND_SET_SYNC_ANGLE /// @param sync_angle Sync angle enable flag /// @param angle Angle value /// @return Resulting status - Status SetSyncAngle(int sync_angle, int angle); + Status set_sync_angle(int sync_angle, int angle); /// @brief Setting mode with PTC_COMMAND_SET_TRIGGER_METHOD /// @param trigger_method Trigger method /// @return Resulting status - Status SetTriggerMethod(int trigger_method); + Status set_trigger_method(int trigger_method); /// @brief Setting mode with PTC_COMMAND_SET_STANDBY_MODE /// @param standby_mode Standby mode /// @return Resulting status - Status SetStandbyMode(int standby_mode); + Status set_standby_mode(int standby_mode); /// @brief Setting mode with PTC_COMMAND_SET_RETURN_MODE /// @param return_mode Return mode /// @return Resulting status - Status SetReturnMode(int return_mode); + Status set_return_mode(int return_mode); /// @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 @@ -286,7 +275,7 @@ class HesaiHwInterface /// @param port LiDAR Destination Port /// @param gps_port GPS Destination Port /// @return Resulting status - Status SetDestinationIp( + Status set_destination_ip( 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 ip_1 Device IP of the 1st byte represents the 1st section @@ -304,22 +293,22 @@ class HesaiHwInterface /// @param vlan_flg VLAN Status /// @param vlan_id VLAN ID /// @return Resulting status - Status SetControlPort( + Status set_control_port( 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); /// @brief Setting values with PTC_COMMAND_SET_LIDAR_RANGE /// @param method Method /// @param data Set data /// @return Resulting status - Status SetLidarRange(int method, std::vector data); + Status set_lidar_range(int method, std::vector data); /// @brief Setting values with PTC_COMMAND_SET_LIDAR_RANGE /// @param start Start angle /// @param end End angle /// @return Resulting status - Status SetLidarRange(int start, int end); + Status set_lidar_range(int start, int end); /// @brief Getting values with PTC_COMMAND_GET_LIDAR_RANGE /// @return Resulting status - HesaiLidarRangeAll GetLidarRange(); + HesaiLidarRangeAll get_lidar_range(); /** * @brief Given the HW interface's sensor configuration and a given calibration, set the sensor @@ -329,9 +318,9 @@ class HesaiHwInterface * @param calibration The calibration file of the sensor * @return Status Resulting status of setting the FoV */ - [[nodiscard]] Status checkAndSetLidarRange(const HesaiCalibrationConfigurationBase & calibration); + [[nodiscard]] Status check_and_set_lidar_range(const HesaiCalibrationConfigurationBase & calibration); - Status SetClockSource(int clock_source); + Status set_clock_source(int clock_source); /// @brief Setting values with PTC_COMMAND_SET_PTP_CONFIG /// @param profile IEEE timing and synchronization standard @@ -345,108 +334,108 @@ class HesaiHwInterface /// @param logMinDelayReqInterval Minimum permitted mean time between Delay_Req messages, in units /// of log seconds (default: 0) /// @return Resulting status - Status SetPtpConfig( + Status set_ptp_config( 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 /// @return Resulting status - HesaiPtpConfig GetPtpConfig(); + HesaiPtpConfig get_ptp_config(); /// @brief Sending command with PTC_COMMAND_RESET /// @return Resulting status - Status SendReset(); + Status send_reset(); /// @brief Setting values with PTC_COMMAND_SET_ROTATE_DIRECTION /// @param mode Rotation of the motor /// @return Resulting status - Status SetRotDir(int mode); + Status set_rot_dir(int mode); /// @brief Getting data with PTC_COMMAND_LIDAR_MONITOR /// @return Resulting status - HesaiLidarMonitor GetLidarMonitor(); + HesaiLidarMonitor get_lidar_monitor(); /// @brief Call run() of IO Context - void IOContextRun(); + void io_context_run(); /// @brief GetIO Context /// @return IO Context - std::shared_ptr GetIOContext(); + std::shared_ptr get_io_context(); /// @brief Setting spin_speed via HTTP API /// @param ctx IO Context used /// @param rpm spin_speed (300, 600, 1200) /// @return Resulting status - HesaiStatus SetSpinSpeedAsyncHttp(std::shared_ptr ctx, uint16_t rpm); + HesaiStatus set_spin_speed_async_http(std::shared_ptr ctx, uint16_t rpm); /// @brief Setting spin_speed via HTTP API /// @param rpm spin_speed (300, 600, 1200) /// @return Resulting status - HesaiStatus SetSpinSpeedAsyncHttp(uint16_t rpm); + HesaiStatus set_spin_speed_async_http(uint16_t rpm); - HesaiStatus SetPtpConfigSyncHttp( + HesaiStatus set_ptp_config_sync_http( std::shared_ptr ctx, int profile, int domain, int network, int logAnnounceInterval, int logSyncInterval, int logMinDelayReqInterval); - HesaiStatus SetPtpConfigSyncHttp( + HesaiStatus set_ptp_config_sync_http( int profile, int domain, int network, int logAnnounceInterval, int logSyncInterval, int logMinDelayReqInterval); - HesaiStatus SetSyncAngleSyncHttp( + HesaiStatus set_sync_angle_sync_http( std::shared_ptr ctx, int enable, int angle); - HesaiStatus SetSyncAngleSyncHttp(int enable, int angle); + HesaiStatus set_sync_angle_sync_http(int enable, int angle); /// @brief Getting lidar_monitor via HTTP API /// @param ctx IO Context /// @param str_callback Callback function for received string /// @return Resulting status - HesaiStatus GetLidarMonitorAsyncHttp( + HesaiStatus get_lidar_monitor_async_http( std::shared_ptr ctx, std::function str_callback); /// @brief Getting lidar_monitor via HTTP API /// @param str_callback Callback function for received string /// @return Resulting status - HesaiStatus GetLidarMonitorAsyncHttp(std::function str_callback); + HesaiStatus get_lidar_monitor_async_http(std::function str_callback); /// @brief Checking the current settings and changing the difference point /// @param sensor_configuration Current SensorConfiguration /// @param hesai_config Current HesaiConfig /// @return Resulting status - HesaiStatus CheckAndSetConfig( + HesaiStatus check_and_set_config( std::shared_ptr sensor_configuration, std::shared_ptr hesai_config); /// @brief Checking the current settings and changing the difference point /// @param sensor_configuration Current SensorConfiguration /// @param hesai_lidar_range_all Current HesaiLidarRangeAll /// @return Resulting status - HesaiStatus CheckAndSetConfig( + HesaiStatus check_and_set_config( std::shared_ptr sensor_configuration, HesaiLidarRangeAll hesai_lidar_range_all); /// @brief Checking the current settings and changing the difference point /// @return Resulting status - HesaiStatus CheckAndSetConfig(); + HesaiStatus check_and_set_config(); /// @brief Convert to model in Hesai protocol from nebula::drivers::SensorModel /// @param model /// @return - int NebulaModelToHesaiModelNo(nebula::drivers::SensorModel model); + int nebula_model_to_hesai_model_no(nebula::drivers::SensorModel model); /// @brief Set target model number (for proper use of HTTP and TCP according to the support of the /// target model) /// @param model Model number - void SetTargetModel(int model); + void set_target_model(int model); /// @brief Set target model number (for proper use of HTTP and TCP according to the support of the /// target model) /// @param model Model - void SetTargetModel(nebula::drivers::SensorModel model); + void set_target_model(nebula::drivers::SensorModel model); /// @brief Whether to use HTTP for setting SpinRate /// @param model Model number /// @return Use HTTP - bool UseHttpSetSpinRate(int model); + bool use_http_set_spin_rate(int model); /// @brief Whether to use HTTP for setting SpinRate /// @return Use HTTP - bool UseHttpSetSpinRate(); + bool use_http_set_spin_rate(); /// @brief Whether to use HTTP for getting LidarMonitor /// @param model Model number /// @return Use HTTP - bool UseHttpGetLidarMonitor(int model); + bool use_http_get_lidar_monitor(int model); /// @brief Whether to use HTTP for getting LidarMonitor /// @return Use HTTP - bool UseHttpGetLidarMonitor(); + bool use_http_get_lidar_monitor(); }; } // namespace nebula::drivers 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 29302bbc..87815fc5 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 @@ -39,19 +39,19 @@ using nlohmann::json; HesaiHwInterface::HesaiHwInterface(const std::shared_ptr & logger) : logger_(logger), cloud_io_context_{new ::drivers::common::IoContext(1)}, - m_owned_ctx{new boost::asio::io_context(1)}, + m_owned_ctx_{new boost::asio::io_context(1)}, cloud_udp_driver_{new ::drivers::udp_driver::UdpDriver(*cloud_io_context_)}, - tcp_driver_{new ::drivers::tcp_driver::TcpDriver(m_owned_ctx)}, - target_model_no(NebulaModelToHesaiModelNo(SensorModel::UNKNOWN)) + tcp_driver_{new ::drivers::tcp_driver::TcpDriver(m_owned_ctx_)}, + target_model_no_(nebula_model_to_hesai_model_no(SensorModel::UNKNOWN)) { } HesaiHwInterface::~HesaiHwInterface() { - FinalizeTcpDriver(); + finalize_tcp_driver(); } -HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( +HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::send_receive( const uint8_t command_id, const std::vector & payload) { std::lock_guard lock(mtx_inflight_tcp_request_); @@ -59,10 +59,10 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( 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(g_ptc_command_header_high); + send_buf.emplace_back(g_ptc_command_header_low); send_buf.emplace_back(command_id); - send_buf.emplace_back(PTC_COMMAND_DUMMY_BYTE); + send_buf.emplace_back(g_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); @@ -106,7 +106,7 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( // rubbish), probably as a result of too many simultaneous TCP connections to the sensor (e.g. // from GUI, Web UI, another nebula instance, etc.) if (header_bytes[2] != command_id) { - error_code->error_flags |= TCP_ERROR_UNRELATED_RESPONSE; + error_code->error_flags |= g_tcp_error_unrelated_response; } if (payload_len == 0) { *response_complete = true; @@ -119,7 +119,7 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( // Header had payload length 0 (thus, header callback processed request successfully already), // but we still received a payload: invalid state if (*response_complete == true) { - error_code->error_flags |= TCP_ERROR_UNEXPECTED_PAYLOAD; + error_code->error_flags |= g_tcp_error_unexpected_payload; return; } @@ -132,16 +132,16 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( tm.unlock(); logger_->debug(log_tag + "Unlocked mutex"); }); - this->IOContextRun(); + this->io_context_run(); if (!tm.try_lock_for(std::chrono::seconds(1))) { logger_->error(log_tag + "Request did not finish within 1s"); - error_code->error_flags |= TCP_ERROR_TIMEOUT; + error_code->error_flags |= g_tcp_error_timeout; return *error_code; } if (!response_complete) { logger_->error(log_tag + "Did not receive response"); - error_code->error_flags |= TCP_ERROR_INCOMPLETE_RESPONSE; + error_code->error_flags |= g_tcp_error_incomplete_response; return *error_code; } @@ -154,7 +154,7 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( return *recv_buf; } -Status HesaiHwInterface::SetSensorConfiguration( +Status HesaiHwInterface::set_sensor_configuration( std::shared_ptr sensor_configuration) { sensor_configuration_ = @@ -162,7 +162,7 @@ Status HesaiHwInterface::SetSensorConfiguration( return Status::OK; } -Status HesaiHwInterface::SensorInterfaceStart() +Status HesaiHwInterface::sensor_interface_start() { try { logger_->info("Starting UDP receiver"); @@ -183,11 +183,11 @@ Status HesaiHwInterface::SensorInterfaceStart() logger_->error("open ok"); #endif - bool success = cloud_udp_driver_->receiver()->setKernelBufferSize(UDP_SOCKET_BUFFER_SIZE); + bool success = cloud_udp_driver_->receiver()->setKernelBufferSize(g_udp_socket_buffer_size); if (!success) { logger_->error( "Could not set receive buffer size. Try increasing net.core.rmem_max to " + - std::to_string(UDP_SOCKET_BUFFER_SIZE) + " B."); + std::to_string(g_udp_socket_buffer_size) + " B."); return Status::ERROR_1; } @@ -197,7 +197,7 @@ Status HesaiHwInterface::SensorInterfaceStart() #endif cloud_udp_driver_->receiver()->asyncReceive( - std::bind(&HesaiHwInterface::ReceiveSensorPacketCallback, this, std::placeholders::_1)); + std::bind(&HesaiHwInterface::receive_sensor_packet_callback, this, std::placeholders::_1)); #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE logger_->error("async receive set"); #endif @@ -210,23 +210,23 @@ Status HesaiHwInterface::SensorInterfaceStart() return Status::OK; } -Status HesaiHwInterface::RegisterScanCallback( +Status HesaiHwInterface::register_scan_callback( std::function &)> scan_callback) { cloud_packet_callback_ = std::move(scan_callback); return Status::OK; } -void HesaiHwInterface::ReceiveSensorPacketCallback(std::vector & buffer) +void HesaiHwInterface::receive_sensor_packet_callback(std::vector & buffer) { cloud_packet_callback_(buffer); } -Status HesaiHwInterface::SensorInterfaceStop() +Status HesaiHwInterface::sensor_interface_stop() { return Status::ERROR_1; } -Status HesaiHwInterface::GetSensorConfiguration( +Status HesaiHwInterface::get_sensor_configuration( const SensorConfigurationBase & sensor_configuration) { std::stringstream ss; @@ -235,14 +235,14 @@ Status HesaiHwInterface::GetSensorConfiguration( return Status::ERROR_1; } -Status HesaiHwInterface::GetCalibrationConfiguration( +Status HesaiHwInterface::get_calibration_configuration( CalibrationConfigurationBase & calibration_configuration) { logger_->debug(calibration_configuration.calibration_file); return Status::ERROR_1; } -Status HesaiHwInterface::InitializeTcpDriver() +Status HesaiHwInterface::initialize_tcp_driver() { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << "HesaiHwInterface::InitializeTcpDriver" << std::endl; @@ -252,8 +252,8 @@ Status HesaiHwInterface::InitializeTcpDriver() std::cout << "PandarTcpCommandPort=" << PandarTcpCommandPort << std::endl; #endif tcp_driver_->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); + sensor_configuration_->sensor_ip, g_pandar_tcp_command_port, sensor_configuration_->host_ip, + g_pandar_tcp_command_port); #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << "ed: tcp_driver_->init_socket" << std::endl; #endif @@ -268,7 +268,7 @@ Status HesaiHwInterface::InitializeTcpDriver() return Status::OK; } -Status HesaiHwInterface::FinalizeTcpDriver() +Status HesaiHwInterface::finalize_tcp_driver() { try { if (tcp_driver_) { @@ -281,7 +281,7 @@ Status HesaiHwInterface::FinalizeTcpDriver() return Status::OK; } -boost::property_tree::ptree HesaiHwInterface::ParseJson(const std::string & str) +boost::property_tree::ptree HesaiHwInterface::parse_json(const std::string & str) { boost::property_tree::ptree tree; try { @@ -294,85 +294,85 @@ boost::property_tree::ptree HesaiHwInterface::ParseJson(const std::string & str) return tree; } -std::vector HesaiHwInterface::GetLidarCalibrationBytes() +std::vector HesaiHwInterface::get_lidar_calibration_bytes() { - auto response_or_err = SendReceive(PTC_COMMAND_GET_LIDAR_CALIBRATION); - return response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + auto response_or_err = send_receive(g_ptc_command_get_lidar_calibration); + return response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); } -std::string HesaiHwInterface::GetLidarCalibrationString() +std::string HesaiHwInterface::get_lidar_calibration_string() { - auto response_or_err = SendReceive(PTC_COMMAND_GET_LIDAR_CALIBRATION); + auto response_or_err = send_receive(g_ptc_command_get_lidar_calibration); auto calib_data = - response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); std::string calib_string(calib_data.begin(), calib_data.end()); return calib_string; } -HesaiPtpDiagStatus HesaiHwInterface::GetPtpDiagStatus() +HesaiPtpDiagStatus HesaiHwInterface::get_ptp_diag_status() { - auto response_or_err = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_STATUS}); - auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - auto diag_status = CheckSizeAndParse(response); + auto response_or_err = send_receive(g_ptc_command_ptp_diagnostics, {g_ptc_command_ptp_status}); + auto response = response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); + auto diag_status = check_size_and_parse(response); return diag_status; } -HesaiPtpDiagPort HesaiHwInterface::GetPtpDiagPort() +HesaiPtpDiagPort HesaiHwInterface::get_ptp_diag_port() { - auto response_or_err = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_PORT_DATA_SET}); - auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - auto diag_port = CheckSizeAndParse(response); + auto response_or_err = send_receive(g_ptc_command_ptp_diagnostics, {g_ptc_command_ptp_port_data_set}); + auto response = response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); + auto diag_port = check_size_and_parse(response); return diag_port; } -HesaiPtpDiagTime HesaiHwInterface::GetPtpDiagTime() +HesaiPtpDiagTime HesaiHwInterface::get_ptp_diag_time() { - auto response_or_err = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_TIME_STATUS_NP}); - auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - auto diag_time = CheckSizeAndParse(response); + auto response_or_err = send_receive(g_ptc_command_ptp_diagnostics, {g_ptc_command_ptp_time_status_np}); + auto response = response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); + auto diag_time = check_size_and_parse(response); return diag_time; } -HesaiPtpDiagGrandmaster HesaiHwInterface::GetPtpDiagGrandmaster() +HesaiPtpDiagGrandmaster HesaiHwInterface::get_ptp_diag_grandmaster() { auto response_or_err = - SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_GRANDMASTER_SETTINGS_NP}); - auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - auto diag_grandmaster = CheckSizeAndParse(response); + send_receive(g_ptc_command_ptp_diagnostics, {g_ptc_command_ptp_grandmaster_settings_np}); + auto response = response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); + auto diag_grandmaster = check_size_and_parse(response); return diag_grandmaster; } -std::shared_ptr HesaiHwInterface::GetInventory() +std::shared_ptr HesaiHwInterface::get_inventory() { - auto response_or_err = SendReceive(PTC_COMMAND_GET_INVENTORY_INFO); - auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + auto response_or_err = send_receive(g_ptc_command_get_inventory_info); + auto response = response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); switch (sensor_configuration_->sensor_model) { default: case SensorModel::HESAI_PANDARXT32: case SensorModel::HESAI_PANDAR40P: { - auto lidar_config = CheckSizeAndParse(response); + auto lidar_config = check_size_and_parse(response); return std::make_shared(lidar_config); } case SensorModel::HESAI_PANDARQT128: { - auto lidar_config = CheckSizeAndParse(response); + auto lidar_config = check_size_and_parse(response); return std::make_shared(lidar_config); } case SensorModel::HESAI_PANDARAT128: { - auto lidar_config = CheckSizeAndParse(response); + auto lidar_config = check_size_and_parse(response); return std::make_shared(lidar_config); } case SensorModel::HESAI_PANDAR128_E4X: { - auto lidar_config = CheckSizeAndParse(response); + auto lidar_config = check_size_and_parse(response); return std::make_shared(lidar_config); } } } -std::shared_ptr HesaiHwInterface::GetConfig() +std::shared_ptr HesaiHwInterface::get_config() { - auto response_or_err = SendReceive(PTC_COMMAND_GET_CONFIG_INFO); - auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + auto response_or_err = send_receive(g_ptc_command_get_config_info); + auto response = response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); switch (sensor_configuration_->sensor_model) { default: @@ -380,57 +380,57 @@ std::shared_ptr HesaiHwInterface::GetConfig() case SensorModel::HESAI_PANDAR64: case SensorModel::HESAI_PANDARQT128: case SensorModel::HESAI_PANDARXT32: { - auto lidar_config = CheckSizeAndParse(response); + auto lidar_config = check_size_and_parse(response); return std::make_shared(lidar_config); } case SensorModel::HESAI_PANDAR128_E4X: case SensorModel::HESAI_PANDARAT128: { - auto lidar_config = CheckSizeAndParse(response); + auto lidar_config = check_size_and_parse(response); return std::make_shared(lidar_config); } } } -std::shared_ptr HesaiHwInterface::GetLidarStatus() +std::shared_ptr HesaiHwInterface::get_lidar_status() { - auto response_or_err = SendReceive(PTC_COMMAND_GET_LIDAR_STATUS); - auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + auto response_or_err = send_receive(g_ptc_command_get_lidar_status); + auto response = response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); switch (sensor_configuration_->sensor_model) { default: case SensorModel::HESAI_PANDAR40P: case SensorModel::HESAI_PANDAR64: case SensorModel::HESAI_PANDARXT32: { - auto hesai_lidarstatus = CheckSizeAndParse(response); + auto hesai_lidarstatus = check_size_and_parse(response); return std::make_shared(hesai_lidarstatus); } case SensorModel::HESAI_PANDAR128_E4X: { - auto hesai_lidarstatus = CheckSizeAndParse(response); + auto hesai_lidarstatus = check_size_and_parse(response); return std::make_shared(hesai_lidarstatus); } case SensorModel::HESAI_PANDARAT128: { - auto hesai_lidarstatus = CheckSizeAndParse(response); + auto hesai_lidarstatus = check_size_and_parse(response); return std::make_shared(hesai_lidarstatus); } case SensorModel::HESAI_PANDARQT128: { - auto hesai_lidarstatus = CheckSizeAndParse(response); + auto hesai_lidarstatus = check_size_and_parse(response); return std::make_shared(hesai_lidarstatus); } } } -Status HesaiHwInterface::SetSpinRate(uint16_t rpm) +Status HesaiHwInterface::set_spin_rate(uint16_t rpm) { std::vector request_payload; request_payload.emplace_back((rpm >> 8) & 0xff); request_payload.emplace_back(rpm & 0xff); - auto response_or_err = SendReceive(PTC_COMMAND_SET_SPIN_RATE, request_payload); - response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + auto response_or_err = send_receive(g_ptc_command_set_spin_rate, request_payload); + response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); return Status::OK; } -Status HesaiHwInterface::SetSyncAngle(int sync_angle, int angle) +Status HesaiHwInterface::set_sync_angle(int sync_angle, int angle) { if (sync_angle < 0 || sync_angle > 360) { return Status::SENSOR_CONFIG_ERROR; @@ -442,42 +442,42 @@ Status HesaiHwInterface::SetSyncAngle(int sync_angle, int angle) request_payload.emplace_back((angle >> 8) & 0xff); request_payload.emplace_back(angle & 0xff); - auto response_or_err = SendReceive(PTC_COMMAND_SET_SYNC_ANGLE, request_payload); - response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + auto response_or_err = send_receive(g_ptc_command_set_sync_angle, request_payload); + response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); return Status::OK; } -Status HesaiHwInterface::SetTriggerMethod(int trigger_method) +Status HesaiHwInterface::set_trigger_method(int trigger_method) { std::vector request_payload; request_payload.emplace_back(trigger_method & 0xff); - auto response_or_err = SendReceive(PTC_COMMAND_SET_TRIGGER_METHOD, request_payload); - response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + auto response_or_err = send_receive(g_ptc_command_set_trigger_method, request_payload); + response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); return Status::OK; } -Status HesaiHwInterface::SetStandbyMode(int standby_mode) +Status HesaiHwInterface::set_standby_mode(int standby_mode) { std::vector request_payload; request_payload.emplace_back(standby_mode & 0xff); - auto response_or_err = SendReceive(PTC_COMMAND_SET_STANDBY_MODE, request_payload); - response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + auto response_or_err = send_receive(g_ptc_command_set_standby_mode, request_payload); + response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); return Status::OK; } -Status HesaiHwInterface::SetReturnMode(int return_mode) +Status HesaiHwInterface::set_return_mode(int return_mode) { std::vector request_payload; request_payload.emplace_back(return_mode & 0xff); - auto response_or_err = SendReceive(PTC_COMMAND_SET_RETURN_MODE, request_payload); - response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + auto response_or_err = send_receive(g_ptc_command_set_return_mode, request_payload); + response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); return Status::OK; } -Status HesaiHwInterface::SetDestinationIp( +Status HesaiHwInterface::set_destination_ip( 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; @@ -490,12 +490,12 @@ Status HesaiHwInterface::SetDestinationIp( request_payload.emplace_back((gps_port >> 8) & 0xff); request_payload.emplace_back(gps_port & 0xff); - auto response_or_err = SendReceive(PTC_COMMAND_SET_DESTINATION_IP, request_payload); - response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + auto response_or_err = send_receive(g_ptc_command_set_destination_ip, request_payload); + response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); return Status::OK; } -Status HesaiHwInterface::SetControlPort( +Status HesaiHwInterface::set_control_port( 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) { @@ -516,12 +516,12 @@ Status HesaiHwInterface::SetControlPort( request_payload.emplace_back((vlan_id >> 8) & 0xff); request_payload.emplace_back(vlan_id & 0xff); - auto response_or_err = SendReceive(PTC_COMMAND_SET_CONTROL_PORT, request_payload); - response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + auto response_or_err = send_receive(g_ptc_command_set_control_port, request_payload); + response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); return Status::OK; } -Status HesaiHwInterface::SetLidarRange(int method, std::vector data) +Status HesaiHwInterface::set_lidar_range(int method, std::vector data) { if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128) { return Status::SENSOR_CONFIG_ERROR; @@ -533,12 +533,12 @@ Status HesaiHwInterface::SetLidarRange(int method, std::vector da request_payload.emplace_back(method & 0xff); request_payload.insert(request_payload.end(), data.begin(), data.end()); - auto response_or_err = SendReceive(PTC_COMMAND_SET_LIDAR_RANGE, request_payload); - response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + auto response_or_err = send_receive(g_ptc_command_set_lidar_range, request_payload); + response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); return Status::OK; } -Status HesaiHwInterface::SetLidarRange(int start_ddeg, int end_ddeg) +Status HesaiHwInterface::set_lidar_range(int start_ddeg, int end_ddeg) { if ( sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128 || @@ -557,12 +557,12 @@ Status HesaiHwInterface::SetLidarRange(int start_ddeg, int end_ddeg) request_payload.emplace_back((end_ddeg >> 8) & 0xff); request_payload.emplace_back(end_ddeg & 0xff); - auto response_or_err = SendReceive(PTC_COMMAND_SET_LIDAR_RANGE, request_payload); - response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + auto response_or_err = send_receive(g_ptc_command_set_lidar_range, request_payload); + response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); return Status::OK; } -HesaiLidarRangeAll HesaiHwInterface::GetLidarRange() +HesaiLidarRangeAll HesaiHwInterface::get_lidar_range() { if ( sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128 || @@ -570,8 +570,8 @@ HesaiLidarRangeAll HesaiHwInterface::GetLidarRange() throw std::runtime_error("Not supported on this sensor"); } - auto response_or_err = SendReceive(PTC_COMMAND_GET_LIDAR_RANGE); - auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + auto response_or_err = send_receive(g_ptc_command_get_lidar_range); + auto response = response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); if (response.size() < 1) { throw std::runtime_error("Response payload too short"); @@ -599,7 +599,7 @@ HesaiLidarRangeAll HesaiHwInterface::GetLidarRange() return hesai_range_all; } -Status HesaiHwInterface::checkAndSetLidarRange( +Status HesaiHwInterface::check_and_set_lidar_range( const HesaiCalibrationConfigurationBase & calibration) { if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128) { @@ -622,20 +622,20 @@ Status HesaiHwInterface::checkAndSetLidarRange( return angle_ddeg; }; - return SetLidarRange(clamp(cloud_min_ddeg), clamp(cloud_max_ddeg)); + return set_lidar_range(clamp(cloud_min_ddeg), clamp(cloud_max_ddeg)); } -Status HesaiHwInterface::SetClockSource(int clock_source) +Status HesaiHwInterface::set_clock_source(int clock_source) { std::vector request_payload; request_payload.emplace_back(clock_source & 0xff); - auto response_or_err = SendReceive(PTC_COMMAND_SET_CLOCK_SOURCE, request_payload); - response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + auto response_or_err = send_receive(g_ptc_command_set_clock_source, request_payload); + response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); return Status::OK; } -Status HesaiHwInterface::SetPtpConfig( +Status HesaiHwInterface::set_ptp_config( int profile, int domain, int network, int switch_type, int logAnnounceInterval, int logSyncInterval, int logMinDelayReqInterval) { @@ -664,15 +664,15 @@ Status HesaiHwInterface::SetPtpConfig( request_payload.emplace_back(switch_type & 0xff); } - auto response_or_err = SendReceive(PTC_COMMAND_SET_PTP_CONFIG, request_payload); - response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + auto response_or_err = send_receive(g_ptc_command_set_ptp_config, request_payload); + response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); return Status::OK; } -HesaiPtpConfig HesaiHwInterface::GetPtpConfig() +HesaiPtpConfig HesaiHwInterface::get_ptp_config() { - auto response_or_err = SendReceive(PTC_COMMAND_GET_PTP_CONFIG); - auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + auto response_or_err = send_receive(g_ptc_command_get_ptp_config); + auto response = response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); if (response.size() < sizeof(HesaiPtpConfig)) { throw std::runtime_error("HesaiPtpConfig has unexpected payload size"); @@ -689,45 +689,45 @@ HesaiPtpConfig HesaiHwInterface::GetPtpConfig() return hesai_ptp_config; } -Status HesaiHwInterface::SendReset() +Status HesaiHwInterface::send_reset() { - auto response_or_err = SendReceive(PTC_COMMAND_RESET); - response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + auto response_or_err = send_receive(g_ptc_command_reset); + response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); return Status::OK; } -Status HesaiHwInterface::SetRotDir(int mode) +Status HesaiHwInterface::set_rot_dir(int mode) { std::vector request_payload; request_payload.emplace_back(mode & 0xff); - auto response_or_err = SendReceive(PTC_COMMAND_SET_ROTATE_DIRECTION, request_payload); - response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + auto response_or_err = send_receive(g_ptc_command_set_rotate_direction, request_payload); + response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); return Status::OK; } -HesaiLidarMonitor HesaiHwInterface::GetLidarMonitor() +HesaiLidarMonitor HesaiHwInterface::get_lidar_monitor() { if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128) { throw std::runtime_error("Not supported on this sensor"); } - auto response_or_err = SendReceive(PTC_COMMAND_LIDAR_MONITOR); - auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - return CheckSizeAndParse(response); + auto response_or_err = send_receive(g_ptc_command_lidar_monitor); + auto response = response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); + return check_size_and_parse(response); } -void HesaiHwInterface::IOContextRun() +void HesaiHwInterface::io_context_run() { - m_owned_ctx->run(); + m_owned_ctx_->run(); } -std::shared_ptr HesaiHwInterface::GetIOContext() +std::shared_ptr HesaiHwInterface::get_io_context() { - return m_owned_ctx; + return m_owned_ctx_; } -HesaiStatus HesaiHwInterface::GetHttpClientDriverOnce( +HesaiStatus HesaiHwInterface::get_http_client_driver_once( std::shared_ptr ctx, std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> & hcd) { @@ -746,11 +746,11 @@ HesaiStatus HesaiHwInterface::GetHttpClientDriverOnce( return Status::OK; } -HesaiStatus HesaiHwInterface::GetHttpClientDriverOnce( +HesaiStatus HesaiHwInterface::get_http_client_driver_once( std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> & hcd) { std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd_tmp; - auto st = GetHttpClientDriverOnce(std::make_shared(), hcd_tmp); + auto st = get_http_client_driver_once(std::make_shared(), hcd_tmp); hcd = std::move(hcd_tmp); return st; } @@ -783,11 +783,11 @@ std::pair HesaiHwInterface::unwrap_http_response( return {Status::ERROR_1, message}; } -HesaiStatus HesaiHwInterface::SetSpinSpeedAsyncHttp( +HesaiStatus HesaiHwInterface::set_spin_speed_async_http( std::shared_ptr ctx, uint16_t rpm) { std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; - auto st = GetHttpClientDriverOnce(ctx, hcd); + auto st = get_http_client_driver_once(ctx, hcd); if (st != Status::OK) { return st; } @@ -814,17 +814,17 @@ HesaiStatus HesaiHwInterface::SetSpinSpeedAsyncHttp( return Status::WAITING_FOR_SENSOR_RESPONSE; } -HesaiStatus HesaiHwInterface::SetSpinSpeedAsyncHttp(uint16_t rpm) +HesaiStatus HesaiHwInterface::set_spin_speed_async_http(uint16_t rpm) { - return SetSpinSpeedAsyncHttp(std::make_shared(), rpm); + return set_spin_speed_async_http(std::make_shared(), rpm); } -HesaiStatus HesaiHwInterface::SetPtpConfigSyncHttp( +HesaiStatus HesaiHwInterface::set_ptp_config_sync_http( 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); + auto st = get_http_client_driver_once(ctx, hcd); if (st != Status::OK) { return st; } @@ -846,20 +846,20 @@ HesaiStatus HesaiHwInterface::SetPtpConfigSyncHttp( return unwrap_http_response(response).first; } -HesaiStatus HesaiHwInterface::SetPtpConfigSyncHttp( +HesaiStatus HesaiHwInterface::set_ptp_config_sync_http( int profile, int domain, int network, int logAnnounceInterval, int logSyncInterval, int logMinDelayReqInterval) { - return SetPtpConfigSyncHttp( + return set_ptp_config_sync_http( std::make_shared(), profile, domain, network, logAnnounceInterval, logSyncInterval, logMinDelayReqInterval); } -HesaiStatus HesaiHwInterface::SetSyncAngleSyncHttp( +HesaiStatus HesaiHwInterface::set_sync_angle_sync_http( std::shared_ptr ctx, int enable, int angle) { std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; - auto st = GetHttpClientDriverOnce(ctx, hcd); + auto st = get_http_client_driver_once(ctx, hcd); if (st != Status::OK) { return st; } @@ -874,17 +874,17 @@ HesaiStatus HesaiHwInterface::SetSyncAngleSyncHttp( return unwrap_http_response(response).first; } -HesaiStatus HesaiHwInterface::SetSyncAngleSyncHttp(int enable, int angle) +HesaiStatus HesaiHwInterface::set_sync_angle_sync_http(int enable, int angle) { - return SetSyncAngleSyncHttp(std::make_shared(), enable, angle); + return set_sync_angle_sync_http(std::make_shared(), enable, angle); } -HesaiStatus HesaiHwInterface::GetLidarMonitorAsyncHttp( +HesaiStatus HesaiHwInterface::get_lidar_monitor_async_http( std::shared_ptr ctx, std::function str_callback) { std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; - auto st = GetHttpClientDriverOnce(ctx, hcd); + auto st = get_http_client_driver_once(ctx, hcd); if (st != Status::OK) { logger_->error("HesaiHwInterface::GetLidarMonitorAsyncHttp: cannot GetHttpClientDriverOnce"); return st; @@ -901,13 +901,13 @@ HesaiStatus HesaiHwInterface::GetLidarMonitorAsyncHttp( return Status::WAITING_FOR_SENSOR_RESPONSE; } -HesaiStatus HesaiHwInterface::GetLidarMonitorAsyncHttp( +HesaiStatus HesaiHwInterface::get_lidar_monitor_async_http( std::function str_callback) { - return GetLidarMonitorAsyncHttp(std::make_shared(), str_callback); + return get_lidar_monitor_async_http(std::make_shared(), str_callback); } -HesaiStatus HesaiHwInterface::CheckAndSetConfig( +HesaiStatus HesaiHwInterface::check_and_set_config( std::shared_ptr sensor_configuration, std::shared_ptr hesai_config_ptr) { @@ -936,7 +936,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( "mode."); return_mode_int = 2; } - SetReturnMode(return_mode_int); + set_return_mode(return_mode_int); }); t.join(); std::this_thread::sleep_for(wait_time); @@ -950,13 +950,13 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( logger_->info( "current configuration rotation_speed: " + std::to_string(sensor_configuration->rotation_speed)); - if (UseHttpSetSpinRate()) { - SetSpinSpeedAsyncHttp(sensor_configuration->rotation_speed); + if (use_http_set_spin_rate()) { + set_spin_speed_async_http(sensor_configuration->rotation_speed); } else { logger_->info( "Setting up spin rate via TCP." + std::to_string(sensor_configuration->rotation_speed)); std::thread t( - [this, sensor_configuration] { SetSpinRate(sensor_configuration->rotation_speed); }); + [this, sensor_configuration] { set_spin_rate(sensor_configuration->rotation_speed); }); t.join(); } std::this_thread::sleep_for(wait_time); @@ -1002,7 +1002,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( std::vector list_string; boost::split(list_string, desired_host_addr, boost::is_any_of(".")); std::thread t([this, sensor_configuration, list_string] { - SetDestinationIp( + set_destination_ip( std::stoi(list_string[0]), std::stoi(list_string[1]), std::stoi(list_string[2]), std::stoi(list_string[3]), sensor_configuration->data_port, sensor_configuration->gnss_port); @@ -1024,7 +1024,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( logger_->info("current lidar sync_angle: " + std::to_string(sensor_sync_angle)); logger_->info("current configuration sync_angle: " + std::to_string(config_sync_angle)); std::thread t( - [this, sync_flg, config_sync_angle] { SetSyncAngle(sync_flg, config_sync_angle); }); + [this, sync_flg, config_sync_angle] { set_sync_angle(sync_flg, config_sync_angle); }); t.join(); std::this_thread::sleep_for(wait_time); } @@ -1037,7 +1037,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( sensor_configuration->sensor_model == SensorModel::HESAI_PANDARXT32 || sensor_configuration->sensor_model == SensorModel::HESAI_PANDARXT32M) { logger_->info("Trying to set Clock source to PTP"); - SetClockSource(HESAI_LIDAR_PTP_CLOCK_SOURCE); + set_clock_source(g_hesai_lidar_ptp_clock_source); } std::ostringstream tmp_ostringstream; tmp_ostringstream << "Trying to set PTP Config: " << sensor_configuration->ptp_profile @@ -1045,11 +1045,11 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( << ", Transport: " << sensor_configuration->ptp_transport_type << ", Switch Type: " << sensor_configuration->ptp_switch_type << " via TCP"; logger_->info(tmp_ostringstream.str()); - SetPtpConfig( + set_ptp_config( 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); + static_cast(sensor_configuration->ptp_switch_type), g_ptp_log_announce_interval, + g_ptp_sync_interval, g_ptp_log_min_delay_interval); logger_->debug("Setting properties done"); }); logger_->debug("Waiting for thread to finish"); @@ -1060,16 +1060,16 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( std::this_thread::sleep_for(wait_time); } else { // AT128 only supports PTP setup via HTTP logger_->info("Trying to set SyncAngle via HTTP"); - SetSyncAngleSyncHttp(1, sensor_configuration->sync_angle); + set_sync_angle_sync_http(1, sensor_configuration->sync_angle); 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"; logger_->info(tmp_ostringstream.str()); - SetPtpConfigSyncHttp( + set_ptp_config_sync_http( 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); + static_cast(sensor_configuration->ptp_transport_type), g_ptp_log_announce_interval, + g_ptp_sync_interval, g_ptp_log_min_delay_interval); } #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE @@ -1080,7 +1080,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( return Status::OK; } -HesaiStatus HesaiHwInterface::CheckAndSetConfig( +HesaiStatus HesaiHwInterface::check_and_set_config( std::shared_ptr sensor_configuration, HesaiLidarRangeAll hesai_lidar_range_all) { @@ -1088,7 +1088,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( std::cout << "Start CheckAndSetConfig(HesaiLidarRangeAll)!!" << std::endl; #endif //* - // PTC_COMMAND_SET_LIDAR_RANGE + // g_ptc_command_set_lidar_range bool set_flg = false; if (hesai_lidar_range_all.method != 0) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE @@ -1126,7 +1126,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( if (set_flg) { std::thread t([this, sensor_configuration] { - SetLidarRange( + set_lidar_range( static_cast(sensor_configuration->cloud_min_angle * 10), static_cast(sensor_configuration->cloud_max_angle * 10)); }); @@ -1139,14 +1139,14 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( return Status::WAITING_FOR_SENSOR_RESPONSE; } -HesaiStatus HesaiHwInterface::CheckAndSetConfig() +HesaiStatus HesaiHwInterface::check_and_set_config() { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << "Start CheckAndSetConfig!!" << std::endl; #endif std::thread t([this] { - auto result = GetConfig(); - CheckAndSetConfig( + auto result = get_config(); + check_and_set_config( std::static_pointer_cast(sensor_configuration_), result); }); t.join(); @@ -1158,8 +1158,8 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig() } std::thread t2([this] { - auto result = GetLidarRange(); - CheckAndSetConfig( + auto result = get_lidar_range(); + check_and_set_config( std::static_pointer_cast(sensor_configuration_), result); }); t2.join(); @@ -1184,7 +1184,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig() 42: OT128 48: ? */ -int HesaiHwInterface::NebulaModelToHesaiModelNo(nebula::drivers::SensorModel model) +int HesaiHwInterface::nebula_model_to_hesai_model_no(nebula::drivers::SensorModel model) { switch (model) { case SensorModel::HESAI_PANDAR40P: @@ -1212,16 +1212,16 @@ int HesaiHwInterface::NebulaModelToHesaiModelNo(nebula::drivers::SensorModel mod return -1; } } -void HesaiHwInterface::SetTargetModel(int model) +void HesaiHwInterface::set_target_model(int model) { - target_model_no = model; + target_model_no_ = model; } -void HesaiHwInterface::SetTargetModel(nebula::drivers::SensorModel model) +void HesaiHwInterface::set_target_model(nebula::drivers::SensorModel model) { - target_model_no = NebulaModelToHesaiModelNo(model); + target_model_no_ = nebula_model_to_hesai_model_no(model); } -bool HesaiHwInterface::UseHttpSetSpinRate(int model) +bool HesaiHwInterface::use_http_set_spin_rate(int model) { switch (model) { case 0: @@ -1262,11 +1262,11 @@ bool HesaiHwInterface::UseHttpSetSpinRate(int model) break; } } -bool HesaiHwInterface::UseHttpSetSpinRate() +bool HesaiHwInterface::use_http_set_spin_rate() { - return UseHttpSetSpinRate(target_model_no); + return use_http_set_spin_rate(target_model_no_); } -bool HesaiHwInterface::UseHttpGetLidarMonitor(int model) +bool HesaiHwInterface::use_http_get_lidar_monitor(int model) { switch (model) { case 0: @@ -1307,12 +1307,12 @@ bool HesaiHwInterface::UseHttpGetLidarMonitor(int model) break; } } -bool HesaiHwInterface::UseHttpGetLidarMonitor() +bool HesaiHwInterface::use_http_get_lidar_monitor() { - return UseHttpGetLidarMonitor(target_model_no); + return use_http_get_lidar_monitor(target_model_no_); } -std::string HesaiHwInterface::PrettyPrintPTCError(ptc_error_t error_code) +std::string HesaiHwInterface::pretty_print_ptc_error(ptc_error_t error_code) { if (error_code.ok()) { return "No error"; @@ -1328,27 +1328,27 @@ std::string HesaiHwInterface::PrettyPrintPTCError(ptc_error_t error_code) } switch (ptc_error) { - case PTC_ERROR_CODE_NO_ERROR: + case g_ptc_error_code_no_error: break; - case PTC_ERROR_CODE_INVALID_INPUT_PARAM: + case g_ptc_error_code_invalid_input_param: ss << "Invalid input parameter"; break; - case PTC_ERROR_CODE_SERVER_CONN_FAILED: + case g_ptc_error_code_server_conn_failed: ss << "Failure to connect to server"; break; - case PTC_ERROR_CODE_INVALID_DATA: + case g_ptc_error_code_invalid_data: ss << "No valid data returned"; break; - case PTC_ERROR_CODE_OUT_OF_MEMORY: + case g_ptc_error_code_out_of_memory: ss << "Server does not have enough memory"; break; - case PTC_ERROR_CODE_UNSUPPORTED_CMD: + case g_ptc_error_code_unsupported_cmd: ss << "Server does not support this command yet"; break; - case PTC_ERROR_CODE_FPGA_COMM_FAILED: + case g_ptc_error_code_fpga_comm_failed: ss << "Server failed to communicate with FPGA"; break; - case PTC_ERROR_CODE_OTHER: + case g_ptc_error_code_other: ss << "Unspecified internal error"; break; default: @@ -1367,16 +1367,16 @@ std::string HesaiHwInterface::PrettyPrintPTCError(ptc_error_t error_code) ss << "Communication error: "; std::vector nebula_errors; - if (error_flags & TCP_ERROR_INCOMPLETE_RESPONSE) { + if (error_flags & g_tcp_error_incomplete_response) { nebula_errors.emplace_back("Incomplete response payload"); } - if (error_flags & TCP_ERROR_TIMEOUT) { + if (error_flags & g_tcp_error_timeout) { nebula_errors.emplace_back("Request timeout"); } - if (error_flags & TCP_ERROR_UNEXPECTED_PAYLOAD) { + if (error_flags & g_tcp_error_unexpected_payload) { nebula_errors.emplace_back("Received payload but expected payload length 0"); } - if (error_flags & TCP_ERROR_UNRELATED_RESPONSE) { + if (error_flags & g_tcp_error_unrelated_response) { nebula_errors.emplace_back("Received unrelated response"); } @@ -1386,7 +1386,7 @@ std::string HesaiHwInterface::PrettyPrintPTCError(ptc_error_t error_code) } template -T HesaiHwInterface::CheckSizeAndParse(const std::vector & data) +T HesaiHwInterface::check_size_and_parse(const std::vector & data) { if (data.size() < sizeof(T)) { throw std::runtime_error("Attempted to parse too-small payload"); diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index dbf5fe09..136e5a08 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -70,7 +70,7 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) if (hw_interface_wrapper_ && !use_udp_only && lidar_range_supported) { auto status = - hw_interface_wrapper_->hw_interface()->checkAndSetLidarRange(*calibration_result.value()); + hw_interface_wrapper_->hw_interface()->check_and_set_lidar_range(*calibration_result.value()); if (status != Status::OK) { throw std::runtime_error( (std::stringstream{} << "Could not set sensor FoV: " << status).str()); @@ -82,7 +82,7 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) RCLCPP_DEBUG(get_logger(), "Starting stream"); if (launch_hw_) { - hw_interface_wrapper_->hw_interface()->RegisterScanCallback( + hw_interface_wrapper_->hw_interface()->register_scan_callback( std::bind(&HesaiRosWrapper::receive_cloud_packet_callback, this, std::placeholders::_1)); stream_start(); } else { @@ -301,7 +301,7 @@ Status HesaiRosWrapper::stream_start() return hw_interface_wrapper_->status(); } - return hw_interface_wrapper_->hw_interface()->SensorInterfaceStart(); + return hw_interface_wrapper_->hw_interface()->sensor_interface_start(); } rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::on_parameter_change( @@ -396,7 +396,7 @@ rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::on_parameter_change( new_calibration_ptr && hw_interface_wrapper_ && sensor_cfg_ptr_->sensor_model != drivers::SensorModel::HESAI_PANDARAT128) { auto status = - hw_interface_wrapper_->hw_interface()->checkAndSetLidarRange(*new_calibration_ptr); + hw_interface_wrapper_->hw_interface()->check_and_set_lidar_range(*new_calibration_ptr); if (status != Status::OK) { RCLCPP_ERROR_STREAM( get_logger(), "Sensor configuration updated, but setting hardware FoV failed: " << status); @@ -458,7 +458,7 @@ HesaiRosWrapper::get_calibration_result_t HesaiRosWrapper::get_calibration_data( // If a sensor is connected, try to download and save its calibration data if (!ignore_others && launch_hw_) { try { - auto raw_data = hw_interface_wrapper_->hw_interface()->GetLidarCalibrationBytes(); + auto raw_data = hw_interface_wrapper_->hw_interface()->get_lidar_calibration_bytes(); RCLCPP_INFO(logger, "Downloaded calibration data from sensor."); auto status = calib->save_to_file_from_bytes(calibration_file_path_from_sensor, raw_data); if (status != Status::OK) { diff --git a/nebula_ros/src/hesai/hw_interface_wrapper.cpp b/nebula_ros/src/hesai/hw_interface_wrapper.cpp index 22765bf7..d66ad0ef 100644 --- a/nebula_ros/src/hesai/hw_interface_wrapper.cpp +++ b/nebula_ros/src/hesai/hw_interface_wrapper.cpp @@ -24,7 +24,7 @@ HesaiHwInterfaceWrapper::HesaiHwInterfaceWrapper( setup_sensor_ = parent_node->declare_parameter("setup_sensor", param_read_only()); bool retry_connect = parent_node->declare_parameter("retry_hw", param_read_only()); - status_ = hw_interface_->SetSensorConfiguration( + status_ = hw_interface_->set_sensor_configuration( std::static_pointer_cast(config)); if (status_ != Status::OK) { @@ -32,7 +32,7 @@ HesaiHwInterfaceWrapper::HesaiHwInterfaceWrapper( (std::stringstream{} << "Could not initialize HW interface: " << status_).str()); } - hw_interface_->SetTargetModel(config->sensor_model); + hw_interface_->set_target_model(config->sensor_model); if (use_udp_only) { // Do not initialize TCP @@ -42,7 +42,7 @@ HesaiHwInterfaceWrapper::HesaiHwInterfaceWrapper( int retry_count = 0; while (true) { - status_ = hw_interface_->InitializeTcpDriver(); + status_ = hw_interface_->initialize_tcp_driver(); if (status_ == Status::OK || !retry_connect) { break; } @@ -54,13 +54,13 @@ HesaiHwInterfaceWrapper::HesaiHwInterfaceWrapper( if (status_ == Status::OK) { try { - auto inventory = hw_interface_->GetInventory(); - hw_interface_->SetTargetModel(inventory->model_number()); + auto inventory = hw_interface_->get_inventory(); + hw_interface_->set_target_model(inventory->model_number()); } catch (...) { RCLCPP_ERROR_STREAM(logger_, "Failed to get model from sensor..."); } if (setup_sensor_) { - hw_interface_->CheckAndSetConfig(); + hw_interface_->check_and_set_config(); } } else { RCLCPP_ERROR_STREAM( @@ -73,10 +73,10 @@ HesaiHwInterfaceWrapper::HesaiHwInterfaceWrapper( void HesaiHwInterfaceWrapper::on_config_change( const std::shared_ptr & new_config) { - hw_interface_->SetSensorConfiguration( + hw_interface_->set_sensor_configuration( std::static_pointer_cast(new_config)); if (!use_udp_only_ && setup_sensor_) { - hw_interface_->CheckAndSetConfig(); + hw_interface_->check_and_set_config(); } } diff --git a/nebula_ros/src/hesai/hw_monitor_wrapper.cpp b/nebula_ros/src/hesai/hw_monitor_wrapper.cpp index 83084a3c..5a01a715 100644 --- a/nebula_ros/src/hesai/hw_monitor_wrapper.cpp +++ b/nebula_ros/src/hesai/hw_monitor_wrapper.cpp @@ -51,7 +51,7 @@ HesaiHwMonitorWrapper::HesaiHwMonitorWrapper( config->sensor_model != drivers::SensorModel::HESAI_PANDAR40P && config->sensor_model != drivers::SensorModel::HESAI_PANDAR64; - std::shared_ptr inventory = hw_interface->GetInventory(); + std::shared_ptr inventory = hw_interface->get_inventory(); RCLCPP_INFO_STREAM(logger_, "Inventory info: " << *inventory); json inventory_json = inventory->to_json(); @@ -87,7 +87,7 @@ void HesaiHwMonitorWrapper::initialize_hesai_diagnostics(bool monitor_enabled) if (!monitor_enabled) return; - if (hw_interface_->UseHttpGetLidarMonitor()) { + if (hw_interface_->use_http_get_lidar_monitor()) { on_hesai_lidar_monitor_timer_http(); } else { on_hesai_lidar_monitor_timer(); @@ -98,7 +98,7 @@ void HesaiHwMonitorWrapper::initialize_hesai_diagnostics(bool monitor_enabled) std::chrono::milliseconds(diag_span_), std::move(fetch_diag_from_sensor)); if (monitor_enabled) { - if (hw_interface_->UseHttpGetLidarMonitor()) { + if (hw_interface_->use_http_get_lidar_monitor()) { diagnostics_updater_.add( "hesai_voltage", this, &HesaiHwMonitorWrapper::hesai_check_voltage_http); } else { @@ -161,7 +161,7 @@ void HesaiHwMonitorWrapper::on_hesai_status_timer() { RCLCPP_DEBUG_STREAM(logger_, "on_hesai_status_timer" << std::endl); try { - auto result = hw_interface_->GetLidarStatus(); + auto result = hw_interface_->get_lidar_status(); std::scoped_lock lock(mtx_lidar_status_); current_status_time_ = std::make_unique(parent_node_->get_clock()->now()); current_status_ = result; @@ -182,12 +182,12 @@ void HesaiHwMonitorWrapper::on_hesai_lidar_monitor_timer_http() { RCLCPP_DEBUG_STREAM(logger_, "on_hesai_lidar_monitor_timer_http"); try { - hw_interface_->GetLidarMonitorAsyncHttp([this](const std::string & str) { + hw_interface_->get_lidar_monitor_async_http([this](const std::string & str) { std::scoped_lock lock(mtx_lidar_monitor_); current_lidar_monitor_time_ = std::make_unique(parent_node_->get_clock()->now()); current_lidar_monitor_tree_ = - std::make_unique(hw_interface_->ParseJson(str)); + std::make_unique(hw_interface_->parse_json(str)); }); } catch (const std::system_error & error) { RCLCPP_ERROR_STREAM( @@ -208,7 +208,7 @@ void HesaiHwMonitorWrapper::on_hesai_lidar_monitor_timer() { RCLCPP_DEBUG_STREAM(logger_, "on_hesai_lidar_monitor_timer"); try { - auto result = hw_interface_->GetLidarMonitor(); + auto result = hw_interface_->get_lidar_monitor(); std::scoped_lock lock(mtx_lidar_monitor_); current_lidar_monitor_time_ = std::make_unique(parent_node_->get_clock()->now()); current_monitor_ = std::make_shared(result); From cc8d56351431ed856dd9b730031e6ad6a98f6fe2 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 26 Nov 2024 11:33:39 +0000 Subject: [PATCH 2/2] ci(pre-commit): autofix --- .../hesai_hw_interface.hpp | 9 +++-- .../hesai_hw_interface.cpp | 36 ++++++++++++------- 2 files changed, 30 insertions(+), 15 deletions(-) diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp index 9d25e2d7..b48fb636 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 @@ -172,7 +172,8 @@ class HesaiHwInterface /// @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. - ptc_cmd_result_t send_receive(const uint8_t command_id, const std::vector & payload = {}); + ptc_cmd_result_t send_receive( + const uint8_t command_id, const std::vector & payload = {}); static std::pair unwrap_http_response(const std::string & response); @@ -318,7 +319,8 @@ class HesaiHwInterface * @param calibration The calibration file of the sensor * @return Status Resulting status of setting the FoV */ - [[nodiscard]] Status check_and_set_lidar_range(const HesaiCalibrationConfigurationBase & calibration); + [[nodiscard]] Status check_and_set_lidar_range( + const HesaiCalibrationConfigurationBase & calibration); Status set_clock_source(int clock_source); @@ -387,7 +389,8 @@ class HesaiHwInterface /// @brief Getting lidar_monitor via HTTP API /// @param str_callback Callback function for received string /// @return Resulting status - HesaiStatus get_lidar_monitor_async_http(std::function str_callback); + HesaiStatus get_lidar_monitor_async_http( + std::function str_callback); /// @brief Checking the current settings and changing the difference point /// @param sensor_configuration Current SensorConfiguration 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 87815fc5..0efa6fb5 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 @@ -312,23 +312,28 @@ std::string HesaiHwInterface::get_lidar_calibration_string() HesaiPtpDiagStatus HesaiHwInterface::get_ptp_diag_status() { auto response_or_err = send_receive(g_ptc_command_ptp_diagnostics, {g_ptc_command_ptp_status}); - auto response = response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); + auto response = + response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); auto diag_status = check_size_and_parse(response); return diag_status; } HesaiPtpDiagPort HesaiHwInterface::get_ptp_diag_port() { - auto response_or_err = send_receive(g_ptc_command_ptp_diagnostics, {g_ptc_command_ptp_port_data_set}); - auto response = response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); + auto response_or_err = + send_receive(g_ptc_command_ptp_diagnostics, {g_ptc_command_ptp_port_data_set}); + auto response = + response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); auto diag_port = check_size_and_parse(response); return diag_port; } HesaiPtpDiagTime HesaiHwInterface::get_ptp_diag_time() { - auto response_or_err = send_receive(g_ptc_command_ptp_diagnostics, {g_ptc_command_ptp_time_status_np}); - auto response = response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); + auto response_or_err = + send_receive(g_ptc_command_ptp_diagnostics, {g_ptc_command_ptp_time_status_np}); + auto response = + response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); auto diag_time = check_size_and_parse(response); return diag_time; } @@ -337,7 +342,8 @@ HesaiPtpDiagGrandmaster HesaiHwInterface::get_ptp_diag_grandmaster() { auto response_or_err = send_receive(g_ptc_command_ptp_diagnostics, {g_ptc_command_ptp_grandmaster_settings_np}); - auto response = response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); + auto response = + response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); auto diag_grandmaster = check_size_and_parse(response); return diag_grandmaster; } @@ -345,7 +351,8 @@ HesaiPtpDiagGrandmaster HesaiHwInterface::get_ptp_diag_grandmaster() std::shared_ptr HesaiHwInterface::get_inventory() { auto response_or_err = send_receive(g_ptc_command_get_inventory_info); - auto response = response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); + auto response = + response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); switch (sensor_configuration_->sensor_model) { default: @@ -372,7 +379,8 @@ std::shared_ptr HesaiHwInterface::get_inventory() std::shared_ptr HesaiHwInterface::get_config() { auto response_or_err = send_receive(g_ptc_command_get_config_info); - auto response = response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); + auto response = + response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); switch (sensor_configuration_->sensor_model) { default: @@ -394,7 +402,8 @@ std::shared_ptr HesaiHwInterface::get_config() std::shared_ptr HesaiHwInterface::get_lidar_status() { auto response_or_err = send_receive(g_ptc_command_get_lidar_status); - auto response = response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); + auto response = + response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); switch (sensor_configuration_->sensor_model) { default: @@ -571,7 +580,8 @@ HesaiLidarRangeAll HesaiHwInterface::get_lidar_range() } auto response_or_err = send_receive(g_ptc_command_get_lidar_range); - auto response = response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); + auto response = + response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); if (response.size() < 1) { throw std::runtime_error("Response payload too short"); @@ -672,7 +682,8 @@ Status HesaiHwInterface::set_ptp_config( HesaiPtpConfig HesaiHwInterface::get_ptp_config() { auto response_or_err = send_receive(g_ptc_command_get_ptp_config); - auto response = response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); + auto response = + response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); if (response.size() < sizeof(HesaiPtpConfig)) { throw std::runtime_error("HesaiPtpConfig has unexpected payload size"); @@ -713,7 +724,8 @@ HesaiLidarMonitor HesaiHwInterface::get_lidar_monitor() } auto response_or_err = send_receive(g_ptc_command_lidar_monitor); - auto response = response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); + auto response = + response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); return check_size_and_parse(response); }