diff --git a/docs/parameters/vendors/hesai/common.md b/docs/parameters/vendors/hesai/common.md index 359bba31c..a804ccbb9 100644 --- a/docs/parameters/vendors/hesai/common.md +++ b/docs/parameters/vendors/hesai/common.md @@ -24,6 +24,14 @@ While this is not found in the PTP standards, this influences how often the sens Set this to `TSN` if your switch supports gPTP or the AutoSAR protocol, and `NON_TSN` if the switch does not. In the latter case, the sensor will measure more often. +### `ptp_lock_threshold` + +_Only applies to `OT128` and `QT128`_ + +The maximum difference between the sensor and PTP master that will still be considered `locked` by the sensor, in microseconds. +When this threshold is crossed, the sensor will report its synchronization state to be `tracking`. +Nebula's hardware monitor treats only the `locked` state as `OK`, `tracking` as `WARNING` and `frozen` and `free run` as `ERROR`. + ## Scan Cutting and Field of View Scan cutting influences the time stamps of points and the point cloud headers. diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index 1487047fb..21faa7efe 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_common.hpp +++ b/nebula_common/include/nebula_common/hesai/hesai_common.hpp @@ -51,6 +51,7 @@ struct HesaiSensorConfiguration : public LidarConfigurationBase uint8_t ptp_domain; PtpTransportType ptp_transport_type; PtpSwitchType ptp_switch_type; + uint8_t ptp_lock_threshold; std::vector> point_filters; }; /// @brief Convert HesaiSensorConfiguration to string (Overloading the << operator) @@ -74,7 +75,8 @@ inline std::ostream & operator<<(std::ostream & os, HesaiSensorConfiguration con os << "PTP Profile: " << arg.ptp_profile << '\n'; os << "PTP Domain: " << std::to_string(arg.ptp_domain) << '\n'; os << "PTP Transport Type: " << arg.ptp_transport_type << '\n'; - os << "PTP Switch Type: " << arg.ptp_switch_type; + os << "PTP Switch Type: " << arg.ptp_switch_type << '\n'; + os << "PTP Lock Threshold: " << std::to_string(arg.ptp_lock_threshold); return os; } diff --git a/nebula_examples/CMakeLists.txt b/nebula_examples/CMakeLists.txt index 311105e30..779987293 100644 --- a/nebula_examples/CMakeLists.txt +++ b/nebula_examples/CMakeLists.txt @@ -92,6 +92,12 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() +install(TARGETS hesai_ros_offline_extract_pcd_node + hesai_ros_offline_extract_bag_pcd_node + velodyne_ros_offline_extract_bag_pcd_node + DESTINATION lib/${PROJECT_NAME} +) + ament_auto_package( INSTALL_TO_SHARE launch diff --git a/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp b/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp index 8b3e0520c..9427ef7fd 100644 --- a/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp +++ b/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp @@ -69,11 +69,17 @@ class HesaiRosOfflineExtractBag final : public rclcpp::Node std::string storage_id_; std::string out_path_; std::string format_; - std::string target_topic_; + std::string input_topic_; std::string correction_file_path_; + std::string frame_id_; + std::string output_pointcloud_topic_; int out_num_; int skip_num_; bool only_xyz_; + + bool output_pcd_; + bool output_rosbag_; + bool forward_packets_to_rosbag_; }; } // namespace nebula::ros diff --git a/nebula_examples/launch/hesai_offline_bag_pcd.xml b/nebula_examples/launch/hesai_offline_bag_pcd.xml index acb604321..b9e308e89 100644 --- a/nebula_examples/launch/hesai_offline_bag_pcd.xml +++ b/nebula_examples/launch/hesai_offline_bag_pcd.xml @@ -1,38 +1,41 @@ - - - - + + + + + + + + + + + + + + + + - - - - - - - - - - - - - + + + + - + + + - - diff --git a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp index 576a7f8bb..06e586aaa 100644 --- a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp +++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp @@ -24,6 +24,8 @@ #include +#include + #include namespace nebula::ros @@ -91,6 +93,7 @@ Status HesaiRosOfflineExtractBag::get_parameters( nebula::drivers::return_mode_from_string_hesai(return_mode_, sensor_configuration.sensor_model); this->declare_parameter("frame_id", "pandar", param_read_only()); sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); + frame_id_ = sensor_configuration.frame_id; { rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); @@ -105,6 +108,19 @@ Status HesaiRosOfflineExtractBag::get_parameters( descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; descriptor.floating_point_range = float_range(0, 360, 0.01); sensor_configuration.cut_angle = declare_parameter("cut_angle", 0., param_read_only()); + sensor_configuration.cloud_min_angle = + declare_parameter("cloud_min_angle", 0, param_read_only()); + sensor_configuration.cloud_max_angle = + declare_parameter("cloud_max_angle", 360, param_read_only()); + sensor_configuration.rotation_speed = + declare_parameter("rotation_speed", 600, param_read_only()); + sensor_configuration.dual_return_distance_threshold = + declare_parameter("dual_return_distance_threshold", 0.1, param_read_only()); + sensor_configuration.min_range = declare_parameter("min_range", 0.3, param_read_only()); + sensor_configuration.max_range = + declare_parameter("max_range", 300.0, param_read_only()); + sensor_configuration.packet_mtu_size = + declare_parameter("packet_mtu_size", 1500, param_read_only()); } calibration_configuration.calibration_file = @@ -121,7 +137,13 @@ Status HesaiRosOfflineExtractBag::get_parameters( out_num_ = this->declare_parameter("out_num", 3, param_read_only()); skip_num_ = this->declare_parameter("skip_num", 3, param_read_only()); only_xyz_ = this->declare_parameter("only_xyz", false, param_read_only()); - target_topic_ = this->declare_parameter("target_topic", "", param_read_only()); + input_topic_ = this->declare_parameter("input_topic", "", param_read_only()); + output_pointcloud_topic_ = + this->declare_parameter("output_topic", "", param_read_only()); + output_pcd_ = this->declare_parameter("output_pcd", false, param_read_only()); + output_rosbag_ = this->declare_parameter("output_rosbag", true, param_read_only()); + forward_packets_to_rosbag_ = + this->declare_parameter("forward_packets_to_rosbag", false, param_read_only()); if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { return Status::INVALID_SENSOR_MODEL; @@ -170,18 +192,18 @@ Status HesaiRosOfflineExtractBag::read_bag() std::cout << storage_id_ << std::endl; std::cout << out_path_ << std::endl; std::cout << format_ << std::endl; - std::cout << target_topic_ << std::endl; + std::cout << input_topic_ << std::endl; std::cout << out_num_ << std::endl; std::cout << skip_num_ << std::endl; std::cout << only_xyz_ << std::endl; rcpputils::fs::path o_dir(out_path_); - auto target_topic_name = target_topic_; - if (target_topic_name.substr(0, 1) == "/") { - target_topic_name = target_topic_name.substr(1); + auto input_topic_name = input_topic_; + if (input_topic_name.substr(0, 1) == "/") { + input_topic_name = input_topic_name.substr(1); } - target_topic_name = std::regex_replace(target_topic_name, std::regex("/"), "_"); - o_dir = o_dir / rcpputils::fs::path(target_topic_name); + input_topic_name = std::regex_replace(input_topic_name, std::regex("/"), "_"); + o_dir = o_dir / rcpputils::fs::path(input_topic_name); if (rcpputils::fs::create_directories(o_dir)) { std::cout << "created: " << o_dir << std::endl; } @@ -197,12 +219,13 @@ Status HesaiRosOfflineExtractBag::read_bag() reader.open(storage_options, converter_options); int cnt = 0; int out_cnt = 0; + bool output_limit_reached = false; while (reader.has_next()) { auto bag_message = reader.read_next(); std::cout << "Found topic name " << bag_message->topic_name << std::endl; - if (bag_message->topic_name != target_topic_) { + if (bag_message->topic_name != input_topic_) { continue; } @@ -215,6 +238,31 @@ Status HesaiRosOfflineExtractBag::read_bag() std::cout << "Found data in topic " << bag_message->topic_name << ": " << bag_message->time_stamp << std::endl; + // Initialize the bag writer if it is not initialized + if (!bag_writer) { + const rosbag2_storage::StorageOptions storage_options_w( + {(o_dir / std::to_string(bag_message->time_stamp)).string(), "sqlite3"}); + const rosbag2_cpp::ConverterOptions converter_options_w( + {rmw_get_serialization_format(), rmw_get_serialization_format()}); + bag_writer = std::make_unique(); + bag_writer->open(storage_options_w, converter_options_w); + if (forward_packets_to_rosbag_) { + bag_writer->create_topic( + {bag_message->topic_name, "nebula_msgs/msg/NebulaPackets", rmw_get_serialization_format(), + ""}); + } + if (output_rosbag_) { + bag_writer->create_topic( + {output_pointcloud_topic_, "sensor_msgs/msg/PointCloud2", rmw_get_serialization_format(), + ""}); + } + } + + // Forward the bag_message + if (forward_packets_to_rosbag_) { + bag_writer->write(bag_message); + } + nebula_msgs::msg::NebulaPackets nebula_msg; nebula_msg.header = extracted_msg.header; for (auto & pkt : extracted_msg.packets) { @@ -233,35 +281,55 @@ Status HesaiRosOfflineExtractBag::read_bag() auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; - if (!bag_writer) { - const rosbag2_storage::StorageOptions storage_options_w( - {(o_dir / std::to_string(bag_message->time_stamp)).string(), "sqlite3"}); - const rosbag2_cpp::ConverterOptions converter_options_w( - {rmw_get_serialization_format(), rmw_get_serialization_format()}); - bag_writer = std::make_unique(); - bag_writer->open(storage_options_w, converter_options_w); - bag_writer->create_topic( - {bag_message->topic_name, "nebula_msgs/msg/NebulaPackets", rmw_get_serialization_format(), - ""}); - } - - bag_writer->write(bag_message); cnt++; if (skip_num_ < cnt) { out_cnt++; - if (only_xyz_) { - pcl::PointCloud cloud_xyz; - pcl::copyPointCloud(*pointcloud, cloud_xyz); - pcd_writer.writeBinary((o_dir / fn).string(), cloud_xyz); - } else { - pcd_writer.writeBinary((o_dir / fn).string(), *pointcloud); + + if (output_pcd_) { + if (only_xyz_) { + pcl::PointCloud cloud_xyz; + pcl::copyPointCloud(*pointcloud, cloud_xyz); + pcd_writer.writeBinary((o_dir / fn).string(), cloud_xyz); + } else { + pcd_writer.writeBinary((o_dir / fn).string(), *pointcloud); + } + } + + if (output_rosbag_) { + // Create ROS Pointcloud from PCL pointcloud + sensor_msgs::msg::PointCloud2 cloud_msg; + pcl::toROSMsg(*pointcloud, cloud_msg); + cloud_msg.header = extracted_msg.header; + cloud_msg.header.frame_id = frame_id_; + + // Create a serialized message for the pointcloud + rclcpp::SerializedMessage cloud_serialized_msg; + rclcpp::Serialization cloud_serialization; + cloud_serialization.serialize_message(&cloud_msg, &cloud_serialized_msg); + + // Create a bag message for the pointcloud + auto cloud_bag_msg = std::make_shared(); + cloud_bag_msg->topic_name = output_pointcloud_topic_; + cloud_bag_msg->time_stamp = bag_message->time_stamp; + + // Create a new shared_ptr for the serialized data + cloud_bag_msg->serialized_data = std::make_shared( + cloud_serialized_msg.get_rcl_serialized_message()); + + // Write both messages to the bag + bag_writer->write(cloud_bag_msg); } } - if (out_num_ <= out_cnt) { + if (out_num_ != 0 && out_num_ <= out_cnt) { + output_limit_reached = true; break; } } + + if (output_limit_reached) { + break; + } } return Status::OK; } 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 d37a33e02..6f738c0b1 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 @@ -17,7 +17,7 @@ // Have to define macros to silence warnings about deprecated headers being used by // boost/property_tree/ in some versions of boost. // See: https://github.com/boostorg/property_tree/issues/51 -#include "nebula_common/nebula_status.hpp" +#include #include @@ -28,15 +28,15 @@ #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_common/util/expected.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp" -#include +#include +#include +#include +#include +#include +#include +#include #include #include @@ -50,77 +50,68 @@ 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_ptp_command_set_ptp_lock_offset = 0x39; +const uint8_t g_ptp_command_get_ptp_lock_offset = 0x3a; +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 @@ -136,8 +127,9 @@ class HesaiHwInterface using ptc_cmd_result_t = nebula::util::expected, ptc_error_t>; + 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_; @@ -146,151 +138,138 @@ 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 void str_cb(const std::string & str); - std::shared_ptr parent_node_logger; - /// @brief Printing the string to RCLCPP_INFO_STREAM - /// @param info Target string - void PrintInfo(std::string info); - /// @brief Printing the string to RCLCPP_ERROR_STREAM - /// @param error Target string - void PrintError(std::string error); - /// @brief Printing the string to RCLCPP_DEBUG_STREAM - /// @param debug Target string - void PrintDebug(std::string debug); - /// @brief Printing the bytes to RCLCPP_DEBUG_STREAM - /// @param bytes Target byte vector - void PrintDebug(const std::vector & bytes); - /// @brief Convert an error code to a human-readable string /// @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); public: /// @brief Constructor - HesaiHwInterface(); + explicit HesaiHwInterface(const std::shared_ptr & logger); /// @brief Destructor ~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 @@ -299,7 +278,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 @@ -317,22 +296,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 @@ -342,9 +321,10 @@ 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 @@ -358,112 +338,114 @@ 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(); + + Status set_ptp_lock_offset(uint8_t lock_offset); + + uint8_t get_ptp_lock_offset(); + /// @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(); - - /// @brief Setting rclcpp::Logger - /// @param node Logger - void SetLogger(std::shared_ptr node); + 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 0a08dd93e..f25344bf2 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 @@ -4,6 +4,7 @@ #include "nebula_common/hesai/hesai_common.hpp" #include "nebula_common/hesai/hesai_status.hpp" +#include "nebula_common/loggers/logger.hpp" #include "nebula_common/nebula_common.hpp" #include "nebula_common/nebula_status.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp" @@ -17,6 +18,7 @@ #include #include #include +#include // #define WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE @@ -27,26 +29,30 @@ #include +#include + namespace nebula::drivers { using std::string_literals::operator""s; using nlohmann::json; -HesaiHwInterface::HesaiHwInterface() -: cloud_io_context_{new ::drivers::common::IoContext(1)}, - m_owned_ctx{new boost::asio::io_context(1)}, +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)}, cloud_udp_driver_{new ::drivers::udp_driver::UdpDriver(*cloud_io_context_)}, - tcp_driver_{new ::drivers::tcp_driver::TcpDriver(m_owned_ctx)} + 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_); @@ -54,10 +60,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); @@ -76,17 +82,17 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( << " (" << len << ") "; std::string log_tag = ss.str(); - PrintDebug(log_tag + "Entering lock"); + logger_->debug(log_tag + "Entering lock"); std::timed_mutex tm; tm.lock(); if (tcp_driver_->GetIOContext()->stopped()) { - PrintDebug(log_tag + "IOContext was stopped"); + logger_->debug(log_tag + "IOContext was stopped"); tcp_driver_->GetIOContext()->restart(); } - PrintDebug(log_tag + "Sending payload"); + logger_->debug(log_tag + "Sending payload"); tcp_driver_->asyncSendReceiveHeaderPayload( send_buf, [this, log_tag, command_id, response_complete, @@ -95,13 +101,13 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( size_t payload_len = (header_bytes[4] << 24) | (header_bytes[5] << 16) | (header_bytes[6] << 8) | header_bytes[7]; - PrintDebug( + logger_->debug( log_tag + "Received header (expecting " + std::to_string(payload_len) + "B payload)"); // If command_id in the response does not match, we got a response for another command (or // rubbish), probably as a result of too many simultaneous TCP connections to the sensor (e.g. // from GUI, Web UI, another nebula instance, etc.) if (header_bytes[2] != command_id) { - error_code->error_flags |= TCP_ERROR_UNRELATED_RESPONSE; + error_code->error_flags |= g_tcp_error_unrelated_response; } if (payload_len == 0) { *response_complete = true; @@ -109,12 +115,12 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( }, [this, log_tag, recv_buf, response_complete, error_code](const std::vector & payload_bytes) { - PrintDebug(log_tag + "Received payload"); + logger_->debug(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 (*response_complete == true) { - error_code->error_flags |= TCP_ERROR_UNEXPECTED_PAYLOAD; + error_code->error_flags |= g_tcp_error_unexpected_payload; return; } @@ -123,20 +129,20 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( *response_complete = true; }, [this, log_tag, &tm]() { - PrintDebug(log_tag + "Unlocking mutex"); + logger_->debug(log_tag + "Unlocking mutex"); tm.unlock(); - PrintDebug(log_tag + "Unlocked mutex"); + logger_->debug(log_tag + "Unlocked mutex"); }); - this->IOContextRun(); + this->io_context_run(); if (!tm.try_lock_for(std::chrono::seconds(1))) { - PrintError(log_tag + "Request did not finish within 1s"); - error_code->error_flags |= TCP_ERROR_TIMEOUT; + logger_->error(log_tag + "Request did not finish within 1s"); + error_code->error_flags |= g_tcp_error_timeout; return *error_code; } if (!response_complete) { - PrintError(log_tag + "Did not receive response"); - error_code->error_flags |= TCP_ERROR_INCOMPLETE_RESPONSE; + logger_->error(log_tag + "Did not receive response"); + error_code->error_flags |= g_tcp_error_incomplete_response; return *error_code; } @@ -144,12 +150,12 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( return *error_code; } - PrintDebug(log_tag + "Received response"); + logger_->debug(log_tag + "Received response"); return *recv_buf; } -Status HesaiHwInterface::SetSensorConfiguration( +Status HesaiHwInterface::set_sensor_configuration( std::shared_ptr sensor_configuration) { sensor_configuration_ = @@ -157,10 +163,10 @@ Status HesaiHwInterface::SetSensorConfiguration( return Status::OK; } -Status HesaiHwInterface::SensorInterfaceStart() +Status HesaiHwInterface::sensor_interface_start() { try { - PrintInfo("Starting UDP receiver"); + logger_->info("Starting UDP receiver"); if (sensor_configuration_->multicast_ip.empty()) { cloud_udp_driver_->init_receiver( sensor_configuration_->host_ip, sensor_configuration_->data_port); @@ -171,30 +177,30 @@ Status HesaiHwInterface::SensorInterfaceStart() cloud_udp_driver_->receiver()->setMulticast(true); } #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - PrintError("init ok"); + logger_->error("init ok"); #endif cloud_udp_driver_->receiver()->open(); #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - PrintError("open ok"); + 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) { - PrintError( + 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; } cloud_udp_driver_->receiver()->bind(); #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - PrintError("bind ok"); + logger_->error("bind ok"); #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 - PrintError("async receive set"); + logger_->error("async receive set"); #endif } catch (const std::exception & ex) { Status status = Status::UDP_CONNECTION_ERROR; @@ -205,39 +211,39 @@ 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; ss << sensor_configuration; - PrintDebug(ss.str()); + logger_->debug(ss.str()); return Status::ERROR_1; } -Status HesaiHwInterface::GetCalibrationConfiguration( +Status HesaiHwInterface::get_calibration_configuration( CalibrationConfigurationBase & calibration_configuration) { - PrintDebug(calibration_configuration.calibration_file); + 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; @@ -247,8 +253,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 @@ -263,20 +269,20 @@ Status HesaiHwInterface::InitializeTcpDriver() return Status::OK; } -Status HesaiHwInterface::FinalizeTcpDriver() +Status HesaiHwInterface::finalize_tcp_driver() { try { if (tcp_driver_) { tcp_driver_->close(); } } catch (std::exception & e) { - PrintError("Error while finalizing the TcpDriver"); + logger_->error("Error while finalizing the TcpDriver"); return Status::UDP_CONNECTION_ERROR; } 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 { @@ -289,85 +295,93 @@ 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: @@ -375,57 +389,58 @@ 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; @@ -437,42 +452,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; @@ -485,12 +500,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) { @@ -511,12 +526,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; @@ -528,12 +543,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 || @@ -552,12 +567,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 || @@ -565,8 +580,9 @@ 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"); @@ -594,7 +610,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) { @@ -617,20 +633,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) { @@ -659,20 +675,21 @@ 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"); } else if (response.size() > sizeof(HesaiPtpConfig)) { - PrintError("HesaiPtpConfig from Sensor has unknown format. Will parse anyway."); + logger_->error("HesaiPtpConfig from Sensor has unknown format. Will parse anyway."); } HesaiPtpConfig hesai_ptp_config; @@ -684,45 +701,64 @@ HesaiPtpConfig HesaiHwInterface::GetPtpConfig() return hesai_ptp_config; } -Status HesaiHwInterface::SendReset() +Status HesaiHwInterface::set_ptp_lock_offset(uint8_t lock_offset_us) { - auto response_or_err = SendReceive(PTC_COMMAND_RESET); - response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + std::vector request_payload; + request_payload.emplace_back(lock_offset_us); + + auto response_or_err = send_receive(g_ptp_command_set_ptp_lock_offset, request_payload); + response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); return Status::OK; } -Status HesaiHwInterface::SetRotDir(int mode) +uint8_t HesaiHwInterface::get_ptp_lock_offset() +{ + auto response_or_err = send_receive(g_ptp_command_get_ptp_lock_offset); + auto response = + response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); + return check_size_and_parse(response); +} + +Status HesaiHwInterface::send_reset() +{ + 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::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) { @@ -735,24 +771,24 @@ HesaiStatus HesaiHwInterface::GetHttpClientDriverOnce( std::stringstream ss; ss << "HesaiHwInterface::GetHttpClientDriverOnce: " << status << sensor_configuration_->sensor_ip << "," << 80 << std::endl; - PrintError(ss.str()); + logger_->error(ss.str()); return Status::HTTP_CONNECTION_ERROR; } 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; } void HesaiHwInterface::str_cb(const std::string & str) { - PrintInfo(str); + logger_->info(str); } std::pair HesaiHwInterface::unwrap_http_response( @@ -778,11 +814,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; } @@ -809,17 +845,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; } @@ -841,20 +877,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; } @@ -869,19 +905,19 @@ 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) { - PrintError("HesaiHwInterface::GetLidarMonitorAsyncHttp: cannot GetHttpClientDriverOnce"); + logger_->error("HesaiHwInterface::GetLidarMonitorAsyncHttp: cannot GetHttpClientDriverOnce"); return st; } @@ -891,18 +927,18 @@ HesaiStatus HesaiHwInterface::GetLidarMonitorAsyncHttp( boost::system::error_code ec; ctx->run(ec); if (ec) { - PrintError("HesaiHwInterface::GetLidarMonitorAsyncHttp: " + ec.message()); + logger_->error("HesaiHwInterface::GetLidarMonitorAsyncHttp: " + ec.message()); } 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) { @@ -918,20 +954,20 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( if (sensor_configuration->return_mode != current_return_mode) { std::stringstream ss; ss << current_return_mode; - PrintInfo("Current LiDAR return_mode: " + ss.str()); + logger_->info("Current LiDAR return_mode: " + ss.str()); std::stringstream ss2; ss2 << sensor_configuration->return_mode; - PrintInfo("Current Configuration return_mode: " + ss2.str()); + logger_->info("Current Configuration return_mode: " + ss2.str()); std::thread t([this, sensor_configuration] { auto return_mode_int = nebula::drivers::int_from_return_mode_hesai( sensor_configuration->return_mode, sensor_configuration->sensor_model); if (return_mode_int < 0) { - PrintError( + logger_->error( "Invalid Return Mode for this sensor. Please check your settings. Falling back to Dual " "mode."); return_mode_int = 2; } - SetReturnMode(return_mode_int); + set_return_mode(return_mode_int); }); t.join(); std::this_thread::sleep_for(wait_time); @@ -939,19 +975,19 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( auto current_rotation_speed = hesai_config.spin_rate; if (sensor_configuration->rotation_speed != current_rotation_speed.value()) { - PrintInfo( + logger_->info( "current lidar rotation_speed: " + std::to_string(static_cast(current_rotation_speed.value()))); - PrintInfo( + 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 { - PrintInfo( + 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); @@ -969,27 +1005,27 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( : sensor_configuration->multicast_ip; if (desired_host_addr != current_host_addr) { set_flg = true; - PrintInfo("current lidar dest_ipaddr: " + current_host_addr); - PrintInfo("current configuration host_ip: " + desired_host_addr); + logger_->info("current lidar dest_ipaddr: " + current_host_addr); + logger_->info("current configuration host_ip: " + desired_host_addr); } auto current_host_dport = hesai_config.dest_LiDAR_udp_port; if (sensor_configuration->data_port != current_host_dport.value()) { set_flg = true; - PrintInfo( + logger_->info( "current lidar dest_LiDAR_udp_port: " + std::to_string(static_cast(current_host_dport.value()))); - PrintInfo( + logger_->info( "current configuration data_port: " + std::to_string(sensor_configuration->data_port)); } auto current_host_tport = hesai_config.dest_gps_udp_port; if (sensor_configuration->gnss_port != current_host_tport.value()) { set_flg = true; - PrintInfo( + logger_->info( "current lidar dest_gps_udp_port: " + std::to_string(static_cast(current_host_tport.value()))); - PrintInfo( + logger_->info( "current configuration gnss_port: " + std::to_string(sensor_configuration->gnss_port)); } @@ -997,7 +1033,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); @@ -1015,11 +1051,11 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( set_flg = true; } if (sync_flg && set_flg) { - PrintInfo("current lidar sync: " + std::to_string(hesai_config.sync)); - PrintInfo("current lidar sync_angle: " + std::to_string(sensor_sync_angle)); - PrintInfo("current configuration sync_angle: " + std::to_string(config_sync_angle)); + logger_->info("current lidar sync: " + std::to_string(hesai_config.sync)); + 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); } @@ -1031,51 +1067,64 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( 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); + logger_->info("Trying to set Clock source to PTP"); + set_clock_source(g_hesai_lidar_ptp_clock_source); } std::ostringstream tmp_ostringstream; tmp_ostringstream << "Trying to set PTP Config: " << sensor_configuration->ptp_profile << ", Domain: " << std::to_string(sensor_configuration->ptp_domain) << ", Transport: " << sensor_configuration->ptp_transport_type << ", Switch Type: " << sensor_configuration->ptp_switch_type << " via TCP"; - PrintInfo(tmp_ostringstream.str()); - SetPtpConfig( + logger_->info(tmp_ostringstream.str()); + 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); - PrintDebug("Setting properties done"); + 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"); }); - PrintDebug("Waiting for thread to finish"); + logger_->debug("Waiting for thread to finish"); t.join(); - PrintDebug("Thread finished"); + logger_->debug("Thread finished"); + + if ( + sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR128_E4X || + sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARQT128) { + uint8_t sensor_ptp_lock_threshold = get_ptp_lock_offset(); + if (sensor_ptp_lock_threshold != sensor_configuration_->ptp_lock_threshold) { + NEBULA_LOG_STREAM( + logger_->info, "changing sensor PTP lock offset from " + << static_cast(sensor_ptp_lock_threshold) << " to " + << static_cast(sensor_configuration_->ptp_lock_threshold)); + set_ptp_lock_offset(sensor_configuration_->ptp_lock_threshold); + } + } std::this_thread::sleep_for(wait_time); } else { // AT128 only supports PTP setup via HTTP - PrintInfo("Trying to set SyncAngle via HTTP"); - SetSyncAngleSyncHttp(1, sensor_configuration->sync_angle); + logger_->info("Trying to set SyncAngle via HTTP"); + 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"; - PrintInfo(tmp_ostringstream.str()); - SetPtpConfigSyncHttp( + logger_->info(tmp_ostringstream.str()); + 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 std::cout << "End CheckAndSetConfig(HesaiConfig)!!" << std::endl; #endif - PrintDebug("GetAndCheckConfig(HesaiConfig) finished"); + logger_->debug("GetAndCheckConfig(HesaiConfig) finished"); return Status::OK; } -HesaiStatus HesaiHwInterface::CheckAndSetConfig( +HesaiStatus HesaiHwInterface::check_and_set_config( std::shared_ptr sensor_configuration, HesaiLidarRangeAll hesai_lidar_range_all) { @@ -1083,7 +1132,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 @@ -1097,10 +1146,10 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( static_cast(sensor_configuration->cloud_min_angle * 10) != current_cloud_min_angle_ddeg.value()) { set_flg = true; - PrintInfo( + logger_->info( "current lidar range.start: " + std::to_string(static_cast(current_cloud_min_angle_ddeg.value()))); - PrintInfo( + logger_->info( "current configuration cloud_min_angle: " + std::to_string(sensor_configuration->cloud_min_angle)); } @@ -1110,10 +1159,10 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( static_cast(sensor_configuration->cloud_max_angle * 10) != current_cloud_max_angle_ddeg.value()) { set_flg = true; - PrintInfo( + logger_->info( "current lidar range.end: " + std::to_string(static_cast(current_cloud_max_angle_ddeg.value()))); - PrintInfo( + logger_->info( "current configuration cloud_max_angle: " + std::to_string(sensor_configuration->cloud_max_angle)); } @@ -1121,7 +1170,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)); }); @@ -1134,14 +1183,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(); @@ -1153,8 +1202,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(); @@ -1179,7 +1228,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: @@ -1207,16 +1256,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: @@ -1257,11 +1306,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: @@ -1302,54 +1351,12 @@ bool HesaiHwInterface::UseHttpGetLidarMonitor(int model) break; } } -bool HesaiHwInterface::UseHttpGetLidarMonitor() -{ - return UseHttpGetLidarMonitor(target_model_no); -} - -void HesaiHwInterface::SetLogger(std::shared_ptr logger) +bool HesaiHwInterface::use_http_get_lidar_monitor() { - parent_node_logger = logger; + return use_http_get_lidar_monitor(target_model_no_); } -void HesaiHwInterface::PrintInfo(std::string info) -{ - if (parent_node_logger) { - RCLCPP_INFO_STREAM((*parent_node_logger), info); - } else { - std::cout << info << std::endl; - } -} - -void HesaiHwInterface::PrintError(std::string error) -{ - if (parent_node_logger) { - RCLCPP_ERROR_STREAM((*parent_node_logger), error); - } else { - std::cerr << error << std::endl; - } -} - -void HesaiHwInterface::PrintDebug(std::string debug) -{ - if (parent_node_logger) { - RCLCPP_DEBUG_STREAM((*parent_node_logger), debug); - } else { - std::cout << debug << std::endl; - } -} - -void HesaiHwInterface::PrintDebug(const std::vector & bytes) -{ - std::stringstream ss; - for (const auto & b : bytes) { - ss << static_cast(b) << ", "; - } - ss << std::endl; - PrintDebug(ss.str()); -} - -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"; @@ -1365,27 +1372,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: @@ -1404,16 +1411,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"); } @@ -1423,16 +1430,20 @@ 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"); } if (data.size() > sizeof(T)) { - RCLCPP_WARN_ONCE( - *parent_node_logger, - "Sensor returned longer payload than expected. Truncating and parsing anyway."); + // TODO(mojomex): having a static variable for this is not optimal, but the loggers::Logger + // class does not support things like _ONCE macros yet + static bool already_warned_for_this_type = false; + if (!already_warned_for_this_type) { + logger_->warn("Sensor returned longer payload than expected. Truncating and parsing anyway."); + already_warned_for_this_type = true; + } } T parsed; diff --git a/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml b/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml index 33ac9cfb4..39beead4a 100644 --- a/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml +++ b/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml @@ -25,6 +25,7 @@ ptp_domain: 0 ptp_transport_type: L2 ptp_switch_type: TSN + ptp_lock_threshold: 100 retry_hw: true dual_return_distance_threshold: 0.1 point_filters: "{}" diff --git a/nebula_ros/config/lidar/hesai/Pandar40P.param.yaml b/nebula_ros/config/lidar/hesai/Pandar40P.param.yaml index 411500013..4f40283e8 100644 --- a/nebula_ros/config/lidar/hesai/Pandar40P.param.yaml +++ b/nebula_ros/config/lidar/hesai/Pandar40P.param.yaml @@ -25,6 +25,7 @@ ptp_domain: 0 ptp_transport_type: UDP ptp_switch_type: TSN + ptp_lock_threshold: 100 retry_hw: true dual_return_distance_threshold: 0.1 point_filters: "{}" diff --git a/nebula_ros/config/lidar/hesai/Pandar64.param.yaml b/nebula_ros/config/lidar/hesai/Pandar64.param.yaml index a5b748eb8..cf9cc9c1b 100644 --- a/nebula_ros/config/lidar/hesai/Pandar64.param.yaml +++ b/nebula_ros/config/lidar/hesai/Pandar64.param.yaml @@ -25,6 +25,7 @@ ptp_domain: 0 ptp_transport_type: UDP ptp_switch_type: TSN + ptp_lock_threshold: 100 retry_hw: true dual_return_distance_threshold: 0.1 point_filters: "{}" diff --git a/nebula_ros/config/lidar/hesai/PandarAT128.param.yaml b/nebula_ros/config/lidar/hesai/PandarAT128.param.yaml index af6e15aa2..5b53043f7 100644 --- a/nebula_ros/config/lidar/hesai/PandarAT128.param.yaml +++ b/nebula_ros/config/lidar/hesai/PandarAT128.param.yaml @@ -26,6 +26,7 @@ ptp_domain: 0 ptp_transport_type: UDP ptp_switch_type: TSN + ptp_lock_threshold: 100 retry_hw: true dual_return_distance_threshold: 0.1 point_filters: "{}" diff --git a/nebula_ros/config/lidar/hesai/PandarQT128.param.yaml b/nebula_ros/config/lidar/hesai/PandarQT128.param.yaml index 9f99ee5d5..2d80d0c53 100644 --- a/nebula_ros/config/lidar/hesai/PandarQT128.param.yaml +++ b/nebula_ros/config/lidar/hesai/PandarQT128.param.yaml @@ -25,6 +25,7 @@ ptp_domain: 0 ptp_transport_type: UDP ptp_switch_type: TSN + ptp_lock_threshold: 100 retry_hw: true dual_return_distance_threshold: 0.1 point_filters: "{}" diff --git a/nebula_ros/config/lidar/hesai/PandarQT64.param.yaml b/nebula_ros/config/lidar/hesai/PandarQT64.param.yaml index f75d982da..7ec9045bc 100644 --- a/nebula_ros/config/lidar/hesai/PandarQT64.param.yaml +++ b/nebula_ros/config/lidar/hesai/PandarQT64.param.yaml @@ -25,6 +25,7 @@ ptp_domain: 0 ptp_transport_type: UDP ptp_switch_type: TSN + ptp_lock_threshold: 100 retry_hw: true dual_return_distance_threshold: 0.1 point_filters: "{}" diff --git a/nebula_ros/config/lidar/hesai/PandarXT32.param.yaml b/nebula_ros/config/lidar/hesai/PandarXT32.param.yaml index 13243819c..d2e32e7ba 100644 --- a/nebula_ros/config/lidar/hesai/PandarXT32.param.yaml +++ b/nebula_ros/config/lidar/hesai/PandarXT32.param.yaml @@ -25,6 +25,7 @@ ptp_domain: 0 ptp_transport_type: UDP ptp_switch_type: TSN + ptp_lock_threshold: 100 retry_hw: true dual_return_distance_threshold: 0.1 point_filters: "{}" diff --git a/nebula_ros/config/lidar/hesai/PandarXT32M.param.yaml b/nebula_ros/config/lidar/hesai/PandarXT32M.param.yaml index 0ab76740a..35960244a 100644 --- a/nebula_ros/config/lidar/hesai/PandarXT32M.param.yaml +++ b/nebula_ros/config/lidar/hesai/PandarXT32M.param.yaml @@ -25,6 +25,7 @@ ptp_domain: 0 ptp_transport_type: UDP ptp_switch_type: TSN + ptp_lock_threshold: 100 retry_hw: true dual_return_distance_threshold: 0.1 point_filters: "{}" diff --git a/nebula_ros/schema/Pandar128E4X.schema.json b/nebula_ros/schema/Pandar128E4X.schema.json index 752dfcc55..e50a9cc72 100644 --- a/nebula_ros/schema/Pandar128E4X.schema.json +++ b/nebula_ros/schema/Pandar128E4X.schema.json @@ -99,6 +99,9 @@ "ptp_switch_type": { "$ref": "sub/lidar_hesai.json#/definitions/ptp_switch_type" }, + "ptp_lock_threshold": { + "$ref": "sub/lidar_hesai.json#/definitions/ptp_lock_threshold" + }, "retry_hw": { "$ref": "sub/hardware.json#/definitions/retry_hw" }, @@ -133,6 +136,7 @@ "ptp_domain", "ptp_transport_type", "ptp_switch_type", + "ptp_lock_threshold", "retry_hw", "dual_return_distance_threshold", "point_filters" diff --git a/nebula_ros/schema/Pandar40P.schema.json b/nebula_ros/schema/Pandar40P.schema.json index a7d95ce0d..4fa080875 100644 --- a/nebula_ros/schema/Pandar40P.schema.json +++ b/nebula_ros/schema/Pandar40P.schema.json @@ -90,6 +90,9 @@ "ptp_switch_type": { "$ref": "sub/lidar_hesai.json#/definitions/ptp_switch_type" }, + "ptp_lock_threshold": { + "$ref": "sub/lidar_hesai.json#/definitions/ptp_lock_threshold" + }, "retry_hw": { "$ref": "sub/hardware.json#/definitions/retry_hw" }, @@ -124,6 +127,7 @@ "ptp_domain", "ptp_transport_type", "ptp_switch_type", + "ptp_lock_threshold", "retry_hw", "dual_return_distance_threshold", "point_filters" diff --git a/nebula_ros/schema/Pandar64.schema.json b/nebula_ros/schema/Pandar64.schema.json index 77f70c726..d3b0abe8b 100644 --- a/nebula_ros/schema/Pandar64.schema.json +++ b/nebula_ros/schema/Pandar64.schema.json @@ -90,6 +90,9 @@ "ptp_switch_type": { "$ref": "sub/lidar_hesai.json#/definitions/ptp_switch_type" }, + "ptp_lock_threshold": { + "$ref": "sub/lidar_hesai.json#/definitions/ptp_lock_threshold" + }, "retry_hw": { "$ref": "sub/hardware.json#/definitions/retry_hw" }, @@ -124,6 +127,7 @@ "ptp_domain", "ptp_transport_type", "ptp_switch_type", + "ptp_lock_threshold", "retry_hw", "dual_return_distance_threshold", "point_filters" diff --git a/nebula_ros/schema/PandarAT128.schema.json b/nebula_ros/schema/PandarAT128.schema.json index 3c95d0abf..932d3a3fa 100644 --- a/nebula_ros/schema/PandarAT128.schema.json +++ b/nebula_ros/schema/PandarAT128.schema.json @@ -110,6 +110,9 @@ "ptp_switch_type": { "$ref": "sub/lidar_hesai.json#/definitions/ptp_switch_type" }, + "ptp_lock_threshold": { + "$ref": "sub/lidar_hesai.json#/definitions/ptp_lock_threshold" + }, "retry_hw": { "$ref": "sub/hardware.json#/definitions/retry_hw" }, @@ -145,6 +148,7 @@ "ptp_domain", "ptp_transport_type", "ptp_switch_type", + "ptp_lock_threshold", "retry_hw", "dual_return_distance_threshold", "point_filters" diff --git a/nebula_ros/schema/PandarQT128.schema.json b/nebula_ros/schema/PandarQT128.schema.json index e6675ec22..0adfb121a 100644 --- a/nebula_ros/schema/PandarQT128.schema.json +++ b/nebula_ros/schema/PandarQT128.schema.json @@ -93,6 +93,9 @@ "ptp_switch_type": { "$ref": "sub/lidar_hesai.json#/definitions/ptp_switch_type" }, + "ptp_lock_threshold": { + "$ref": "sub/lidar_hesai.json#/definitions/ptp_lock_threshold" + }, "retry_hw": { "$ref": "sub/hardware.json#/definitions/retry_hw" }, @@ -127,6 +130,7 @@ "ptp_domain", "ptp_transport_type", "ptp_switch_type", + "ptp_lock_threshold", "retry_hw", "dual_return_distance_threshold", "point_filters" diff --git a/nebula_ros/schema/PandarQT64.schema.json b/nebula_ros/schema/PandarQT64.schema.json index 949e8d236..46c9e6f2c 100644 --- a/nebula_ros/schema/PandarQT64.schema.json +++ b/nebula_ros/schema/PandarQT64.schema.json @@ -90,6 +90,9 @@ "ptp_switch_type": { "$ref": "sub/lidar_hesai.json#/definitions/ptp_switch_type" }, + "ptp_lock_threshold": { + "$ref": "sub/lidar_hesai.json#/definitions/ptp_lock_threshold" + }, "retry_hw": { "$ref": "sub/hardware.json#/definitions/retry_hw" }, @@ -124,6 +127,7 @@ "ptp_domain", "ptp_transport_type", "ptp_switch_type", + "ptp_lock_threshold", "retry_hw", "dual_return_distance_threshold", "point_filters" diff --git a/nebula_ros/schema/PandarXT32.schema.json b/nebula_ros/schema/PandarXT32.schema.json index 9ccfb3e5c..7f0e32aff 100644 --- a/nebula_ros/schema/PandarXT32.schema.json +++ b/nebula_ros/schema/PandarXT32.schema.json @@ -93,6 +93,9 @@ "ptp_switch_type": { "$ref": "sub/lidar_hesai.json#/definitions/ptp_switch_type" }, + "ptp_lock_threshold": { + "$ref": "sub/lidar_hesai.json#/definitions/ptp_lock_threshold" + }, "retry_hw": { "$ref": "sub/hardware.json#/definitions/retry_hw" }, @@ -127,6 +130,7 @@ "ptp_domain", "ptp_transport_type", "ptp_switch_type", + "ptp_lock_threshold", "retry_hw", "dual_return_distance_threshold", "point_filters" diff --git a/nebula_ros/schema/PandarXT32M.schema.json b/nebula_ros/schema/PandarXT32M.schema.json index 3b96af81b..c53739d94 100644 --- a/nebula_ros/schema/PandarXT32M.schema.json +++ b/nebula_ros/schema/PandarXT32M.schema.json @@ -93,6 +93,9 @@ "ptp_switch_type": { "$ref": "sub/lidar_hesai.json#/definitions/ptp_switch_type" }, + "ptp_lock_threshold": { + "$ref": "sub/lidar_hesai.json#/definitions/ptp_lock_threshold" + }, "retry_hw": { "$ref": "sub/hardware.json#/definitions/retry_hw" }, @@ -127,6 +130,7 @@ "ptp_domain", "ptp_transport_type", "ptp_switch_type", + "ptp_lock_threshold", "retry_hw", "dual_return_distance_threshold", "point_filters" diff --git a/nebula_ros/schema/sub/lidar_hesai.json b/nebula_ros/schema/sub/lidar_hesai.json index b86a6d1ad..eb2e24c31 100644 --- a/nebula_ros/schema/sub/lidar_hesai.json +++ b/nebula_ros/schema/sub/lidar_hesai.json @@ -71,6 +71,13 @@ ], "description": "For automotive profile,'TSN' or 'NON_TSN'." }, + "ptp_lock_threshold": { + "type": "integer", + "default": "100", + "minimum": 1, + "maximum": 100, + "description": "The maximum sensor clock offset in microseconds from the master clock which will still be treated as being locked." + }, "sync_angle": { "type": "integer", "minimum": 0, diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index 6b5a74ba0..fcea59821 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -71,7 +71,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()); @@ -83,7 +83,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 { @@ -175,32 +175,37 @@ nebula::Status HesaiRosWrapper::declare_and_get_sensor_config_params() config.calibration_path = declare_parameter(calibration_parameter_name, param_read_write()); - auto _ptp_profile = declare_parameter("ptp_profile", param_read_only()); - config.ptp_profile = drivers::ptp_profile_from_string(_ptp_profile); + auto ptp_profile = declare_parameter("ptp_profile", param_read_only()); + config.ptp_profile = drivers::ptp_profile_from_string(ptp_profile); - auto _ptp_transport = declare_parameter("ptp_transport_type", param_read_only()); - config.ptp_transport_type = drivers::ptp_transport_type_from_string(_ptp_transport); + auto ptp_transport = declare_parameter("ptp_transport_type", param_read_only()); + config.ptp_transport_type = drivers::ptp_transport_type_from_string(ptp_transport); if ( config.ptp_transport_type != drivers::PtpTransportType::L2 && config.ptp_profile != drivers::PtpProfile::IEEE_1588v2 && config.ptp_profile != drivers::PtpProfile::UNKNOWN_PROFILE) { RCLCPP_WARN_STREAM( - get_logger(), "PTP transport was set to '" << _ptp_transport << "' but PTP profile '" - << _ptp_profile + get_logger(), "PTP transport was set to '" << ptp_transport << "' but PTP profile '" + << ptp_profile << "' only supports 'L2'. Setting it to 'L2'."); config.ptp_transport_type = drivers::PtpTransportType::L2; set_parameter(rclcpp::Parameter("ptp_transport_type", "L2")); } - auto _ptp_switch = declare_parameter("ptp_switch_type", param_read_only()); - config.ptp_switch_type = drivers::ptp_switch_type_from_string(_ptp_switch); + auto ptp_switch = declare_parameter("ptp_switch_type", param_read_only()); + config.ptp_switch_type = drivers::ptp_switch_type_from_string(ptp_switch); { rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); descriptor.integer_range = int_range(0, 127, 1); config.ptp_domain = declare_parameter("ptp_domain", descriptor); } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); + descriptor.integer_range = int_range(1, 100, 1); + config.ptp_lock_threshold = declare_parameter("ptp_lock_threshold", descriptor); + } auto point_filters_raw = declare_parameter("point_filters", param_read_write()); auto point_filters = drivers::parse_point_filters(point_filters_raw, config.sensor_model); @@ -309,7 +314,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( @@ -418,7 +423,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); @@ -480,7 +485,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) { @@ -505,7 +510,7 @@ HesaiRosWrapper::get_calibration_result_t HesaiRosWrapper::get_calibration_data( RCLCPP_ERROR_STREAM(logger, "Could not load downloaded calibration data: " << status); } else if (!ignore_others) { - RCLCPP_ERROR(logger, "No downloaded calibration data found."); + RCLCPP_WARN(logger, "No downloaded calibration data found."); } if (!ignore_others) { diff --git a/nebula_ros/src/hesai/hw_interface_wrapper.cpp b/nebula_ros/src/hesai/hw_interface_wrapper.cpp index b5f991090..d66ad0eff 100644 --- a/nebula_ros/src/hesai/hw_interface_wrapper.cpp +++ b/nebula_ros/src/hesai/hw_interface_wrapper.cpp @@ -3,6 +3,11 @@ #include "nebula_ros/hesai/hw_interface_wrapper.hpp" #include "nebula_ros/common/parameter_descriptors.hpp" +#include "nebula_ros/common/rclcpp_logger.hpp" + +#include + +#include namespace nebula::ros { @@ -10,15 +15,16 @@ namespace nebula::ros HesaiHwInterfaceWrapper::HesaiHwInterfaceWrapper( rclcpp::Node * const parent_node, std::shared_ptr & config, bool use_udp_only) -: hw_interface_(new nebula::drivers::HesaiHwInterface()), - logger_(parent_node->get_logger().get_child("HwInterface")), +: hw_interface_(std::make_shared( + drivers::loggers::RclcppLogger(parent_node->get_logger()).child("HwInterface"))), + logger_(parent_node->get_logger().get_child("HwInterfaceWrapper")), status_(Status::NOT_INITIALIZED), use_udp_only_(use_udp_only) { 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) { @@ -26,8 +32,7 @@ HesaiHwInterfaceWrapper::HesaiHwInterfaceWrapper( (std::stringstream{} << "Could not initialize HW interface: " << status_).str()); } - hw_interface_->SetLogger(std::make_shared(parent_node->get_logger())); - hw_interface_->SetTargetModel(config->sensor_model); + hw_interface_->set_target_model(config->sensor_model); if (use_udp_only) { // Do not initialize TCP @@ -37,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; } @@ -49,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( @@ -68,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 83084a3c9..5b08ece6b 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); @@ -265,6 +265,9 @@ void HesaiHwMonitorWrapper::hesai_check_ptp( if (str == "locked") { level = diagnostic_msgs::msg::DiagnosticStatus::OK; msg = "synchronized"; + } else if (str == "tracking") { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "synchronized, degraded"; } }