From f71026f45c224f8927350533c3bf10231b1fa4a3 Mon Sep 17 00:00:00 2001 From: amc-nu Date: Mon, 13 Nov 2023 14:14:48 +0900 Subject: [PATCH 01/31] add initial support for ot128/128e4x Signed-off-by: amc-nu --- .../nebula_common/hesai/hesai_common.hpp | 3 +++ .../hesai_hw_interface.hpp | 2 +- .../hesai_hw_interface.cpp | 18 ++++++++++++++++-- 3 files changed, 20 insertions(+), 3 deletions(-) diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index a19b025b0..cee8b36a5 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_common.hpp +++ b/nebula_common/include/nebula_common/hesai/hesai_common.hpp @@ -392,6 +392,7 @@ inline ReturnMode ReturnModeFromStringHesai( switch (sensor_model) { case SensorModel::HESAI_PANDARXT32M: case SensorModel::HESAI_PANDARAT128: + case SensorModel::HESAI_PANDAR128_E4X: if (return_mode == "Last") return ReturnMode::LAST; if (return_mode == "Strongest") return ReturnMode::STRONGEST; if (return_mode == "LastStrongest") return ReturnMode::DUAL_LAST_STRONGEST; @@ -424,6 +425,7 @@ inline ReturnMode ReturnModeFromIntHesai(const int return_mode, const SensorMode switch (sensor_model) { case SensorModel::HESAI_PANDARXT32M: case SensorModel::HESAI_PANDARAT128: + case SensorModel::HESAI_PANDAR128_E4X: if (return_mode == 0) return ReturnMode::LAST; if (return_mode == 1) return ReturnMode::STRONGEST; if (return_mode == 2) return ReturnMode::DUAL_LAST_STRONGEST; @@ -455,6 +457,7 @@ inline int IntFromReturnModeHesai(const ReturnMode return_mode, const SensorMode switch (sensor_model) { case SensorModel::HESAI_PANDARXT32M: case SensorModel::HESAI_PANDARAT128: + case SensorModel::HESAI_PANDAR128_E4X: if (return_mode == ReturnMode::LAST) return 0; if (return_mode == ReturnMode::STRONGEST) return 1; if (return_mode == ReturnMode::DUAL_LAST_STRONGEST 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 ad0ce4c16..56777acb1 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 @@ -116,7 +116,7 @@ class HesaiHwInterface : NebulaHwInterfaceBase std::timed_mutex tms_; int tms_fail_cnt = 0; int tms_fail_cnt_max = 3; - bool wl = true; + bool wl = false; bool is_solid_state = false; int target_model_no; 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 29c3f69a0..fd5ec96e7 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 @@ -2903,6 +2903,7 @@ HesaiStatus HesaiHwInterface::GetLidarMonitorAsyncHttp( HesaiStatus HesaiHwInterface::CheckAndSetConfig( std::shared_ptr sensor_configuration, HesaiConfig hesai_config) { + using namespace std::chrono_literals; #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << "Start CheckAndSetConfig(HesaiConfig)!!" << std::endl; #endif @@ -2925,7 +2926,9 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( SetReturnMode(return_mode_int); }); t.join(); + std::this_thread::sleep_for(100ms); } + auto current_rotation_speed = hesai_config.spin_rate; if (sensor_configuration->rotation_speed != current_rotation_speed) { PrintInfo("current lidar rotation_speed: " + std::to_string(current_rotation_speed)); @@ -2935,10 +2938,12 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( if (UseHttpSetSpinRate()) { SetSpinSpeedAsyncHttp(sensor_configuration->rotation_speed); } else { + PrintInfo("Setting up spin rate via TCP." + std::to_string(sensor_configuration->rotation_speed) ); std::thread t( [this, sensor_configuration] { SetSpinRate(sensor_configuration->rotation_speed); }); t.join(); } + std::this_thread::sleep_for(100ms); } bool set_flg = false; @@ -2978,6 +2983,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( sensor_configuration->gnss_port); }); t.join(); + std::this_thread::sleep_for(100ms); } if (sensor_configuration->sensor_model != SensorModel::HESAI_PANDARAT128){ @@ -2996,6 +3002,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( SetSyncAngle(sync_flg, scan_phase); }); t.join(); + std::this_thread::sleep_for(100ms); } std::thread t([this] { @@ -3011,6 +3018,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( ); }); t.join(); + std::this_thread::sleep_for(100ms); } else { //AT128 only supports PTP setup via HTTP PrintInfo("Trying to set SyncAngle via HTTP"); @@ -3198,8 +3206,8 @@ int HesaiHwInterface::NebulaModelToHesaiModelNo(nebula::drivers::SensorModel mod return 38; case SensorModel::HESAI_PANDAR128_E3X://check required return 40; - case SensorModel::HESAI_PANDAR128_E4X://check required - return 40; + case SensorModel::HESAI_PANDAR128_E4X://OT128 + return 42; case SensorModel::HESAI_PANDARAT128: return 48; case SensorModel::VELODYNE_VLS128: @@ -3246,6 +3254,9 @@ bool HesaiHwInterface::UseHttpSetSpinRate(int model) case 38: return false; break; + case 42: + return false; + break; case 48: return false; break; @@ -3285,6 +3296,9 @@ bool HesaiHwInterface::UseHttpGetLidarMonitor(int model) case 38: return false; break; + case 42: + return false; + break; case 48: return false; break; From 05a3b244aaf75be4a074a6661aed3500010d79ee Mon Sep 17 00:00:00 2001 From: amc-nu Date: Mon, 13 Nov 2023 17:04:41 +0900 Subject: [PATCH 02/31] Initial support for QT128 Signed-off-by: amc-nu --- nebula_common/include/nebula_common/hesai/hesai_common.hpp | 3 +++ .../src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp | 3 ++- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index cee8b36a5..ab24c7f36 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_common.hpp +++ b/nebula_common/include/nebula_common/hesai/hesai_common.hpp @@ -393,6 +393,7 @@ inline ReturnMode ReturnModeFromStringHesai( case SensorModel::HESAI_PANDARXT32M: case SensorModel::HESAI_PANDARAT128: case SensorModel::HESAI_PANDAR128_E4X: + case SensorModel::HESAI_PANDARQT128: if (return_mode == "Last") return ReturnMode::LAST; if (return_mode == "Strongest") return ReturnMode::STRONGEST; if (return_mode == "LastStrongest") return ReturnMode::DUAL_LAST_STRONGEST; @@ -426,6 +427,7 @@ inline ReturnMode ReturnModeFromIntHesai(const int return_mode, const SensorMode case SensorModel::HESAI_PANDARXT32M: case SensorModel::HESAI_PANDARAT128: case SensorModel::HESAI_PANDAR128_E4X: + case SensorModel::HESAI_PANDARQT128: if (return_mode == 0) return ReturnMode::LAST; if (return_mode == 1) return ReturnMode::STRONGEST; if (return_mode == 2) return ReturnMode::DUAL_LAST_STRONGEST; @@ -458,6 +460,7 @@ inline int IntFromReturnModeHesai(const ReturnMode return_mode, const SensorMode case SensorModel::HESAI_PANDARXT32M: case SensorModel::HESAI_PANDARAT128: case SensorModel::HESAI_PANDAR128_E4X: + case SensorModel::HESAI_PANDARQT128: if (return_mode == ReturnMode::LAST) return 0; if (return_mode == ReturnMode::STRONGEST) return 1; if (return_mode == ReturnMode::DUAL_LAST_STRONGEST 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 fd5ec96e7..a7791ccbb 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 @@ -2986,7 +2986,8 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( std::this_thread::sleep_for(100ms); } - if (sensor_configuration->sensor_model != SensorModel::HESAI_PANDARAT128){ + if (sensor_configuration->sensor_model != SensorModel::HESAI_PANDARAT128 + && sensor_configuration->sensor_model != SensorModel::HESAI_PANDARQT128) { set_flg = true; auto sync_angle = static_cast(hesai_config.sync_angle / 100); auto scan_phase = static_cast(sensor_configuration->scan_phase); From fbab6ef5a1a4f58422fd9313c884b902b7d22970 Mon Sep 17 00:00:00 2001 From: amc-nu Date: Tue, 12 Dec 2023 06:59:41 +0900 Subject: [PATCH 03/31] hesai. automotive ptp support Signed-off-by: amc-nu --- .../nebula_common/hesai/hesai_common.hpp | 7 +- .../include/nebula_common/nebula_common.hpp | 87 +++++++++++ .../hesai_hw_interface.hpp | 3 - .../hesai_hw_interface.cpp | 38 +++-- nebula_ros/launch/hesai_launch_all_hw.xml | 94 +++++++----- nebula_ros/launch/nebula_launch.py | 7 + .../src/hesai/hesai_decoder_ros_wrapper.cpp | 142 ++++++++++-------- .../hesai/hesai_hw_interface_ros_wrapper.cpp | 43 ++++++ 8 files changed, 303 insertions(+), 118 deletions(-) diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index a19b025b0..1b073839b 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_common.hpp +++ b/nebula_common/include/nebula_common/hesai/hesai_common.hpp @@ -22,6 +22,9 @@ struct HesaiSensorConfiguration : SensorConfigurationBase uint16_t rotation_speed; uint16_t cloud_min_angle; uint16_t cloud_max_angle; + PtpProfile ptp_profile; + uint8_t ptp_domain; + PtpTransportType ptp_transport_type; }; /// @brief Convert HesaiSensorConfiguration to string (Overloading the << operator) /// @param os @@ -32,7 +35,9 @@ inline std::ostream & operator<<(std::ostream & os, HesaiSensorConfiguration con os << (SensorConfigurationBase)(arg) << ", GnssPort: " << arg.gnss_port << ", ScanPhase:" << arg.scan_phase << ", RotationSpeed:" << arg.rotation_speed << ", FOV(Start):" << arg.cloud_min_angle << ", FOV(End):" << arg.cloud_max_angle - << ", DualReturnDistanceThreshold:" << arg.dual_return_distance_threshold; + << ", DualReturnDistanceThreshold:" << arg.dual_return_distance_threshold + << ", PtpProfile:" << arg.ptp_profile << ", PtpDomain:" << std::to_string(arg.ptp_domain) + << ", PtpTransportType:" << arg.ptp_transport_type; return os; } diff --git a/nebula_common/include/nebula_common/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp index 3baccec73..607324b01 100644 --- a/nebula_common/include/nebula_common/nebula_common.hpp +++ b/nebula_common/include/nebula_common/nebula_common.hpp @@ -337,6 +337,19 @@ enum class datatype { FLOAT64 = 8 }; +enum class PtpProfile { + IEEE_1588v2 = 0, + IEEE_802_1AS, + IEEE_802_1AS_AUTO, + PROFILE_UNKNOWN +}; + +enum class PtpTransportType { + UDP_IP = 0, + L2, + UNKNOWN_TRANSPORT +}; + /// @brief not used? struct PointField { @@ -484,6 +497,80 @@ inline ReturnMode ReturnModeFromString(const std::string & return_mode) return ReturnMode::UNKNOWN; } +/// @brief Converts String to PTP Profile +/// @param ptp_profile Profile as String +/// @return Corresponding PtpProfile +inline PtpProfile PtpProfileFromString(const std::string & ptp_profile) +{ + // Hesai + auto tmp_str = ptp_profile; + std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), + [](unsigned char c){ return std::tolower(c); }); + if (tmp_str == "1588v2") return PtpProfile::IEEE_1588v2; + if (tmp_str == "802.1as") return PtpProfile::IEEE_802_1AS; + if (tmp_str == "automotive") return PtpProfile::IEEE_802_1AS_AUTO; + + return PtpProfile::PROFILE_UNKNOWN; +} + +/// @brief Convert PtpProfile enum to string (Overloading the << operator) +/// @param os +/// @param arg +/// @return stream +inline std::ostream & operator<<(std::ostream & os, nebula::drivers::PtpProfile const & arg) +{ + switch (arg) { + case PtpProfile::IEEE_1588v2: + os << "IEEE_1588v2"; + break; + case PtpProfile::IEEE_802_1AS: + os << "IEEE_802.1AS"; + break; + case PtpProfile::IEEE_802_1AS_AUTO: + os << "IEEE_802.1AS Automotive"; + break; + case PtpProfile::PROFILE_UNKNOWN: + os << "UNKNOWN"; + break; + } + return os; +} + +/// @brief Converts String to PTP TransportType +/// @param transport_type Transport as String +/// @return Corresponding PtpTransportType +inline PtpTransportType PtpTransportTypeFromString(const std::string & transport_type) +{ + // Hesai + auto tmp_str = transport_type; + std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), + [](unsigned char c){ return std::tolower(c); }); + if (tmp_str == "udp") return PtpTransportType::UDP_IP; + if (tmp_str == "l2") return PtpTransportType::L2; + + return PtpTransportType::UNKNOWN_TRANSPORT; +} + +/// @brief Convert PtpTransportType enum to string (Overloading the << operator) +/// @param os +/// @param arg +/// @return stream +inline std::ostream & operator<<(std::ostream & os, nebula::drivers::PtpTransportType const & arg) +{ + switch (arg) { + case PtpTransportType::UDP_IP: + os << "UDP/IP"; + break; + case PtpTransportType::L2: + os << "L2"; + break; + case PtpTransportType::UNKNOWN_TRANSPORT: + os << "UNKNOWN"; + break; + } + return os; +} + [[maybe_unused]] pcl::PointCloud::Ptr convertPointXYZIRADTToPointXYZIR( const pcl::PointCloud::ConstPtr & input_pointcloud); diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp index ad0ce4c16..8fd296c4a 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 @@ -77,9 +77,6 @@ const uint16_t PANDAR128_E4X_EXTENDED_PACKET_SIZE = 1117; const uint16_t MTU_SIZE = 1500; -const int PTP_PROFILE = 0; // Fixed: IEEE 1588v2 -const int PTP_DOMAIN_ID = 0; // 0-127, Default: 0 -const int PTP_NETWORK_TRANSPORT = 0; // 0: UDP/IP, 1: L2 const int PTP_LOG_ANNOUNCE_INTERVAL = 1; // Time interval between Announce messages, in units of log seconds (default: 1) const int PTP_SYNC_INTERVAL = 1; //Time interval between Sync messages, in units of log seconds (default: 1) const int PTP_LOG_MIN_DELAY_INTERVAL = 0; //Minimum permitted mean time between Delay_Req messages, in units of log seconds (default: 0) 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 f5815fc40..2714b0254 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 @@ -2322,7 +2322,7 @@ Status HesaiHwInterface::SetPtpConfig( std::vector buf_vec; int len = 6; if (profile == 0) { - } else if (profile == 1) { + } else if (profile >= 1) { len = 3; } else { return Status::ERROR_1; @@ -2995,13 +2995,23 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( t.join(); } - std::thread t([this] { - PrintInfo("Trying to set Clock source to PTP"); - SetClockSource(HESAI_LIDAR_PTP_CLOCK_SOURCE); - PrintInfo("Trying to set PTP Config: IEEE 1588 v2, Domain: 0, Transport: UDP/IP"); - SetPtpConfig(PTP_PROFILE, - PTP_DOMAIN_ID, - PTP_NETWORK_TRANSPORT, + std::thread t([this, sensor_configuration] { + if(sensor_configuration->sensor_model == SensorModel::HESAI_PANDAR40P + || sensor_configuration->sensor_model == SensorModel::HESAI_PANDAR64 + || sensor_configuration->sensor_model == SensorModel::HESAI_PANDARQT64 + || sensor_configuration->sensor_model == SensorModel::HESAI_PANDARXT32 + || sensor_configuration->sensor_model == SensorModel::HESAI_PANDARXT32M) { + PrintInfo("Trying to set Clock source to PTP"); + SetClockSource(HESAI_LIDAR_PTP_CLOCK_SOURCE); + } + std::ostringstream tmp_ostr; + tmp_ostr << "Trying to set PTP Config: " << sensor_configuration->ptp_profile + << ", Domain: " << std::to_string(sensor_configuration->ptp_domain) + << ", Transport: " << sensor_configuration->ptp_transport_type << " via TCP"; + PrintInfo(tmp_ostr.str()); + SetPtpConfig(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 @@ -3013,10 +3023,14 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( PrintInfo("Trying to set SyncAngle via HTTP"); SetSyncAngleSyncHttp(1, static_cast(sensor_configuration->scan_phase)); - PrintInfo("Trying to set PTP Config: IEEE 1588 v2, Domain: 0, Transport: UDP/IP via HTTP"); - SetPtpConfigSyncHttp(PTP_PROFILE, - PTP_DOMAIN_ID, - PTP_NETWORK_TRANSPORT, + std::ostringstream tmp_ostr; + tmp_ostr << "Trying to set PTP Config: " << sensor_configuration->ptp_profile + << ", Domain: " << sensor_configuration->ptp_domain + << ", Transport: " << sensor_configuration->ptp_transport_type << " via HTTP"; + PrintInfo(tmp_ostr.str()); + SetPtpConfigSyncHttp(static_cast(sensor_configuration->ptp_profile), + sensor_configuration->ptp_domain, + static_cast(sensor_configuration->ptp_transport_type), PTP_LOG_ANNOUNCE_INTERVAL, PTP_SYNC_INTERVAL, PTP_LOG_MIN_DELAY_INTERVAL); diff --git a/nebula_ros/launch/hesai_launch_all_hw.xml b/nebula_ros/launch/hesai_launch_all_hw.xml index a04333a59..107a5de67 100644 --- a/nebula_ros/launch/hesai_launch_all_hw.xml +++ b/nebula_ros/launch/hesai_launch_all_hw.xml @@ -20,12 +20,19 @@ + + + + + + + + name="hesai_cloud" output="screen" ros_args="--log-level $(var debug_level)"> @@ -33,47 +40,52 @@ + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + diff --git a/nebula_ros/launch/nebula_launch.py b/nebula_ros/launch/nebula_launch.py index f9c30f47d..6c0c41d09 100644 --- a/nebula_ros/launch/nebula_launch.py +++ b/nebula_ros/launch/nebula_launch.py @@ -91,6 +91,10 @@ def launch_setup(context, *args, **kwargs): "sensor_ip": LaunchConfiguration("sensor_ip"), "return_mode": LaunchConfiguration("return_mode"), "calibration_file": sensor_calib_fp, + "launch_hw": LaunchConfiguration("launch_hw"), + "ptp_profile": LaunchConfiguration("ptp_profile"), + "ptp_domain": LaunchConfiguration("ptp_domain"), + "ptp_transport_type": LaunchConfiguration("ptp_transport_type"), }, ], ), @@ -141,6 +145,9 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("launch_hw", "true"), add_launch_arg("setup_sensor", "true"), add_launch_arg("debug_logging", "false"), + add_launch_arg("ptp_profile", "1588v2"), + add_launch_arg("ptp_domain", "0"), + add_launch_arg("ptp_transport_type", "UDP"), ] + [OpaqueFunction(function=launch_setup)] ) diff --git a/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp index d8e206995..169ace90a 100644 --- a/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp @@ -242,6 +242,16 @@ Status HesaiDriverRosWrapper::GetParameters( sensor_configuration.dual_return_distance_threshold = this->get_parameter("dual_return_distance_threshold").as_double(); } + bool launch_hw; + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("launch_hw", "", descriptor); + launch_hw = this->get_parameter("launch_hw").as_bool(); + } if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { return Status::INVALID_SENSOR_MODEL; } @@ -258,65 +268,71 @@ Status HesaiDriverRosWrapper::GetParameters( hw_interface_.SetSensorConfiguration( std::static_pointer_cast(sensor_cfg_ptr)); - bool run_local = false; - RCLCPP_INFO_STREAM( - this->get_logger(), "Trying to acquire calibration data from sensor: '" - << sensor_configuration.sensor_ip << "'"); - std::cout << "Trying to acquire calibration data from sensor: '" << sensor_configuration.sensor_ip << "'" << std::endl; + bool run_local = !launch_hw; if (sensor_configuration.sensor_model != drivers::SensorModel::HESAI_PANDARAT128) { std::string calibration_file_path_from_sensor; - if (!calibration_configuration.calibration_file.empty()) { + if (launch_hw && !calibration_configuration.calibration_file.empty()) { int ext_pos = calibration_configuration.calibration_file.find_last_of('.'); calibration_file_path_from_sensor += calibration_configuration.calibration_file.substr(0, ext_pos); calibration_file_path_from_sensor += "_from_sensor"; calibration_file_path_from_sensor += calibration_configuration.calibration_file.substr(ext_pos, calibration_configuration.calibration_file.size() - ext_pos); } - std::future future = std::async(std::launch::async, [this, &calibration_configuration, &calibration_file_path_from_sensor, &run_local]() { - if (hw_interface_.InitializeTcpDriver(false) == Status::OK) { - hw_interface_.GetLidarCalibrationFromSensor( - [this, &calibration_configuration, &calibration_file_path_from_sensor](const std::string & str) { - auto rt = calibration_configuration.SaveFileFromString(calibration_file_path_from_sensor, str); - if(rt == Status::OK) - { - RCLCPP_INFO_STREAM(get_logger(), "SaveFileFromString success:" << calibration_file_path_from_sensor << "\n"); - } - else - { - RCLCPP_ERROR_STREAM(get_logger(), "SaveFileFromString failed:" << calibration_file_path_from_sensor << "\n"); - } - rt = calibration_configuration.LoadFromString(str); - if(rt == Status::OK) - { - RCLCPP_INFO_STREAM(get_logger(), "LoadFromString success:" << str << "\n"); - } - else - { - RCLCPP_ERROR_STREAM(get_logger(), "LoadFromString failed:" << str << "\n"); - } - }, - true); - }else{ + if(launch_hw) { + run_local = false; + RCLCPP_INFO_STREAM( + this->get_logger(), "Trying to acquire calibration data from sensor: '" + << sensor_configuration.sensor_ip << "'"); + std::future future = std::async(std::launch::async, + [this, &calibration_configuration, &calibration_file_path_from_sensor, &run_local]() { + if (hw_interface_.InitializeTcpDriver(false) == Status::OK) { + hw_interface_.GetLidarCalibrationFromSensor( + [this, &calibration_configuration, &calibration_file_path_from_sensor]( + const std::string &str) { + auto rt = calibration_configuration.SaveFileFromString( + calibration_file_path_from_sensor, str); + if (rt == Status::OK) { + RCLCPP_INFO_STREAM(get_logger(), "SaveFileFromString success:" + << calibration_file_path_from_sensor << "\n"); + } else { + RCLCPP_ERROR_STREAM(get_logger(), "SaveFileFromString failed:" + << calibration_file_path_from_sensor << "\n"); + } + rt = calibration_configuration.LoadFromString(str); + if (rt == Status::OK) { + RCLCPP_INFO_STREAM(get_logger(), + "LoadFromString success:" << str << "\n"); + } else { + RCLCPP_ERROR_STREAM(get_logger(), + "LoadFromString failed:" << str << "\n"); + } + }, + true); + } else { + run_local = true; + } + }); + std::future_status status; + status = future.wait_for(std::chrono::milliseconds(5000)); + if (status == std::future_status::timeout) { + std::cerr << "# std::future_status::timeout\n"; + RCLCPP_ERROR_STREAM(get_logger(), "GetCalibration Timeout"); run_local = true; + } else if (status == std::future_status::ready && !run_local) { + RCLCPP_INFO_STREAM( + this->get_logger(), "Acquired calibration data from sensor: '" + << sensor_configuration.sensor_ip << "'"); + RCLCPP_INFO_STREAM( + this->get_logger(), "The calibration has been saved in '" + << calibration_file_path_from_sensor << "'"); } - }); - std::future_status status; - status = future.wait_for(std::chrono::milliseconds(8000)); - if (status == std::future_status::timeout) { - std::cerr << "# std::future_status::timeout\n"; - run_local = true; - } else if (status == std::future_status::ready && !run_local) { - RCLCPP_INFO_STREAM( - this->get_logger(), "Acquired calibration data from sensor: '" - << sensor_configuration.sensor_ip << "'"); - RCLCPP_INFO_STREAM( - this->get_logger(), "The calibration has been saved in '" - << calibration_file_path_from_sensor << "'"); } if(run_local) { + RCLCPP_WARN_STREAM(get_logger(), "Running locally"); bool run_org = false; if (calibration_file_path_from_sensor.empty()) { run_org = true; } else { + RCLCPP_INFO_STREAM(get_logger(),"Trying to load file: " << calibration_file_path_from_sensor); auto cal_status = calibration_configuration.LoadFromFile(calibration_file_path_from_sensor); @@ -329,6 +345,7 @@ Status HesaiDriverRosWrapper::GetParameters( } } if(run_org) { + RCLCPP_INFO_STREAM(get_logger(),"Trying to load file: " << calibration_configuration.calibration_file); if (calibration_configuration.calibration_file.empty()) { RCLCPP_ERROR_STREAM( this->get_logger(), "Empty Calibration_file File: '" << calibration_configuration.calibration_file << "'"); @@ -351,16 +368,17 @@ Status HesaiDriverRosWrapper::GetParameters( } } } else { // sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128 - run_local = true; std::string correction_file_path_from_sensor; - if (!correction_file_path.empty()) { + if (launch_hw && !correction_file_path.empty()) { int ext_pos = correction_file_path.find_last_of('.'); correction_file_path_from_sensor += correction_file_path.substr(0, ext_pos); correction_file_path_from_sensor += "_from_sensor"; correction_file_path_from_sensor += correction_file_path.substr(ext_pos, correction_file_path.size() - ext_pos); } - std::future future = std::async(std::launch::async, [this, &correction_configuration, &correction_file_path_from_sensor, &run_local]() { - if (hw_interface_.InitializeTcpDriver(false) == Status::OK) { + std::future future = std::async(std::launch::async, [this, &correction_configuration, &correction_file_path_from_sensor, &run_local, &launch_hw]() { + if (launch_hw && hw_interface_.InitializeTcpDriver(false) == Status::OK) { + RCLCPP_INFO_STREAM( + this->get_logger(), "Trying to acquire calibration data from sensor"); hw_interface_.GetLidarCalibrationFromSensor( [this, &correction_configuration, &correction_file_path_from_sensor, &run_local](const std::vector & received_bytes) { RCLCPP_INFO_STREAM(get_logger(), "AT128 calibration size:" << received_bytes.size() << "\n"); @@ -387,22 +405,24 @@ Status HesaiDriverRosWrapper::GetParameters( } }); }else{ - RCLCPP_ERROR_STREAM(get_logger(), "InitializeTcpDriver failed. Falling back to offline calibration file."); + RCLCPP_ERROR_STREAM(get_logger(), "Falling back to offline calibration file."); run_local = true; } }); - std::future_status status; - status = future.wait_for(std::chrono::milliseconds(8000)); - if (status == std::future_status::timeout) { - std::cerr << "# std::future_status::timeout\n"; - run_local = true; - } else if (status == std::future_status::ready && !run_local) { - RCLCPP_INFO_STREAM( - this->get_logger(), "Acquired correction data from sensor: '" - << sensor_configuration.sensor_ip << "'"); - RCLCPP_INFO_STREAM( - this->get_logger(), "The correction has been saved in '" - << correction_file_path_from_sensor << "'"); + if (!run_local) { + std::future_status status; + status = future.wait_for(std::chrono::milliseconds(8000)); + if (status == std::future_status::timeout) { + std::cerr << "# std::future_status::timeout\n"; + run_local = true; + } else if (status == std::future_status::ready && !run_local) { + RCLCPP_INFO_STREAM( + this->get_logger(), "Acquired correction data from sensor: '" + << sensor_configuration.sensor_ip << "'"); + RCLCPP_INFO_STREAM( + this->get_logger(), "The correction has been saved in '" + << correction_file_path_from_sensor << "'"); + } } if(run_local) { bool run_org = false; diff --git a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp index f3d706c09..d9c0b08cc 100644 --- a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp @@ -346,7 +346,50 @@ Status HesaiHwInterfaceRosWrapper::GetParameters( this->declare_parameter("retry_hw", true, descriptor); this->retry_hw_ = this->get_parameter("retry_hw").as_bool(); } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("ptp_profile", ""); + sensor_configuration.ptp_profile = + nebula::drivers::PtpProfileFromString(this->get_parameter("ptp_profile").as_string()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("ptp_transport_type", ""); + sensor_configuration.ptp_transport_type = + nebula::drivers::PtpTransportTypeFromString(this->get_parameter("ptp_transport_type").as_string()); + if(static_cast(sensor_configuration.ptp_profile) > 0) { + sensor_configuration.ptp_transport_type = nebula::drivers::PtpTransportType::L2; + } + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + rcl_interfaces::msg::IntegerRange range; + range.set__from_value(0).set__to_value(127).set__step(1); + descriptor.integer_range = {range}; + this->declare_parameter("ptp_domain", 0, descriptor); + sensor_configuration.ptp_domain = this->get_parameter("ptp_domain").as_int(); + } + if(sensor_configuration.ptp_profile == nebula::drivers::PtpProfile::PROFILE_UNKNOWN) { + RCLCPP_ERROR_STREAM(get_logger(), "Invalid PTP Profile Provided. Please use '1588v2', '802.1as' or 'automotive'"); + return Status::SENSOR_CONFIG_ERROR; + } + if(sensor_configuration.ptp_transport_type == nebula::drivers::PtpTransportType::UNKNOWN_TRANSPORT) { + RCLCPP_ERROR_STREAM(get_logger(), + "Invalid PTP Transport Provided. Please use 'udp' or 'l2', 'udp' is only available when using the '1588v2' PTP Profile"); + return Status::SENSOR_CONFIG_ERROR; + } if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { return Status::INVALID_SENSOR_MODEL; } From 8959294cb201148de004dfc62c5d7d406b83d337 Mon Sep 17 00:00:00 2001 From: amc-nu Date: Wed, 27 Dec 2023 18:09:13 +0900 Subject: [PATCH 04/31] nebula_decoder. hesai tokenize the calibration file to avoid errors at load Signed-off-by: amc-nu --- .../nebula_common/hesai/hesai_common.hpp | 58 +++++++--------- .../include/nebula_common/nebula_common.hpp | 2 +- nebula_ros/launch/hesai_launch_all_hw.xml | 39 +++++------ nebula_ros/launch/velodyne_launch_all_hw.xml | 68 +++++++++---------- .../src/hesai/hesai_decoder_ros_wrapper.cpp | 6 +- .../hesai/hesai_hw_interface_ros_wrapper.cpp | 2 +- 6 files changed, 86 insertions(+), 89 deletions(-) diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index d31c142f9..b13d829c7 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_common.hpp +++ b/nebula_common/include/nebula_common/hesai/hesai_common.hpp @@ -53,21 +53,10 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase if (!ifs) { return Status::INVALID_CALIBRATION_FILE; } - - std::string header; - std::getline(ifs, header); - - char sep; - int laser_id; - float elevation; - float azimuth; - while (!ifs.eof()) { - ifs >> laser_id >> sep >> elevation >> sep >> azimuth; - elev_angle_map[laser_id - 1] = elevation; - azimuth_offset_map[laser_id - 1] = azimuth; - } + std::ostringstream ss; + ss << ifs.rdbuf(); // reading data ifs.close(); - return Status::OK; + return LoadFromString(ss.str()); } /// @brief Loading calibration data @@ -77,28 +66,32 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase { std::stringstream ss; ss << calibration_content; - - std::string header; - std::getline(ss, header); - - char sep; - int laser_id; - float elevation; - float azimuth; - std::string line; - while (std::getline(ss, line)) { - if (line.empty()) { + constexpr size_t expected_cols = 3; + while(std::getline(ss, line)) { + boost::char_separator sep(","); + boost::tokenizer> tok(line, sep); + + std::vector actual_tokens(tok.begin(), tok.end()); + if (actual_tokens.size() < expected_cols + || actual_tokens.size() > expected_cols + ) { + std::cerr << "Ignoring line with unexpected data:" << line << std::endl; + continue; + } + + try { + int laser_id = std::stoi(actual_tokens[0]); + float elevation = std::stof(actual_tokens[1]); + float azimuth = std::stof(actual_tokens[2]); + elev_angle_map[laser_id - 1] = elevation; + azimuth_offset_map[laser_id - 1] = azimuth; + std::cout << "laser " << laser_id << ", elevation " << elevation << ", azimuth " << azimuth << std::endl; + } catch (const std::invalid_argument& ia) { continue; } - std::stringstream line_ss; - line_ss << line; - line_ss >> laser_id >> sep >> elevation >> sep >> azimuth; - elev_angle_map[laser_id - 1] = elevation; - azimuth_offset_map[laser_id - 1] = azimuth; -// std::cout << "laser_id=" << laser_id << ", elevation=" << elevation << ", azimuth=" << azimuth << std::endl; + } -// std::cout << "LoadFromString fin" << std::endl; return Status::OK; } @@ -136,7 +129,6 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase ofs << calibration_string; // std::cout << calibration_string << std::endl; ofs.close(); - return Status::OK; } }; diff --git a/nebula_common/include/nebula_common/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp index 607324b01..211755762 100644 --- a/nebula_common/include/nebula_common/nebula_common.hpp +++ b/nebula_common/include/nebula_common/nebula_common.hpp @@ -2,7 +2,7 @@ #define NEBULA_COMMON_H #include - +#include #include #include #include diff --git a/nebula_ros/launch/hesai_launch_all_hw.xml b/nebula_ros/launch/hesai_launch_all_hw.xml index 107a5de67..18ad13a71 100644 --- a/nebula_ros/launch/hesai_launch_all_hw.xml +++ b/nebula_ros/launch/hesai_launch_all_hw.xml @@ -27,6 +27,7 @@ + @@ -67,25 +68,25 @@ - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + diff --git a/nebula_ros/launch/velodyne_launch_all_hw.xml b/nebula_ros/launch/velodyne_launch_all_hw.xml index c39437039..3d2059d9c 100644 --- a/nebula_ros/launch/velodyne_launch_all_hw.xml +++ b/nebula_ros/launch/velodyne_launch_all_hw.xml @@ -28,40 +28,40 @@ - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + diff --git a/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp index 169ace90a..f3b8e5baa 100644 --- a/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp @@ -42,8 +42,11 @@ HesaiDriverRosWrapper::HesaiDriverRosWrapper(const rclcpp::NodeOptions & options } RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Wrapper=" << wrapper_status_); + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), + qos_profile); pandar_scan_sub_ = create_subscription( - "pandar_packets", rclcpp::SensorDataQoS(), + "pandar_packets", qos, std::bind(&HesaiDriverRosWrapper::ReceiveScanMsgCallback, this, std::placeholders::_1)); nebula_points_pub_ = this->create_publisher("pandar_points", rclcpp::SensorDataQoS()); @@ -290,6 +293,7 @@ Status HesaiDriverRosWrapper::GetParameters( const std::string &str) { auto rt = calibration_configuration.SaveFileFromString( calibration_file_path_from_sensor, str); + RCLCPP_ERROR_STREAM(get_logger(), str); if (rt == Status::OK) { RCLCPP_INFO_STREAM(get_logger(), "SaveFileFromString success:" << calibration_file_path_from_sensor << "\n"); diff --git a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp index d9c0b08cc..df78c6a34 100644 --- a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp @@ -410,7 +410,7 @@ void HesaiHwInterfaceRosWrapper::ReceiveScanDataCallback( // Publish scan_buffer->header.frame_id = sensor_configuration_.frame_id; scan_buffer->header.stamp = scan_buffer->packets.front().stamp; - pandar_scan_pub_->publish(*scan_buffer); + pandar_scan_pub_->publish(std::move(scan_buffer)); } rcl_interfaces::msg::SetParametersResult HesaiHwInterfaceRosWrapper::paramCallback( From 7a7e981cc803c75ad9beed3621ea38f910323f6f Mon Sep 17 00:00:00 2001 From: amc-nu Date: Wed, 27 Dec 2023 18:49:57 +0900 Subject: [PATCH 05/31] hesai_w. fix spellcheck Signed-off-by: amc-nu --- .../hesai_hw_interface.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp index 23eb14a1b..9b353132d 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 @@ -3012,11 +3012,11 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( PrintInfo("Trying to set Clock source to PTP"); SetClockSource(HESAI_LIDAR_PTP_CLOCK_SOURCE); } - std::ostringstream tmp_ostr; - tmp_ostr << "Trying to set PTP Config: " << sensor_configuration->ptp_profile - << ", Domain: " << std::to_string(sensor_configuration->ptp_domain) - << ", Transport: " << sensor_configuration->ptp_transport_type << " via TCP"; - PrintInfo(tmp_ostr.str()); + 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 << " via TCP"; + PrintInfo(tmp_ostringstream.str()); SetPtpConfig(static_cast(sensor_configuration->ptp_profile), sensor_configuration->ptp_domain, static_cast(sensor_configuration->ptp_transport_type), @@ -3032,11 +3032,11 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( PrintInfo("Trying to set SyncAngle via HTTP"); SetSyncAngleSyncHttp(1, static_cast(sensor_configuration->scan_phase)); - std::ostringstream tmp_ostr; - tmp_ostr << "Trying to set PTP Config: " << sensor_configuration->ptp_profile + 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_ostr.str()); + PrintInfo(tmp_ostringstream.str()); SetPtpConfigSyncHttp(static_cast(sensor_configuration->ptp_profile), sensor_configuration->ptp_domain, static_cast(sensor_configuration->ptp_transport_type), From b06b0239ad3ee8df1b893331774044e7d9d9c8d4 Mon Sep 17 00:00:00 2001 From: amc-nu Date: Tue, 9 Jan 2024 14:41:44 +0900 Subject: [PATCH 06/31] nebula_ros. add xml based component launcher Signed-off-by: amc-nu --- .../launch/hesai_launch_component.launch.xml | 76 +++++++++++++++++++ 1 file changed, 76 insertions(+) create mode 100644 nebula_ros/launch/hesai_launch_component.launch.xml diff --git a/nebula_ros/launch/hesai_launch_component.launch.xml b/nebula_ros/launch/hesai_launch_component.launch.xml new file mode 100644 index 000000000..3a5276e8a --- /dev/null +++ b/nebula_ros/launch/hesai_launch_component.launch.xml @@ -0,0 +1,76 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file From 6cfee936f05c7ccc39bf36de05f2056dc86fb4a1 Mon Sep 17 00:00:00 2001 From: amc-nu Date: Tue, 9 Jan 2024 14:52:55 +0900 Subject: [PATCH 07/31] launch. launch typo Signed-off-by: amc-nu --- nebula_ros/launch/hesai_launch_component.launch.xml | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/nebula_ros/launch/hesai_launch_component.launch.xml b/nebula_ros/launch/hesai_launch_component.launch.xml index 3a5276e8a..57c513ca6 100644 --- a/nebula_ros/launch/hesai_launch_component.launch.xml +++ b/nebula_ros/launch/hesai_launch_component.launch.xml @@ -1,7 +1,7 @@ - + @@ -10,6 +10,9 @@ + + + @@ -38,14 +41,15 @@ - + + - + Date: Tue, 9 Jan 2024 14:58:47 +0900 Subject: [PATCH 08/31] launch. match target container Signed-off-by: amc-nu --- nebula_ros/launch/hesai_launch_component.launch.xml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/nebula_ros/launch/hesai_launch_component.launch.xml b/nebula_ros/launch/hesai_launch_component.launch.xml index 57c513ca6..eb7154777 100644 --- a/nebula_ros/launch/hesai_launch_component.launch.xml +++ b/nebula_ros/launch/hesai_launch_component.launch.xml @@ -31,10 +31,10 @@ - + @@ -50,7 +50,7 @@ - + Date: Tue, 9 Jan 2024 15:11:58 +0900 Subject: [PATCH 09/31] hesai_hw. replace publisher with move operator Signed-off-by: amc-nu --- nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp index f3d706c09..0cd4a84ba 100644 --- a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp @@ -367,7 +367,7 @@ void HesaiHwInterfaceRosWrapper::ReceiveScanDataCallback( // Publish scan_buffer->header.frame_id = sensor_configuration_.frame_id; scan_buffer->header.stamp = scan_buffer->packets.front().stamp; - pandar_scan_pub_->publish(*scan_buffer); + pandar_scan_pub_->publish(std::move(scan_buffer)); } rcl_interfaces::msg::SetParametersResult HesaiHwInterfaceRosWrapper::paramCallback( From bc02e2dc0ad97bf4cff3931ac76ae52bb8d38b03 Mon Sep 17 00:00:00 2001 From: amc-nu Date: Wed, 31 Jan 2024 10:42:58 +0900 Subject: [PATCH 10/31] launch. add ptp support to all launch systems Signed-off-by: amc-nu --- nebula_ros/launch/hesai_launch_all_hw.xml | 8 +++++--- .../launch/hesai_launch_component.launch.xml | 16 ++++++++++++---- nebula_ros/launch/nebula_launch.py | 16 +++++++++++++--- 3 files changed, 30 insertions(+), 10 deletions(-) diff --git a/nebula_ros/launch/hesai_launch_all_hw.xml b/nebula_ros/launch/hesai_launch_all_hw.xml index 18ad13a71..204d08c87 100644 --- a/nebula_ros/launch/hesai_launch_all_hw.xml +++ b/nebula_ros/launch/hesai_launch_all_hw.xml @@ -1,10 +1,14 @@ + + + + @@ -12,11 +16,10 @@ - + - @@ -27,7 +30,6 @@ - diff --git a/nebula_ros/launch/hesai_launch_component.launch.xml b/nebula_ros/launch/hesai_launch_component.launch.xml index eb7154777..b55d0a44e 100644 --- a/nebula_ros/launch/hesai_launch_component.launch.xml +++ b/nebula_ros/launch/hesai_launch_component.launch.xml @@ -1,6 +1,5 @@ - @@ -17,17 +16,23 @@ - + - + + + + + + + @@ -35,7 +40,7 @@ + namespace="/sensing/lidar/top"> @@ -72,6 +77,9 @@ + + + diff --git a/nebula_ros/launch/nebula_launch.py b/nebula_ros/launch/nebula_launch.py index 11d731026..f22a9d6c3 100644 --- a/nebula_ros/launch/nebula_launch.py +++ b/nebula_ros/launch/nebula_launch.py @@ -28,6 +28,8 @@ def get_lidar_make(sensor_name): def launch_setup(context, *args, **kwargs): # Model and make sensor_model = LaunchConfiguration("sensor_model").perform(context) + calibration_file = LaunchConfiguration("calibration_file").perform(context) + correction_file = LaunchConfiguration("correction_file").perform(context) sensor_make, sensor_extension = get_lidar_make(sensor_model) nebula_decoders_share_dir = get_package_share_directory("nebula_decoders") nebula_ros_share_dir = get_package_share_directory("nebula_ros") @@ -64,8 +66,12 @@ def launch_setup(context, *args, **kwargs): "sensor_model": LaunchConfiguration("sensor_model"), "sensor_ip": LaunchConfiguration("sensor_ip"), "return_mode": LaunchConfiguration("return_mode"), - "calibration_file": sensor_calib_fp, + "calibration_file": calibration_file or sensor_calib_fp, + "correction_file": correction_file or sensor_calib_fp, "setup_sensor": LaunchConfiguration("setup_sensor"), + "ptp_profile": LaunchConfiguration("ptp_profile"), + "ptp_domain": LaunchConfiguration("ptp_domain"), + "ptp_transport_type": LaunchConfiguration("ptp_transport_type"), }, ], ), @@ -82,7 +88,8 @@ def launch_setup(context, *args, **kwargs): "sensor_model": sensor_model, "sensor_ip": LaunchConfiguration("sensor_ip"), "return_mode": LaunchConfiguration("return_mode"), - "calibration_file": sensor_calib_fp, + "calibration_file": calibration_file or sensor_calib_fp, + "correction_file": correction_file or sensor_calib_fp, }, ], ) @@ -98,7 +105,8 @@ def launch_setup(context, *args, **kwargs): "sensor_model": sensor_model, "sensor_ip": LaunchConfiguration("sensor_ip"), "return_mode": LaunchConfiguration("return_mode"), - "calibration_file": sensor_calib_fp, + "calibration_file": calibration_file or sensor_calib_fp, + "correction_file": correction_file or sensor_calib_fp, "launch_hw": LaunchConfiguration("launch_hw"), "ptp_profile": LaunchConfiguration("ptp_profile"), "ptp_domain": LaunchConfiguration("ptp_domain"), @@ -149,6 +157,8 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("config_file", ""), add_launch_arg("sensor_model", ""), add_launch_arg("sensor_ip", "192.168.1.201"), + add_launch_arg("calibration_file", ""), + add_launch_arg("correction_file", ""), add_launch_arg("return_mode", "Dual"), add_launch_arg("launch_hw", "true"), add_launch_arg("setup_sensor", "true"), From fc8ff51cf7200f78ed737738c1d0e4284ec451ae Mon Sep 17 00:00:00 2001 From: amc-nu Date: Wed, 31 Jan 2024 14:34:59 +0900 Subject: [PATCH 11/31] launch. component_launch reset namespace Signed-off-by: amc-nu --- nebula_ros/launch/hesai_launch_component.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_ros/launch/hesai_launch_component.launch.xml b/nebula_ros/launch/hesai_launch_component.launch.xml index b55d0a44e..fbd4b2cf7 100644 --- a/nebula_ros/launch/hesai_launch_component.launch.xml +++ b/nebula_ros/launch/hesai_launch_component.launch.xml @@ -40,7 +40,7 @@ + namespace="/"> From 0c9f81071ab39bc8b8dd4361b8ca7670225db156 Mon Sep 17 00:00:00 2001 From: Maximilian Schmeller Date: Thu, 7 Mar 2024 14:46:34 +0900 Subject: [PATCH 12/31] fix(ot128): update timing correction tables to latest datasheet version --- .../decoders/pandar_128e4x.hpp | 132 +++++++++++------- 1 file changed, 78 insertions(+), 54 deletions(-) diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp index 57cbe37ed..d05d1a39f 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp @@ -24,43 +24,67 @@ typedef Packet128E3X Packet128E4X; class Pandar128E4X : public HesaiSensor { private: - enum OperationalState { HIGH_RESOLUTION = 0, STANDARD = 1 }; - - static constexpr int firing_time_offset_static_ns_[128] = { - 49758, 43224, 36690, 30156, 21980, 15446, 8912, 2378, 49758, 43224, 36690, 30156, 2378, - 15446, 8912, 21980, 43224, 30156, 49758, 15446, 36690, 2378, 21980, 8912, 34312, 45002, - 38468, 40846, 40846, 34312, 51536, 47380, 31934, 47380, 31934, 51536, 38468, 27778, 27778, - 45002, 38468, 40846, 51536, 45002, 31934, 47380, 34312, 27778, 38468, 40846, 51536, 45002, - 31934, 34312, 27778, 38468, 40846, 51536, 45002, 31934, 47380, 34312, 27778, 45002, 45002, - 51536, 38468, 40846, 47380, 40846, 31934, 45002, 27778, 38468, 40846, 31934, 27778, 38468, - 34312, 34312, 34312, 47380, 51536, 31934, 51536, 47380, 27778, 49758, 21980, 43224, 15446, - 43224, 36690, 8912, 30156, 2378, 49758, 21980, 36690, 15446, 8912, 30156, 2378, 49758, - 43224, 36690, 30156, 21980, 15446, 8912, 2378, 43224, 49758, 30156, 36690, 21980, 15446, - 2378, 8912, 49758, 43224, 36690, 30156, 21980, 15446, 8912, 2378, 30156}; - - static constexpr int firing_time_offset_as0_ns_[128] = { - -1, -1, -1, -1, 21980, 15446, 8912, 2378, -1, -1, -1, -1, 2378, - 15446, 8912, 21980, -1, 2378, 21980, 8912, 6534, 17224, 10690, 13068, 13068, 6534, - 23758, 19602, 4156, 19602, 4156, 23758, 13068, 13068, 23758, 10690, 4156, 19602, 19602, - 0, 10690, 6534, 23758, 17224, 23758, 19602, 6534, 17224, 4156, 0, 6534, 0, - 17224, 10690, 17224, 17224, 23758, 23758, 10690, 13068, 13068, 13068, 19602, 19602, 6534, - 13068, 4156, 4156, 17224, 17224, 17224, 0, 10690, 10690, 13068, 13068, 4156, 0, - 10690, 6534, 6534, 6534, 19602, 23758, 4156, 23758, 19602, 0, -1, 21980, -1, - 15446, -1, 8912, -1, 2378, -1, -1, 21980, -1, 15446, 8912, -1, 2378, - -1, -1, -1, -1, 21980, 15446, 8912, 2378, -1, -1, -1, -1, 21980, - 15446, 2378, 8912, -1, -1, -1, -1, 21980, 15446, 8912, 2378}; - - static constexpr int firing_time_offset_as1_ns_[128] = { - 21980, 15446, 8912, 2378, -1, -1, -1, -1, 21980, 15446, 8912, 2378, -1, - -1, -1, -1, 8912, -1, -1, -1, 6534, 17224, 10690, 13068, 13068, 6534, - 23758, 19602, 4156, 19602, 4156, 23758, 13068, 13068, 23758, 10690, 4156, 19602, 19602, - 0, 10690, 6534, 23758, 17224, 23758, 19602, 6534, 17224, 4156, 0, 6534, 0, - 17224, 10690, 17224, 17224, 23758, 23758, 10690, 13068, 13068, 13068, 19602, 19602, 6534, - 13068, 4156, 4156, 17224, 17224, 17224, 0, 10690, 10690, 13068, 13068, 4156, 0, - 10690, 6534, 6534, 6534, 19602, 23758, 4156, 23758, 19602, 0, 21980, -1, 15446, - -1, 8912, -1, 2378, -1, 21980, 15446, -1, 8912, -1, -1, 2378, -1, - 21980, 15446, 8912, 2378, -1, -1, -1, -1, 15446, 21980, 2378, 8912, -1, - -1, -1, -1, 21980, 15446, 8912, 2378, -1, -1, -1, -1}; +enum OperationalState { HIGH_RESOLUTION = 0, SHUTDOWN = 1, STANDARD = 2 }; + + static constexpr int hires_as0_time_offset_ns_[128] = { + -1, -1, -1, -1, 21185, 14896, 8607, 2318, -1, -1, -1, -1, 21185, + 14896, 8607, 2318, -1, -1, -1, -1, 21185, 14896, 8607, 2318, 0, 18867, + 12578, 18867, 0, 6289, 16549, 10260, 16549, 10260, 12578, 3971, 3971, 6289, 22838, + 22838, 16549, 18867, 0, 10260, 12578, 18867, 3971, 6289, 12578, 22838, 16549, 6289, + 0, 10260, 16549, 18867, 3971, 10260, 12578, 22838, 3971, 6289, 0, 22838, 16549, + 18867, 0, 10260, 12578, 18867, 3971, 6289, 12578, 22838, 16549, 6289, 0, 10260, + 16549, 18867, 3971, 10260, 12578, 22838, 3971, 6289, 0, 22838, -1, -1, -1, + -1, 21185, 14896, 8607, 2318, -1, -1, -1, -1, 21185, 14896, 8607, 2318, + -1, -1, -1, -1, 21185, 14896, 8607, 2318, -1, -1, -1, -1, 21185, + 14896, 8607, 2318, -1, -1, -1, -1, 21185, 14896, 8607, 2318}; + + static constexpr int hires_as1_time_offset_ns_[128] = { + 21185, 14896, 8607, 2318, -1, -1, -1, -1, 21185, 14896, 8607, 2318, -1, + -1, -1, -1, 21185, 14896, 8607, 2318, -1, -1, -1, -1, 0, 18867, + 12578, 18867, 0, 6289, 16549, 10260, 16549, 10260, 12578, 3971, 3971, 6289, 22838, + 22838, 16549, 18867, 0, 10260, 12578, 18867, 3971, 6289, 12578, 22838, 16549, 6289, + 0, 10260, 16549, 18867, 3971, 10260, 12578, 22838, 3971, 6289, 0, 22838, 16549, + 18867, 0, 10260, 12578, 18867, 3971, 6289, 12578, 22838, 16549, 6289, 0, 10260, + 16549, 18867, 3971, 10260, 12578, 22838, 3971, 6289, 0, 22838, 21185, 14896, 8607, + 2318, -1, -1, -1, -1, 21185, 14896, 8607, 2318, -1, -1, -1, -1, + 21185, 14896, 8607, 2318, -1, -1, -1, -1, 21185, 14896, 8607, 2318, -1, + -1, -1, -1, 21185, 14896, 8607, 2318, -1, -1, -1, -1}; + + static constexpr int hires_as2_time_offset_ns_[128] = { + -1, -1, -1, -1, 21185, 14896, 8607, 2318, -1, -1, -1, -1, 21185, + 14896, 8607, 2318, -1, -1, -1, -1, 21185, 14896, 8607, 2318, 0, 18867, + 12578, 18867, 0, 6289, 16549, 10260, 16549, 10260, 12578, 3971, 3971, 6289, 22838, + 22838, 16549, 18867, 0, 10260, 12578, 18867, 3971, 6289, 12578, 22838, 16549, 6289, + 0, 10260, 16549, 18867, 3971, 10260, 12578, 22838, 3971, 6289, 0, 22838, 16549, + 18867, 0, 10260, 12578, 18867, 3971, 6289, 12578, 22838, 16549, 6289, 0, 10260, + 16549, 18867, 3971, 10260, 12578, 22838, 3971, 6289, 0, 22838, -1, -1, -1, + -1, 21185, 14896, 8607, 2318, -1, -1, -1, -1, 21185, 14896, 8607, 2318, + -1, -1, -1, -1, 21185, 14896, 8607, 2318, -1, -1, -1, -1, 21185, + 14896, 8607, 2318, -1, -1, -1, -1, 21185, 14896, 8607, 2318}; + + static constexpr int hires_as3_time_offset_ns_[128] = { + 21185, 14896, 8607, 2318, -1, -1, -1, -1, 21185, 14896, 8607, 2318, -1, + -1, -1, -1, 21185, 14896, 8607, 2318, -1, -1, -1, -1, 0, 18867, + 12578, 18867, 0, 6289, 16549, 10260, 16549, 10260, 12578, 3971, 3971, 6289, 22838, + 22838, 16549, 18867, 0, 10260, 12578, 18867, 3971, 6289, 12578, 22838, 16549, 6289, + 0, 10260, 16549, 18867, 3971, 10260, 12578, 22838, 3971, 6289, 0, 22838, 16549, + 18867, 0, 10260, 12578, 18867, 3971, 6289, 12578, 22838, 16549, 6289, 0, 10260, + 16549, 18867, 3971, 10260, 12578, 22838, 3971, 6289, 0, 22838, 21185, 14896, 8607, + 2318, -1, -1, -1, -1, 21185, 14896, 8607, 2318, -1, -1, -1, -1, + 21185, 14896, 8607, 2318, -1, -1, -1, -1, 21185, 14896, 8607, 2318, -1, + -1, -1, -1, 21185, 14896, 8607, 2318, -1, -1, -1, -1}; + + static constexpr int standard_time_offset_ns_[128] = { + 48963, 42674, 36385, 30096, 21185, 14896, 8607, 2318, 48963, 42674, 36385, 30096, 21185, + 14896, 8607, 2318, 48963, 42674, 36385, 30096, 21185, 14896, 8607, 2318, 27778, 46645, + 40356, 46645, 27778, 34067, 44327, 38038, 44327, 38038, 40356, 31749, 31749, 34067, 50616, + 50616, 44327, 46645, 27778, 38038, 40356, 46645, 31749, 34067, 40356, 50616, 44327, 34067, + 27778, 38038, 44327, 46645, 31749, 38038, 40356, 50616, 31749, 34067, 27778, 50616, 44327, + 46645, 27778, 38038, 40356, 46645, 31749, 34067, 40356, 50616, 44327, 34067, 27778, 38038, + 44327, 46645, 31749, 38038, 40356, 50616, 31749, 34067, 27778, 50616, 48963, 42674, 36385, + 30096, 21185, 14896, 8607, 2318, 48963, 42674, 36385, 30096, 21185, 14896, 8607, 2318, + 48963, 42674, 36385, 30096, 21185, 14896, 8607, 2318, 48963, 42674, 36385, 30096, 21185, + 14896, 8607, 2318, 48963, 42674, 36385, 30096, 21185, 14896, 8607, 2318}; public: static constexpr float MIN_RANGE = 0.1; @@ -71,28 +95,28 @@ class Pandar128E4X : public HesaiSensor uint32_t block_id, uint32_t channel_id, const packet_t & packet) { auto n_returns = hesai_packet::get_n_returns(packet.tail.return_mode); - int block_offset_ns; - if (n_returns == 1) { - block_offset_ns = -27778 * 2 * (2 - block_id - 1); - } else { - block_offset_ns = 0; - } + int block_offset_ns = -27778 * (2 - block_id - 1) / n_returns; int channel_offset_ns; bool is_hires_mode = packet.tail.operational_state == OperationalState::HIGH_RESOLUTION; - auto azimuth_state = packet.tail.geAzimuthState(block_id); + if (!is_hires_mode) block_offset_ns *= 2; - if (!is_hires_mode) { - channel_offset_ns = firing_time_offset_static_ns_[channel_id]; - } else { - if (azimuth_state == 0) { - channel_offset_ns = firing_time_offset_as0_ns_[channel_id]; - } else /* azimuth_state == 1 */ { - channel_offset_ns = firing_time_offset_as1_ns_[channel_id]; - } - } + auto azimuth_state = packet.tail.geAzimuthState(block_id); - return block_offset_ns + 43346 + channel_offset_ns; + if (is_hires_mode && azimuth_state == 0) + channel_offset_ns = hires_as0_time_offset_ns_[channel_id]; + else if (is_hires_mode && azimuth_state == 1) + channel_offset_ns = hires_as1_time_offset_ns_[channel_id]; + else if (is_hires_mode && azimuth_state == 2) + channel_offset_ns = hires_as2_time_offset_ns_[channel_id]; + else if (is_hires_mode && azimuth_state == 3) + channel_offset_ns = hires_as3_time_offset_ns_[channel_id]; + else if (!is_hires_mode) + channel_offset_ns = standard_time_offset_ns_[channel_id]; + else + throw std::runtime_error("Invalid combination of operational state and azimuth state"); + + return block_offset_ns + channel_offset_ns; } ReturnType getReturnType( From 362ab00a348543fa76c7fa4fae12ba686769bdcf Mon Sep 17 00:00:00 2001 From: Maximilian Schmeller Date: Thu, 7 Mar 2024 16:48:34 +0900 Subject: [PATCH 13/31] testing --- .../decoders/pandar_128e3x.hpp | 1 - .../decoders/pandar_128e4x.hpp | 1 - .../hesai_hw_interface.hpp | 2 - .../hesai_hw_interface.cpp | 301 ++++++------------ scripts/plot_pcap_packet_times.py | 94 ++++++ 5 files changed, 191 insertions(+), 208 deletions(-) create mode 100644 scripts/plot_pcap_packet_times.py diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp index 64861d651..65155efb4 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp @@ -68,7 +68,6 @@ struct Packet128E3X : public PacketBase<2, 128, 2, 100> } // namespace hesai_packet -// FIXME(mojomex) support high resolution mode class Pandar128E3X : public HesaiSensor { private: diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp index d05d1a39f..412c8aac7 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp @@ -20,7 +20,6 @@ typedef Packet128E3X Packet128E4X; } // namespace hesai_packet -// FIXME(mojomex) support high resolution mode class Pandar128E4X : public HesaiSensor { private: 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 c9c54896a..49521b722 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 @@ -90,10 +90,8 @@ class HesaiHwInterface : NebulaHwInterfaceBase private: std::unique_ptr<::drivers::common::IoContext> cloud_io_context_; std::shared_ptr m_owned_ctx; - std::shared_ptr m_owned_ctx_s; std::unique_ptr<::drivers::udp_driver::UdpDriver> cloud_udp_driver_; std::shared_ptr<::drivers::tcp_driver::TcpDriver> tcp_driver_; - std::shared_ptr<::drivers::tcp_driver::TcpDriver> tcp_driver_s_; std::shared_ptr sensor_configuration_; std::shared_ptr calibration_configuration_; size_t azimuth_index_{}; 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 9b353132d..090677373 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 @@ -14,10 +14,8 @@ namespace drivers HesaiHwInterface::HesaiHwInterface() : cloud_io_context_{new ::drivers::common::IoContext(1)}, m_owned_ctx{new boost::asio::io_context(1)}, - m_owned_ctx_s{new boost::asio::io_context(1)}, cloud_udp_driver_{new ::drivers::udp_driver::UdpDriver(*cloud_io_context_)}, tcp_driver_{new ::drivers::tcp_driver::TcpDriver(m_owned_ctx)}, - tcp_driver_s_{new ::drivers::tcp_driver::TcpDriver(m_owned_ctx_s)}, scan_cloud_ptr_{std::make_unique()} { } @@ -43,25 +41,6 @@ HesaiHwInterface::~HesaiHwInterface() } #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << ".......................ed: if(tcp_driver_)" << std::endl; -#endif - } - if(tcp_driver_s_) - { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << ".......................tcp_driver_s_ is available" << std::endl; -#endif - if(tcp_driver_s_->isOpen()) - { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << ".......................st: tcp_driver_s_->close();" << std::endl; -#endif - tcp_driver_s_->close(); -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << ".......................ed: tcp_driver_s_->close();" << std::endl; -#endif - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << ".......................ed: if(tcp_driver_s_)" << std::endl; #endif } #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE @@ -261,20 +240,6 @@ Status HesaiHwInterface::InitializeTcpDriver(bool setup_sensor) tcp_driver_->closeSync(); return Status::ERROR_1; } - if (setup_sensor && tcp_driver_s_) { - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - tcp_driver_s_->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - if (!tcp_driver_s_->open()) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "!tcp_driver_s_->open()" << std::endl; -#endif -// tcp_driver_s_->close(); - tcp_driver_s_->closeSync(); - return Status::ERROR_1; - } - } return Status::OK; } @@ -1638,11 +1603,11 @@ Status HesaiHwInterface::SetSpinRate( Status HesaiHwInterface::SetSpinRate(uint16_t rpm, bool with_run) { if (with_run) { - if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { - tcp_driver_s_->GetIOContext()->restart(); + if (tcp_driver_ && tcp_driver_->GetIOContext()->stopped()) { + tcp_driver_->GetIOContext()->restart(); } } - return SetSpinRate(tcp_driver_s_, rpm, with_run); + return SetSpinRate(tcp_driver_, rpm, with_run); } Status HesaiHwInterface::SetSyncAngle( @@ -1694,11 +1659,11 @@ Status HesaiHwInterface::SetSyncAngle( Status HesaiHwInterface::SetSyncAngle(int sync_angle, int angle, bool with_run) { if (with_run) { - if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { - tcp_driver_s_->GetIOContext()->restart(); + if (tcp_driver_ && tcp_driver_->GetIOContext()->stopped()) { + tcp_driver_->GetIOContext()->restart(); } } - return SetSyncAngle(tcp_driver_s_, sync_angle, angle, with_run); + return SetSyncAngle(tcp_driver_, sync_angle, angle, with_run); } Status HesaiHwInterface::SetTriggerMethod( @@ -1748,11 +1713,11 @@ Status HesaiHwInterface::SetTriggerMethod( Status HesaiHwInterface::SetTriggerMethod(int trigger_method, bool with_run) { if (with_run) { - if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { - tcp_driver_s_->GetIOContext()->restart(); + if (tcp_driver_ && tcp_driver_->GetIOContext()->stopped()) { + tcp_driver_->GetIOContext()->restart(); } } - return SetTriggerMethod(tcp_driver_s_, trigger_method, with_run); + return SetTriggerMethod(tcp_driver_, trigger_method, with_run); } Status HesaiHwInterface::SetStandbyMode( @@ -1801,11 +1766,11 @@ Status HesaiHwInterface::SetStandbyMode( Status HesaiHwInterface::SetStandbyMode(int standby_mode, bool with_run) { if (with_run) { - if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { - tcp_driver_s_->GetIOContext()->restart(); + if (tcp_driver_ && tcp_driver_->GetIOContext()->stopped()) { + tcp_driver_->GetIOContext()->restart(); } } - return SetStandbyMode(tcp_driver_s_, standby_mode, with_run); + return SetStandbyMode(tcp_driver_, standby_mode, with_run); } Status HesaiHwInterface::SetReturnMode( @@ -1855,12 +1820,12 @@ Status HesaiHwInterface::SetReturnMode(int return_mode, bool with_run) { //* if (with_run) { - if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { - tcp_driver_s_->GetIOContext()->restart(); + if (tcp_driver_ && tcp_driver_->GetIOContext()->stopped()) { + tcp_driver_->GetIOContext()->restart(); } } //*/ - return SetReturnMode(tcp_driver_s_, return_mode, with_run); + return SetReturnMode(tcp_driver_, return_mode, with_run); } Status HesaiHwInterface::SetDestinationIp( @@ -1921,12 +1886,12 @@ Status HesaiHwInterface::SetDestinationIp( int dest_ip_1, int dest_ip_2, int dest_ip_3, int dest_ip_4, int port, int gps_port, bool with_run) { if (with_run) { - if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { - tcp_driver_s_->GetIOContext()->restart(); + if (tcp_driver_ && tcp_driver_->GetIOContext()->stopped()) { + tcp_driver_->GetIOContext()->restart(); } } return SetDestinationIp( - tcp_driver_s_, dest_ip_1, dest_ip_2, dest_ip_3, dest_ip_4, port, gps_port, with_run); + tcp_driver_, dest_ip_1, dest_ip_2, dest_ip_3, dest_ip_4, port, gps_port, with_run); } Status HesaiHwInterface::SetControlPort( @@ -2000,12 +1965,12 @@ Status HesaiHwInterface::SetControlPort( bool with_run) { if (with_run) { - if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { - tcp_driver_s_->GetIOContext()->restart(); + if (tcp_driver_ && tcp_driver_->GetIOContext()->stopped()) { + tcp_driver_->GetIOContext()->restart(); } } return SetControlPort( - tcp_driver_s_, ip_1, ip_2, ip_3, ip_4, mask_1, mask_2, mask_3, mask_4, gateway_1, gateway_2, + tcp_driver_, ip_1, ip_2, ip_3, ip_4, mask_1, mask_2, mask_3, mask_4, gateway_1, gateway_2, gateway_3, gateway_4, vlan_flg, vlan_id, with_run); } @@ -2063,11 +2028,11 @@ Status HesaiHwInterface::SetLidarRange( Status HesaiHwInterface::SetLidarRange(int method, std::vector data, bool with_run) { if (with_run) { - if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { - tcp_driver_s_->GetIOContext()->restart(); + if (tcp_driver_ && tcp_driver_->GetIOContext()->stopped()) { + tcp_driver_->GetIOContext()->restart(); } } - return SetLidarRange(tcp_driver_s_, method, data, with_run); + return SetLidarRange(tcp_driver_, method, data, with_run); } Status HesaiHwInterface::SetLidarRange( @@ -2128,11 +2093,11 @@ Status HesaiHwInterface::SetLidarRange( Status HesaiHwInterface::SetLidarRange(int start, int end, bool with_run) { if (with_run) { - if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { - tcp_driver_s_->GetIOContext()->restart(); + if (tcp_driver_ && tcp_driver_->GetIOContext()->stopped()) { + tcp_driver_->GetIOContext()->restart(); } } - return SetLidarRange(tcp_driver_s_, start, end, with_run); + return SetLidarRange(tcp_driver_, start, end, with_run); } Status HesaiHwInterface::GetLidarRange( @@ -2306,12 +2271,12 @@ Status HesaiHwInterface::SetClockSource(int clock_source, bool with_run) { //* if (with_run) { - if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { - tcp_driver_s_->GetIOContext()->restart(); + if (tcp_driver_ && tcp_driver_->GetIOContext()->stopped()) { + tcp_driver_->GetIOContext()->restart(); } } //*/ - return SetClockSource(tcp_driver_s_, clock_source, with_run); + return SetClockSource(tcp_driver_, clock_source, with_run); } Status HesaiHwInterface::SetPtpConfig( @@ -2383,12 +2348,12 @@ Status HesaiHwInterface::SetPtpConfig( int logMinDelayReqInterval, bool with_run) { if (with_run) { - if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { - tcp_driver_s_->GetIOContext()->restart(); + if (tcp_driver_ && tcp_driver_->GetIOContext()->stopped()) { + tcp_driver_->GetIOContext()->restart(); } } return SetPtpConfig( - tcp_driver_s_, profile, domain, network, logAnnounceInterval, logSyncInterval, + tcp_driver_, profile, domain, network, logAnnounceInterval, logSyncInterval, logMinDelayReqInterval, with_run); } @@ -2485,11 +2450,11 @@ Status HesaiHwInterface::GetPtpConfig(std::shared_ptr c Status HesaiHwInterface::GetPtpConfig(bool with_run) { if (with_run) { - if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { - tcp_driver_s_->GetIOContext()->restart(); + if (tcp_driver_ && tcp_driver_->GetIOContext()->stopped()) { + tcp_driver_->GetIOContext()->restart(); } } - return GetPtpConfig(tcp_driver_s_, with_run); + return GetPtpConfig(tcp_driver_, with_run); } Status HesaiHwInterface::SendReset( @@ -2536,12 +2501,12 @@ Status HesaiHwInterface::SendReset(bool with_run) { //* if (with_run) { - if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { - tcp_driver_s_->GetIOContext()->restart(); + if (tcp_driver_ && tcp_driver_->GetIOContext()->stopped()) { + tcp_driver_->GetIOContext()->restart(); } } //*/ - return SendReset(tcp_driver_s_, with_run); + return SendReset(tcp_driver_, with_run); } Status HesaiHwInterface::SetRotDir( @@ -2590,11 +2555,11 @@ Status HesaiHwInterface::SetRotDir( Status HesaiHwInterface::SetRotDir(int mode, bool with_run) { if (with_run) { - if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { - tcp_driver_s_->GetIOContext()->restart(); + if (tcp_driver_ && tcp_driver_->GetIOContext()->stopped()) { + tcp_driver_->GetIOContext()->restart(); } } - return SetRotDir(tcp_driver_s_, mode, with_run); + return SetRotDir(tcp_driver_, mode, with_run); } Status HesaiHwInterface::GetLidarMonitor( @@ -2913,16 +2878,13 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( std::stringstream ss2; ss2 << sensor_configuration->return_mode; PrintInfo("Current Configuration return_mode: " + ss2.str()); - std::thread t([this, sensor_configuration] { - auto return_mode_int = nebula::drivers::IntFromReturnModeHesai( - sensor_configuration->return_mode, sensor_configuration->sensor_model); - if (return_mode_int < 0) { - PrintError("Invalid Return Mode for this sensor. Please check your settings. Falling back to Dual mode."); - return_mode_int = 2; - } - SetReturnMode(return_mode_int); - }); - t.join(); + auto return_mode_int = nebula::drivers::IntFromReturnModeHesai( + sensor_configuration->return_mode, sensor_configuration->sensor_model); + if (return_mode_int < 0) { + PrintError("Invalid Return Mode for this sensor. Please check your settings. Falling back to Dual mode."); + return_mode_int = 2; + } + SetReturnMode(return_mode_int); std::this_thread::sleep_for(100ms); } @@ -2936,9 +2898,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( SetSpinSpeedAsyncHttp(sensor_configuration->rotation_speed); } else { PrintInfo("Setting up spin rate via TCP." + std::to_string(sensor_configuration->rotation_speed) ); - std::thread t( - [this, sensor_configuration] { SetSpinRate(sensor_configuration->rotation_speed); }); - t.join(); + SetSpinRate(sensor_configuration->rotation_speed); } std::this_thread::sleep_for(100ms); } @@ -2973,13 +2933,10 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( if (set_flg) { std::vector list_string; boost::split(list_string, sensor_configuration->host_ip, boost::is_any_of(".")); - std::thread t([this, sensor_configuration, list_string] { - SetDestinationIp( - 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); - }); - t.join(); + SetDestinationIp( + 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); std::this_thread::sleep_for(100ms); } @@ -2996,36 +2953,30 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( PrintInfo("current lidar sync: " + std::to_string(hesai_config.sync)); PrintInfo("current lidar sync_angle: " + std::to_string(sync_angle)); PrintInfo("current configuration scan_phase: " + std::to_string(scan_phase)); - std::thread t([this, sync_flg, scan_phase] { - SetSyncAngle(sync_flg, scan_phase); - }); - t.join(); + SetSyncAngle(sync_flg, scan_phase); std::this_thread::sleep_for(100ms); } - std::thread t([this, sensor_configuration] { - if(sensor_configuration->sensor_model == SensorModel::HESAI_PANDAR40P - || sensor_configuration->sensor_model == SensorModel::HESAI_PANDAR64 - || sensor_configuration->sensor_model == SensorModel::HESAI_PANDARQT64 - || sensor_configuration->sensor_model == SensorModel::HESAI_PANDARXT32 - || sensor_configuration->sensor_model == SensorModel::HESAI_PANDARXT32M) { - PrintInfo("Trying to set Clock source to PTP"); - SetClockSource(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 << " via TCP"; - PrintInfo(tmp_ostringstream.str()); - SetPtpConfig(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 - ); - }); - t.join(); + if(sensor_configuration->sensor_model == SensorModel::HESAI_PANDAR40P + || sensor_configuration->sensor_model == SensorModel::HESAI_PANDAR64 + || sensor_configuration->sensor_model == SensorModel::HESAI_PANDARQT64 + || sensor_configuration->sensor_model == SensorModel::HESAI_PANDARXT32 + || sensor_configuration->sensor_model == SensorModel::HESAI_PANDARXT32M) { + PrintInfo("Trying to set Clock source to PTP"); + SetClockSource(HESAI_LIDAR_PTP_CLOCK_SOURCE); + } + 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 << " via TCP"; + PrintInfo(tmp_ostringstream.str()); + SetPtpConfig(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 + ); std::this_thread::sleep_for(100ms); } else { //AT128 only supports PTP setup via HTTP @@ -3089,14 +3040,11 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( } if (set_flg) { - std::thread t([this, sensor_configuration] { - SetLidarRange( - static_cast(sensor_configuration->cloud_min_angle * 10), - static_cast(sensor_configuration->cloud_max_angle * 10) //, - // false - ); - }); - t.join(); + SetLidarRange( + static_cast(sensor_configuration->cloud_min_angle * 10), + static_cast(sensor_configuration->cloud_max_angle * 10) //, + // false + ); } #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE @@ -3110,74 +3058,23 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig() #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << "Start CheckAndSetConfig!!" << std::endl; #endif - if (true) { - std::thread t([this] { - GetConfig( // ctx, - [this](HesaiConfig & result) { - std::stringstream ss; - ss << result; - PrintInfo(ss.str()); - CheckAndSetConfig( - std::static_pointer_cast(sensor_configuration_), result); - }); - }); - t.join(); - - std::thread t2([this] { - GetLidarRange( // ctx, - [this](HesaiLidarRangeAll & result) { - std::stringstream ss; - ss << result; - PrintInfo(ss.str()); - CheckAndSetConfig( - std::static_pointer_cast(sensor_configuration_), result); - }); - }); - t2.join(); - } else if (false) { - GetConfig([this](HesaiConfig & result) { - std::cout << result << std::endl; - CheckAndSetConfig( - std::static_pointer_cast(sensor_configuration_), result); - }); - GetLidarRange([this](HesaiLidarRangeAll & result) { - std::cout << result << std::endl; - CheckAndSetConfig( - std::static_pointer_cast(sensor_configuration_), result); - }); - std::cout << "make thread t2" << std::endl; - std::thread t2([this] { - tcp_driver_->GetIOContext()->restart(); - tcp_driver_->run(); - }); - std::cout << "made thread t2" << std::endl; - t2.join(); - std::cout << "joined thread t2" << std::endl; - - } else { - bool stopped = tcp_driver_->GetIOContext()->stopped(); - std::cout << "stopped: " << stopped << std::endl; - if (stopped) tcp_driver_->GetIOContext()->restart(); - GetConfig( - [this](HesaiConfig & result) { - std::cout << result << std::endl; - CheckAndSetConfig( - std::static_pointer_cast(sensor_configuration_), result); - }, - false); - if (stopped) tcp_driver_->run(); - stopped = tcp_driver_->GetIOContext()->stopped(); - std::cout << "stopped2: " << stopped << std::endl; - if (stopped) tcp_driver_->GetIOContext()->restart(); - GetLidarRange( - [this](HesaiLidarRangeAll & result) { - std::cout << result << std::endl; - CheckAndSetConfig( - std::static_pointer_cast(sensor_configuration_), result); - }, - false); - if (stopped) tcp_driver_->run(); - } +GetConfig( // ctx, + [this](HesaiConfig & result) { + std::stringstream ss; + ss << result; + PrintInfo(ss.str()); + CheckAndSetConfig( + std::static_pointer_cast(sensor_configuration_), result); + }); + +GetLidarRange( // ctx, + [this](HesaiLidarRangeAll & result) { + std::stringstream ss; + ss << result; + PrintInfo(ss.str()); + CheckAndSetConfig( + std::static_pointer_cast(sensor_configuration_), result); + }); #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << "End CheckAndSetConfig!!" << std::endl; #endif @@ -3352,10 +3249,6 @@ bool HesaiHwInterface::CheckLock( tcp_driver_->closeSync(); tcp_driver_->open(); } - if (tcp_driver_s_ && tcp_driver_s_->isOpen()) { - tcp_driver_s_->closeSync(); - tcp_driver_s_->open(); - } } else { return true; diff --git a/scripts/plot_pcap_packet_times.py b/scripts/plot_pcap_packet_times.py new file mode 100644 index 000000000..cf07d9b5b --- /dev/null +++ b/scripts/plot_pcap_packet_times.py @@ -0,0 +1,94 @@ +#!/usr/bin/python3 + +import socket +import dpkt +import time +import argparse +import os +import tqdm + +from dpkt.udp import UDP +from dpkt.tcp import TCP +from dpkt.arp import ARP +from dpkt.ip import IP + +from matplotlib import pyplot as plt + +Y_ARP = 0 +Y_TCP1 = 2 +Y_TCP2 = 3 +Y_TCP3 = 4 +Y_TCP4 = 5 +Y_UDP = 7 + +parser = argparse.ArgumentParser( + description="Replay a PCAP file containing UDP packets while rewriting the target IP address" +) +parser.add_argument("input", help="The PCAP file to read from. Supported formats: .pcap, .pcapng") + +args = parser.parse_args() + +file_type = os.path.splitext(args.input)[1] + +t0 = None + +packets = { + Y_ARP: [], + Y_TCP1: [], + Y_TCP2: [], + Y_TCP3: [], + Y_TCP4: [], + Y_UDP: [], +} + +tcp_streams = {} + +with open(args.input, "rb") as f: + if file_type == ".pcap": + pcap = dpkt.pcap.Reader(f) + elif file_type == ".pcapng": + pcap = dpkt.pcapng.Reader(f) + else: + print(f"Unknown file type: {file_type}. Expected .pcap or .pcapng.") + exit(1) + + for i, (timestamp_s, buf) in enumerate(pcap): + # This automatically discards everything that is not a UDP packet + try: + eth = dpkt.ethernet.Ethernet(buf) + + if t0 is None: + t0 = timestamp_s + + t = timestamp_s - t0 + + if type(eth.data) == ARP: + packets[Y_ARP].append(t) + continue + + ip: IP = eth.data + + if type(ip.data) == UDP: + packets[Y_UDP].append(t) + continue + + if type(ip.data) != TCP: + print(f"Skipped {type(ip)}") + continue + + tcp: TCP = ip.data + stream_tup = (ip.src, ip.dst, tcp.sport, tcp.dport) + if stream_tup not in tcp_streams: + tcp_streams[stream_tup] = len(tcp_streams) + + stream_id = tcp_streams[stream_tup] + packets[Y_TCP1 + stream_id].append(t) + except AttributeError: + continue + +fig, ax = plt.subplots() +ax: plt.Axes +for y in [Y_ARP, Y_TCP1, Y_TCP2, Y_TCP3, Y_TCP4, Y_UDP]: + ax.scatter(packets[y], [y] * len(packets[y])) + +plt.show() \ No newline at end of file From 6428c137da2e548e41d1d6463612bbba161a4382 Mon Sep 17 00:00:00 2001 From: David Wong Date: Mon, 11 Mar 2024 13:45:23 +0900 Subject: [PATCH 14/31] feat: add tsn settings for hesai (AT128 and OT128) Signed-off-by: David Wong --- .../nebula_common/hesai/hesai_common.hpp | 4 +- .../include/nebula_common/nebula_common.hpp | 41 +++++++++++++++++++ .../hesai_hw_interface.hpp | 9 ++-- .../hesai_hw_interface.cpp | 34 ++++++++------- nebula_ros/launch/hesai_launch_all_hw.xml | 2 + .../launch/hesai_launch_component.launch.xml | 2 + nebula_ros/launch/nebula_launch.py | 3 ++ .../hesai/hesai_hw_interface_ros_wrapper.cpp | 15 +++++++ 8 files changed, 91 insertions(+), 19 deletions(-) diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index ad1dcdf57..154034111 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_common.hpp +++ b/nebula_common/include/nebula_common/hesai/hesai_common.hpp @@ -25,6 +25,7 @@ struct HesaiSensorConfiguration : SensorConfigurationBase PtpProfile ptp_profile; uint8_t ptp_domain; PtpTransportType ptp_transport_type; + PtpSwitchType ptp_switch_type; }; /// @brief Convert HesaiSensorConfiguration to string (Overloading the << operator) /// @param os @@ -37,7 +38,8 @@ inline std::ostream & operator<<(std::ostream & os, HesaiSensorConfiguration con << ", FOV(Start):" << arg.cloud_min_angle << ", FOV(End):" << arg.cloud_max_angle << ", DualReturnDistanceThreshold:" << arg.dual_return_distance_threshold << ", PtpProfile:" << arg.ptp_profile << ", PtpDomain:" << std::to_string(arg.ptp_domain) - << ", PtpTransportType:" << arg.ptp_transport_type; + << ", PtpTransportType:" << arg.ptp_transport_type + << ", PtpSwitchType:" << arg.ptp_switch_type; return os; } diff --git a/nebula_common/include/nebula_common/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp index 70466c141..563f166ab 100644 --- a/nebula_common/include/nebula_common/nebula_common.hpp +++ b/nebula_common/include/nebula_common/nebula_common.hpp @@ -354,6 +354,12 @@ enum class PtpTransportType { UNKNOWN_TRANSPORT }; +enum class PtpSwitchType { + NON_TSN = 0, + TSN, + UNKNOWN_SWITCH +}; + /// @brief not used? struct PointField { @@ -643,6 +649,41 @@ inline std::ostream & operator<<(std::ostream & os, nebula::drivers::PtpTranspor return os; } +/// @brief Converts String to PTP SwitchType +/// @param switch_type Switch as String +/// @return Corresponding PtpSwitchType +inline PtpSwitchType PtpSwitchTypeFromString(const std::string & switch_type) +{ + // Hesai + auto tmp_str = switch_type; + std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), + [](unsigned char c){ return std::tolower(c); }); + if (tmp_str == "tsn") return PtpSwitchType::TSN; + if (tmp_str == "non_tsn") return PtpSwitchType::NON_TSN; + + return PtpSwitchType::UNKNOWN_SWITCH; +} + +/// @brief Convert PtpSwitchType enum to string (Overloading the << operator) +/// @param os +/// @param arg +/// @return stream +inline std::ostream & operator<<(std::ostream & os, nebula::drivers::PtpSwitchType const & arg) +{ + switch (arg) { + case PtpSwitchType::TSN: + os << "TSN"; + break; + case PtpSwitchType::NON_TSN: + os << "NON_TSN"; + break; + case PtpSwitchType::UNKNOWN_SWITCH: + os << "UNKNOWN"; + break; + } + return os; +} + [[maybe_unused]] pcl::PointCloud::Ptr convertPointXYZIRADTToPointXYZIR( const pcl::PointCloud::ConstPtr & input_pointcloud); diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp index c9c54896a..1413db869 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 @@ -776,6 +776,7 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @param profile IEEE timing and synchronization standard /// @param domain Domain attribute of the local clock /// @param network Network transport type of 1588v2 + /// @param switch_type Switch type of 802.1AS Automotive /// @param logAnnounceInterval Time interval between Announce messages, in units of log seconds /// (default: 1) /// @param logSyncInterval Time interval between Sync messages, in units of log seconds (default: @@ -786,13 +787,14 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @return Resulting status Status SetPtpConfig( std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int profile, int domain, - int network, int logAnnounceInterval, int logSyncInterval, int logMinDelayReqInterval, + int network, int switch_type, int logAnnounceInterval, int logSyncInterval, int logMinDelayReqInterval, bool with_run = true); /// @brief Setting values with PTC_COMMAND_SET_PTP_CONFIG /// @param ctx IO Context used /// @param profile IEEE timing and synchronization standard /// @param domain Domain attribute of the local clock /// @param network Network transport type of 1588v2 + /// @param switch_type Switch type of 802.1AS Automotive /// @param logAnnounceInterval Time interval between Announce messages, in units of log seconds /// (default: 1) /// @param logSyncInterval Time interval between Sync messages, in units of log seconds (default: @@ -802,12 +804,13 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status Status SetPtpConfig( - std::shared_ptr ctx, int profile, int domain, int network, + std::shared_ptr ctx, int profile, int domain, int network, int switch_type, int logAnnounceInterval, int logSyncInterval, int logMinDelayReqInterval, bool with_run = true); /// @brief Setting values with PTC_COMMAND_SET_PTP_CONFIG /// @param profile IEEE timing and synchronization standard /// @param domain Domain attribute of the local clock /// @param network Network transport type of 1588v2 + /// @param switch_type Switch type of 802.1AS Automotive /// @param logAnnounceInterval Time interval between Announce messages, in units of log seconds /// (default: 1) /// @param logSyncInterval Time interval between Sync messages, in units of log seconds (default: @@ -817,7 +820,7 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status Status SetPtpConfig( - int profile, int domain, int network, int logAnnounceInterval, int logSyncInterval, + int profile, int domain, int network, int switch_type, int logAnnounceInterval, int logSyncInterval, int logMinDelayReqInterval, bool with_run = true); /// @brief Getting data with PTC_COMMAND_GET_PTP_CONFIG /// @param target_tcp_driver TcpDriver used 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 a5589f4e7..71ec7c4c3 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 @@ -2316,8 +2316,8 @@ Status HesaiHwInterface::SetClockSource(int clock_source, bool with_run) Status HesaiHwInterface::SetPtpConfig( std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int profile, int domain, - int network, int logAnnounceInterval = 1, int logSyncInterval = 1, int logMinDelayReqInterval = 0, - bool with_run) + int network, int switch_type, int logAnnounceInterval = 1, int logSyncInterval = 1, + int logMinDelayReqInterval = 0, bool with_run) { std::vector buf_vec; int len = 6; @@ -2344,11 +2344,14 @@ Status HesaiHwInterface::SetPtpConfig( buf_vec.emplace_back((logSyncInterval >> 0) & 0xff); buf_vec.emplace_back((logMinDelayReqInterval >> 0) & 0xff); } + if (profile == 3) { + buf_vec.emplace_back((switch_type >> 0) & 0xff); + } if (!CheckLock(tms_, tms_fail_cnt, tms_fail_cnt_max, "SetPtpConfig")) { return SetPtpConfig( - target_tcp_driver, profile, domain, network, logAnnounceInterval, logSyncInterval, - logMinDelayReqInterval, with_run); + target_tcp_driver, profile, domain, network, switch_type, logAnnounceInterval, + logSyncInterval, logMinDelayReqInterval, with_run); } PrintDebug("SetPtpConfig: start"); @@ -2367,20 +2370,20 @@ Status HesaiHwInterface::SetPtpConfig( } Status HesaiHwInterface::SetPtpConfig( std::shared_ptr ctx, int profile, int domain, int network, - int logAnnounceInterval = 1, int logSyncInterval = 1, int logMinDelayReqInterval = 0, - bool with_run) + int switch_type, int logAnnounceInterval = 1, int logSyncInterval = 1, + int logMinDelayReqInterval = 0, bool with_run) { auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); tcp_driver_local->init_socket( sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, PandarTcpCommandPort); return SetPtpConfig( - tcp_driver_local, profile, domain, network, logAnnounceInterval, logSyncInterval, - logMinDelayReqInterval, with_run); + tcp_driver_local, profile, domain, network, switch_type, logAnnounceInterval, + logSyncInterval, logMinDelayReqInterval, with_run); } Status HesaiHwInterface::SetPtpConfig( - int profile, int domain, int network, int logAnnounceInterval, int logSyncInterval, - int logMinDelayReqInterval, bool with_run) + int profile, int domain, int network, int switch_type, int logAnnounceInterval, + int logSyncInterval, int logMinDelayReqInterval, bool with_run) { if (with_run) { if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { @@ -2388,8 +2391,8 @@ Status HesaiHwInterface::SetPtpConfig( } } return SetPtpConfig( - tcp_driver_s_, profile, domain, network, logAnnounceInterval, logSyncInterval, - logMinDelayReqInterval, with_run); + tcp_driver_s_, profile, domain, network, switch_type, logAnnounceInterval, + logSyncInterval, logMinDelayReqInterval, with_run); } Status HesaiHwInterface::GetPtpConfig( @@ -2984,8 +2987,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( std::this_thread::sleep_for(wait_time); } - if (sensor_configuration->sensor_model != SensorModel::HESAI_PANDARAT128 - && sensor_configuration->sensor_model != SensorModel::HESAI_PANDARQT128) { + if (sensor_configuration->sensor_model != SensorModel::HESAI_PANDARAT128) { set_flg = true; auto sync_angle = static_cast(hesai_config.sync_angle / 100); auto scan_phase = static_cast(sensor_configuration->scan_phase); @@ -3016,11 +3018,13 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( 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 << " via TCP"; + << ", Transport: " << sensor_configuration->ptp_transport_type + << ", Switch Type: " << sensor_configuration->ptp_switch_type << " via TCP"; PrintInfo(tmp_ostringstream.str()); SetPtpConfig(static_cast(sensor_configuration->ptp_profile), sensor_configuration->ptp_domain, static_cast(sensor_configuration->ptp_transport_type), + static_cast(sensor_configuration->ptp_switch_type), PTP_LOG_ANNOUNCE_INTERVAL, PTP_SYNC_INTERVAL, PTP_LOG_MIN_DELAY_INTERVAL diff --git a/nebula_ros/launch/hesai_launch_all_hw.xml b/nebula_ros/launch/hesai_launch_all_hw.xml index 204d08c87..b26193414 100644 --- a/nebula_ros/launch/hesai_launch_all_hw.xml +++ b/nebula_ros/launch/hesai_launch_all_hw.xml @@ -26,6 +26,7 @@ + @@ -68,6 +69,7 @@ + + @@ -80,6 +81,7 @@ + diff --git a/nebula_ros/launch/nebula_launch.py b/nebula_ros/launch/nebula_launch.py index f22a9d6c3..e29dbb4f0 100644 --- a/nebula_ros/launch/nebula_launch.py +++ b/nebula_ros/launch/nebula_launch.py @@ -72,6 +72,7 @@ def launch_setup(context, *args, **kwargs): "ptp_profile": LaunchConfiguration("ptp_profile"), "ptp_domain": LaunchConfiguration("ptp_domain"), "ptp_transport_type": LaunchConfiguration("ptp_transport_type"), + "ptp_switch_type": LaunchConfiguration("ptp_switch_type"), }, ], ), @@ -111,6 +112,7 @@ def launch_setup(context, *args, **kwargs): "ptp_profile": LaunchConfiguration("ptp_profile"), "ptp_domain": LaunchConfiguration("ptp_domain"), "ptp_transport_type": LaunchConfiguration("ptp_transport_type"), + "ptp_switch_type": LaunchConfiguration("ptp_switch_type"), }, ], ), @@ -166,6 +168,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("ptp_profile", "1588v2"), add_launch_arg("ptp_domain", "0"), add_launch_arg("ptp_transport_type", "UDP"), + add_launch_arg("ptp_switch_type", "TSN"), ] + [OpaqueFunction(function=launch_setup)] ) diff --git a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp index df78c6a34..8a6060d21 100644 --- a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp @@ -369,6 +369,16 @@ Status HesaiHwInterfaceRosWrapper::GetParameters( sensor_configuration.ptp_transport_type = nebula::drivers::PtpTransportType::L2; } } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("ptp_switch_type", ""); + sensor_configuration.ptp_switch_type = + nebula::drivers::PtpSwitchTypeFromString(this->get_parameter("ptp_switch_type").as_string()); + } { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; @@ -390,6 +400,11 @@ Status HesaiHwInterfaceRosWrapper::GetParameters( "Invalid PTP Transport Provided. Please use 'udp' or 'l2', 'udp' is only available when using the '1588v2' PTP Profile"); return Status::SENSOR_CONFIG_ERROR; } + if(sensor_configuration.ptp_switch_type == nebula::drivers::PtpSwitchType::UNKNOWN_SWITCH) { + RCLCPP_ERROR_STREAM(get_logger(), + "Invalid PTP Switch Type Provided. Please use 'tsn' or 'non_tsn'"); + return Status::SENSOR_CONFIG_ERROR; + } if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { return Status::INVALID_SENSOR_MODEL; } From 0b13f5756f41ed061e69d0da4e4eda9781d97b36 Mon Sep 17 00:00:00 2001 From: Maximilian Schmeller Date: Mon, 11 Mar 2024 16:07:40 +0900 Subject: [PATCH 15/31] fix(hesai_hw_interface): remove obsolete setup_sensor mutex --- .../hesai_hw_interface.hpp | 4 +- .../hesai_hw_interface.cpp | 52 +++++++++---------- 2 files changed, 27 insertions(+), 29 deletions(-) diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp index c885e0327..07e25493f 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 @@ -108,9 +108,7 @@ class HesaiHwInterface : NebulaHwInterfaceBase std::timed_mutex tm_; int tm_fail_cnt = 0; int tm_fail_cnt_max = 0; - std::timed_mutex tms_; - int tms_fail_cnt = 0; - int tms_fail_cnt_max = 3; + int tm_fail_cnt_max_sensor_setup = 3; bool wl = false; bool is_solid_state = false; int target_model_no; 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 0ab5840d6..b0c550f14 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 @@ -1573,12 +1573,12 @@ Status HesaiHwInterface::SetSpinRate( buf_vec.emplace_back((rpm >> 8) & 0xff); buf_vec.emplace_back((rpm >> 0) & 0xff); - if (!CheckLock(tms_, tms_fail_cnt, tms_fail_cnt_max, "SetSpinRate")) { + if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max_sensor_setup, "SetSpinRate")) { return SetSpinRate(target_tcp_driver, rpm, with_run); } PrintDebug("SetSpinRate: start"); - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tms_, "SetSpinRate"); }); + target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tm_, "SetSpinRate"); }); if (with_run) { boost::system::error_code ec = target_tcp_driver->run(); if (ec) { @@ -1629,12 +1629,12 @@ Status HesaiHwInterface::SetSyncAngle( buf_vec.emplace_back((angle >> 8) & 0xff); buf_vec.emplace_back((angle >> 0) & 0xff); - if (!CheckLock(tms_, tms_fail_cnt, tms_fail_cnt_max, "SetSyncAngle")) { + if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max_sensor_setup, "SetSyncAngle")) { return SetSyncAngle(target_tcp_driver, sync_angle, angle, with_run); } PrintDebug("SetSyncAngle: start"); - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tms_, "SetSyncAngle"); }); + target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tm_, "SetSyncAngle"); }); if (with_run) { boost::system::error_code ec = target_tcp_driver->run(); if (ec) { @@ -1683,12 +1683,12 @@ Status HesaiHwInterface::SetTriggerMethod( buf_vec.emplace_back((trigger_method >> 0) & 0xff); - if (!CheckLock(tms_, tms_fail_cnt, tms_fail_cnt_max, "SetTriggerMethod")) { + if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max_sensor_setup, "SetTriggerMethod")) { return SetTriggerMethod(target_tcp_driver, trigger_method, with_run); } PrintDebug("SetTriggerMethod: start"); - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tms_, "SetTriggerMethod"); }); + target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tm_, "SetTriggerMethod"); }); if (with_run) { boost::system::error_code ec = target_tcp_driver->run(); if (ec) { @@ -1736,12 +1736,12 @@ Status HesaiHwInterface::SetStandbyMode( buf_vec.emplace_back((len >> 0) & 0xff); buf_vec.emplace_back((standby_mode >> 0) & 0xff); - if (!CheckLock(tms_, tms_fail_cnt, tms_fail_cnt_max, "SetStandbyMode")) { + if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max_sensor_setup, "SetStandbyMode")) { return SetStandbyMode(target_tcp_driver, standby_mode, with_run); } std::cout << "start: SetStandbyMode" << std::endl; - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tms_, "SetStandbyMode"); }); + target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tm_, "SetStandbyMode"); }); if (with_run) { boost::system::error_code ec = target_tcp_driver->run(); if (ec) { @@ -1790,11 +1790,11 @@ Status HesaiHwInterface::SetReturnMode( buf_vec.emplace_back((return_mode >> 0) & 0xff); PrintDebug("SetReturnMode: start" + std::to_string(return_mode)); - if (!CheckLock(tms_, tms_fail_cnt, tms_fail_cnt_max, "SetReturnMode")) { + if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max_sensor_setup, "SetReturnMode")) { return SetReturnMode(target_tcp_driver, return_mode, with_run); } PrintDebug("SetReturnMode: asyncSend"); - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tms_, "SetReturnMode"); }); + target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tm_, "SetReturnMode"); }); if (with_run) { boost::system::error_code ec = target_tcp_driver->run(); if (ec) { @@ -1852,13 +1852,13 @@ Status HesaiHwInterface::SetDestinationIp( buf_vec.emplace_back((gps_port >> 8) & 0xff); buf_vec.emplace_back((gps_port >> 0) & 0xff); - if (!CheckLock(tms_, tms_fail_cnt, tms_fail_cnt_max, "SetDestinationIp")) { + if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max_sensor_setup, "SetDestinationIp")) { return SetDestinationIp( target_tcp_driver, dest_ip_1, dest_ip_2, dest_ip_3, dest_ip_4, port, gps_port, with_run); } PrintDebug("SetDestinationIp: start"); - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tms_, "SetDestinationIp"); }); + target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tm_, "SetDestinationIp"); }); if (with_run) { boost::system::error_code ec = target_tcp_driver->run(); if (ec) { @@ -1926,14 +1926,14 @@ Status HesaiHwInterface::SetControlPort( buf_vec.emplace_back((vlan_id >> 8) & 0xff); buf_vec.emplace_back((vlan_id >> 0) & 0xff); - if (!CheckLock(tms_, tms_fail_cnt, tms_fail_cnt_max, "SetControlPort")) { + if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max_sensor_setup, "SetControlPort")) { return SetControlPort( target_tcp_driver, ip_1, ip_2, ip_3, ip_4, mask_1, mask_2, mask_3, mask_4, gateway_1, gateway_2, gateway_3, gateway_4, vlan_flg, vlan_id, with_run); } PrintDebug("SetControlPort: start"); - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tms_, "SetControlPort"); }); + target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tm_, "SetControlPort"); }); if (with_run) { boost::system::error_code ec = target_tcp_driver->run(); if (ec) { @@ -1997,12 +1997,12 @@ Status HesaiHwInterface::SetLidarRange( buf_vec.emplace_back(d); } - if (!CheckLock(tms_, tms_fail_cnt, tms_fail_cnt_max, "SetLidarRange")) { + if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max_sensor_setup, "SetLidarRange")) { return SetLidarRange(target_tcp_driver, method, data, with_run); } PrintDebug("SetLidarRange: start"); - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tms_, "SetLidarRange"); }); + target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tm_, "SetLidarRange"); }); if (with_run) { boost::system::error_code ec = target_tcp_driver->run(); if (ec) { @@ -2060,12 +2060,12 @@ Status HesaiHwInterface::SetLidarRange( buf_vec.emplace_back((end >> 8) & 0xff); buf_vec.emplace_back((end >> 0) & 0xff); - if (!CheckLock(tms_, tms_fail_cnt, tms_fail_cnt_max, "SetLidarRange(All)")) { + if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max_sensor_setup, "SetLidarRange(All)")) { return SetLidarRange(target_tcp_driver, start, end, with_run); } PrintDebug("SetLidarRange(All): start"); - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tms_, "SetLidarRange(All)"); }); + target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tm_, "SetLidarRange(All)"); }); if (with_run) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << "start ctx->run(): SetLidarRange(All)" << std::endl; @@ -2240,12 +2240,12 @@ Status HesaiHwInterface::SetClockSource( buf_vec.emplace_back((clock_source >> 0) & 0xff); - if (!CheckLock(tms_, tms_fail_cnt, tms_fail_cnt_max, "SetClockSource")) { + if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max_sensor_setup, "SetClockSource")) { return SetClockSource(target_tcp_driver, clock_source, with_run); } PrintDebug("SetClockSource: start"); - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tms_, "SetClockSource"); }); + target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tm_, "SetClockSource"); }); if (with_run) { boost::system::error_code ec = target_tcp_driver->run(); if (ec) { @@ -2313,14 +2313,14 @@ Status HesaiHwInterface::SetPtpConfig( buf_vec.emplace_back((switch_type >> 0) & 0xff); } - if (!CheckLock(tms_, tms_fail_cnt, tms_fail_cnt_max, "SetPtpConfig")) { + if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max_sensor_setup, "SetPtpConfig")) { return SetPtpConfig( target_tcp_driver, profile, domain, network, switch_type, logAnnounceInterval, logSyncInterval, logMinDelayReqInterval, with_run); } PrintDebug("SetPtpConfig: start"); - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tms_, "SetPtpConfig"); }); + target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tm_, "SetPtpConfig"); }); if (with_run) { boost::system::error_code ec = target_tcp_driver->run(); if (ec) { @@ -2474,12 +2474,12 @@ Status HesaiHwInterface::SendReset( buf_vec.emplace_back((len >> 8) & 0xff); buf_vec.emplace_back((len >> 0) & 0xff); - if (!CheckLock(tms_, tms_fail_cnt, tms_fail_cnt_max, "SendReset")) { + if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max_sensor_setup, "SendReset")) { return SendReset(target_tcp_driver, with_run); } PrintDebug("SendReset: start"); - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tms_, "SendReset"); }); + target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tm_, "SendReset"); }); if (with_run) { boost::system::error_code ec = target_tcp_driver->run(); if (ec) { @@ -2528,12 +2528,12 @@ Status HesaiHwInterface::SetRotDir( buf_vec.emplace_back((mode >> 0) & 0xff); - if (!CheckLock(tms_, tms_fail_cnt, tms_fail_cnt_max, "SetRotDir")) { + if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max_sensor_setup, "SetRotDir")) { return SetRotDir(target_tcp_driver, mode, with_run); } PrintDebug("SetRotDir: start"); - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tms_, "SetRotDir"); }); + target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tm_, "SetRotDir"); }); if (with_run) { boost::system::error_code ec = target_tcp_driver->run(); if (ec) { From 4301e4d2a1c0aa36fde1bb1e367623779d59d8c9 Mon Sep 17 00:00:00 2001 From: Maximilian Schmeller Date: Mon, 11 Mar 2024 16:08:29 +0900 Subject: [PATCH 16/31] fix(hesai_hw_interface): validate and rewrite OT128's PTP mode in PTC request --- .../src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) 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 b0c550f14..652d35c82 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 @@ -2284,6 +2284,14 @@ Status HesaiHwInterface::SetPtpConfig( int network, int switch_type, int logAnnounceInterval = 1, int logSyncInterval = 1, int logMinDelayReqInterval = 0, bool with_run) { + if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR128_E4X) { + if (profile != static_cast(PtpProfile::IEEE_802_1AS_AUTO)) { + return Status::SENSOR_CONFIG_ERROR; + } + + profile = 3; // OT128 expects 0x03 as its profile; the other sensors define 802.1AS automotive as 0x02 + } + std::vector buf_vec; int len = 6; if (profile == 0) { From 62df70b92af27e86d9630df4afa6a0f5b96e1a00 Mon Sep 17 00:00:00 2001 From: Maximilian Schmeller Date: Mon, 11 Mar 2024 16:09:27 +0900 Subject: [PATCH 17/31] refactor(hesai_hw_interface): remove local_tcp_driver-versions of functions --- .../hesai_hw_interface.cpp | 270 ++---------------- 1 file changed, 26 insertions(+), 244 deletions(-) diff --git a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp index 652d35c82..9f8fe7321 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 @@ -340,18 +340,7 @@ Status HesaiHwInterface::syncGetLidarCalibration( PrintInfo(calib_string); }); } -Status HesaiHwInterface::syncGetLidarCalibration( - std::shared_ptr ctx, - std::function str_callback) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - return syncGetLidarCalibration(tcp_driver_local, str_callback); -} -Status HesaiHwInterface::syncGetLidarCalibration(std::shared_ptr ctx) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - return syncGetLidarCalibration(tcp_driver_local); -} + Status HesaiHwInterface::syncGetLidarCalibrationFromSensor( std::function & received_bytes)> bytes_callback) { @@ -457,19 +446,7 @@ Status HesaiHwInterface::GetLidarCalibration( }, with_run); } -Status HesaiHwInterface::GetLidarCalibration( - std::shared_ptr ctx, - std::function str_callback, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - return GetLidarCalibration(tcp_driver_local, str_callback, with_run); -} -Status HesaiHwInterface::GetLidarCalibration( - std::shared_ptr ctx, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - return GetLidarCalibration(tcp_driver_local, with_run); -} + Status HesaiHwInterface::GetLidarCalibrationFromSensor( std::function & received_bytes)> bytes_callback, bool with_run) { @@ -606,12 +583,7 @@ Status HesaiHwInterface::GetPtpDiagStatus( return Status::WAITING_FOR_SENSOR_RESPONSE; } -Status HesaiHwInterface::GetPtpDiagStatus( - std::shared_ptr ctx, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - return GetPtpDiagStatus(tcp_driver_local, with_run); -} + Status HesaiHwInterface::GetPtpDiagStatus(bool with_run) { if (with_run) { @@ -732,11 +704,7 @@ Status HesaiHwInterface::GetPtpDiagPort( return Status::WAITING_FOR_SENSOR_RESPONSE; } -Status HesaiHwInterface::GetPtpDiagPort(std::shared_ptr ctx, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - return GetPtpDiagPort(tcp_driver_local, with_run); -} + Status HesaiHwInterface::GetPtpDiagPort(bool with_run) { if (with_run) { @@ -893,11 +861,7 @@ Status HesaiHwInterface::GetPtpDiagTime( return Status::WAITING_FOR_SENSOR_RESPONSE; } -Status HesaiHwInterface::GetPtpDiagTime(std::shared_ptr ctx, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - return GetPtpDiagTime(tcp_driver_local, with_run); -} + Status HesaiHwInterface::GetPtpDiagTime(bool with_run) { if (with_run) { @@ -994,12 +958,7 @@ Status HesaiHwInterface::GetPtpDiagGrandmaster( return Status::WAITING_FOR_SENSOR_RESPONSE; } -Status HesaiHwInterface::GetPtpDiagGrandmaster( - std::shared_ptr ctx, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - return GetPtpDiagGrandmaster(tcp_driver_local, with_run); -} + Status HesaiHwInterface::GetPtpDiagGrandmaster(bool with_run) { if (with_run) { @@ -1119,18 +1078,7 @@ Status HesaiHwInterface::syncGetInventory( return syncGetInventory(target_tcp_driver, [this](HesaiInventory & result) { std::cout << result << std::endl; }); } -Status HesaiHwInterface::syncGetInventory( - std::shared_ptr ctx, - std::function callback) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - return syncGetInventory(tcp_driver_local, callback); -} -Status HesaiHwInterface::syncGetInventory(std::shared_ptr ctx) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - return syncGetInventory(tcp_driver_local); -} + Status HesaiHwInterface::syncGetInventory(std::function callback) { return syncGetInventory( @@ -1240,16 +1188,7 @@ Status HesaiHwInterface::GetInventory( return Status::WAITING_FOR_SENSOR_RESPONSE; } -Status HesaiHwInterface::GetInventory( - std::shared_ptr ctx, - std::function callback, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return GetInventory(tcp_driver_local, callback, with_run); -} + Status HesaiHwInterface::GetInventory(std::shared_ptr ctx, bool with_run) { return GetInventory( @@ -1390,16 +1329,7 @@ Status HesaiHwInterface::GetConfig( return Status::WAITING_FOR_SENSOR_RESPONSE; } -Status HesaiHwInterface::GetConfig( - std::shared_ptr ctx, std::function callback, - bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return GetConfig(tcp_driver_local, callback, with_run); -} + Status HesaiHwInterface::GetConfig(std::shared_ptr ctx, bool with_run) { return GetConfig( @@ -1521,16 +1451,7 @@ Status HesaiHwInterface::GetLidarStatus( return Status::WAITING_FOR_SENSOR_RESPONSE; } -Status HesaiHwInterface::GetLidarStatus( - std::shared_ptr ctx, - std::function callback, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return GetLidarStatus(tcp_driver_local, callback, with_run); -} + Status HesaiHwInterface::GetLidarStatus(std::shared_ptr ctx, bool with_run) { return GetLidarStatus( @@ -1591,15 +1512,7 @@ Status HesaiHwInterface::SetSpinRate( return Status::WAITING_FOR_SENSOR_RESPONSE; } -Status HesaiHwInterface::SetSpinRate( - std::shared_ptr ctx, uint16_t rpm, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return SetSpinRate(tcp_driver_local, rpm, with_run); -} + Status HesaiHwInterface::SetSpinRate(uint16_t rpm, bool with_run) { if (with_run) { @@ -1647,15 +1560,7 @@ Status HesaiHwInterface::SetSyncAngle( return Status::WAITING_FOR_SENSOR_RESPONSE; } -Status HesaiHwInterface::SetSyncAngle( - std::shared_ptr ctx, int sync_angle, int angle, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return SetSyncAngle(tcp_driver_local, sync_angle, angle, with_run); -} + Status HesaiHwInterface::SetSyncAngle(int sync_angle, int angle, bool with_run) { if (with_run) { @@ -1701,15 +1606,7 @@ Status HesaiHwInterface::SetTriggerMethod( return Status::WAITING_FOR_SENSOR_RESPONSE; } -Status HesaiHwInterface::SetTriggerMethod( - std::shared_ptr ctx, int trigger_method, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return SetTriggerMethod(tcp_driver_local, trigger_method, with_run); -} + Status HesaiHwInterface::SetTriggerMethod(int trigger_method, bool with_run) { if (with_run) { @@ -1754,15 +1651,7 @@ Status HesaiHwInterface::SetStandbyMode( return Status::WAITING_FOR_SENSOR_RESPONSE; } -Status HesaiHwInterface::SetStandbyMode( - std::shared_ptr ctx, int standby_mode, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return SetStandbyMode(tcp_driver_local, standby_mode, with_run); -} + Status HesaiHwInterface::SetStandbyMode(int standby_mode, bool with_run) { if (with_run) { @@ -1807,15 +1696,7 @@ Status HesaiHwInterface::SetReturnMode( PrintDebug("SetReturnMode: done"); return Status::WAITING_FOR_SENSOR_RESPONSE; } -Status HesaiHwInterface::SetReturnMode( - std::shared_ptr ctx, int return_mode, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return SetReturnMode(tcp_driver_local, return_mode, with_run); -} + Status HesaiHwInterface::SetReturnMode(int return_mode, bool with_run) { //* @@ -1871,17 +1752,7 @@ Status HesaiHwInterface::SetDestinationIp( return Status::WAITING_FOR_SENSOR_RESPONSE; } -Status HesaiHwInterface::SetDestinationIp( - std::shared_ptr ctx, int dest_ip_1, int dest_ip_2, int dest_ip_3, - int dest_ip_4, int port, int gps_port, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return SetDestinationIp( - tcp_driver_local, dest_ip_1, dest_ip_2, dest_ip_3, dest_ip_4, port, gps_port, with_run); -} + Status HesaiHwInterface::SetDestinationIp( int dest_ip_1, int dest_ip_2, int dest_ip_3, int dest_ip_4, int port, int gps_port, bool with_run) { @@ -1946,19 +1817,7 @@ Status HesaiHwInterface::SetControlPort( return Status::WAITING_FOR_SENSOR_RESPONSE; } -Status HesaiHwInterface::SetControlPort( - std::shared_ptr ctx, int ip_1, int ip_2, int ip_3, int ip_4, int mask_1, - int mask_2, int mask_3, int mask_4, int gateway_1, int gateway_2, int gateway_3, int gateway_4, - int vlan_flg, int vlan_id, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return SetControlPort( - tcp_driver_local, ip_1, ip_2, ip_3, ip_4, mask_1, mask_2, mask_3, mask_4, gateway_1, gateway_2, - gateway_3, gateway_4, vlan_flg, vlan_id, with_run); -} + Status HesaiHwInterface::SetControlPort( int ip_1, int ip_2, int ip_3, int ip_4, int mask_1, int mask_2, int mask_3, int mask_4, int gateway_1, int gateway_2, int gateway_3, int gateway_4, int vlan_flg, int vlan_id, @@ -2015,16 +1874,7 @@ Status HesaiHwInterface::SetLidarRange( return Status::WAITING_FOR_SENSOR_RESPONSE; } -Status HesaiHwInterface::SetLidarRange( - std::shared_ptr ctx, int method, std::vector data, - bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return SetLidarRange(tcp_driver_local, method, data, with_run); -} + Status HesaiHwInterface::SetLidarRange(int method, std::vector data, bool with_run) { if (with_run) { @@ -2081,15 +1931,7 @@ Status HesaiHwInterface::SetLidarRange( return Status::WAITING_FOR_SENSOR_RESPONSE; } -Status HesaiHwInterface::SetLidarRange( - std::shared_ptr ctx, int start, int end, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return SetLidarRange(tcp_driver_local, start, end, with_run); -} + Status HesaiHwInterface::SetLidarRange(int start, int end, bool with_run) { if (with_run) { @@ -2192,16 +2034,7 @@ Status HesaiHwInterface::GetLidarRange( return Status::WAITING_FOR_SENSOR_RESPONSE; } -Status HesaiHwInterface::GetLidarRange( - std::shared_ptr ctx, - std::function callback, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return GetLidarRange(tcp_driver_local, callback, with_run); -} + Status HesaiHwInterface::GetLidarRange(std::shared_ptr ctx, bool with_run) { return GetLidarRange( @@ -2258,15 +2091,7 @@ Status HesaiHwInterface::SetClockSource( return Status::WAITING_FOR_SENSOR_RESPONSE; } -Status HesaiHwInterface::SetClockSource( - std::shared_ptr ctx, int clock_source, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return SetClockSource(tcp_driver_local, clock_source, with_run); -} + Status HesaiHwInterface::SetClockSource(int clock_source, bool with_run) { //* @@ -2341,19 +2166,7 @@ Status HesaiHwInterface::SetPtpConfig( return Status::WAITING_FOR_SENSOR_RESPONSE; } -Status HesaiHwInterface::SetPtpConfig( - std::shared_ptr ctx, int profile, int domain, int network, - int switch_type, int logAnnounceInterval = 1, int logSyncInterval = 1, - int logMinDelayReqInterval = 0, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return SetPtpConfig( - tcp_driver_local, profile, domain, network, switch_type, logAnnounceInterval, - logSyncInterval, logMinDelayReqInterval, with_run); -} + Status HesaiHwInterface::SetPtpConfig( int profile, int domain, int network, int switch_type, int logAnnounceInterval, int logSyncInterval, int logMinDelayReqInterval, bool with_run) @@ -2450,14 +2263,7 @@ Status HesaiHwInterface::GetPtpConfig( return Status::WAITING_FOR_SENSOR_RESPONSE; } -Status HesaiHwInterface::GetPtpConfig(std::shared_ptr ctx, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return GetPtpConfig(tcp_driver_local, with_run); -} + Status HesaiHwInterface::GetPtpConfig(bool with_run) { if (with_run) { @@ -2500,14 +2306,7 @@ Status HesaiHwInterface::SendReset( return Status::WAITING_FOR_SENSOR_RESPONSE; } -Status HesaiHwInterface::SendReset(std::shared_ptr ctx, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return SendReset(tcp_driver_local, with_run); -} + Status HesaiHwInterface::SendReset(bool with_run) { //* @@ -2554,15 +2353,7 @@ Status HesaiHwInterface::SetRotDir( return Status::WAITING_FOR_SENSOR_RESPONSE; } -Status HesaiHwInterface::SetRotDir( - std::shared_ptr ctx, int mode, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return SetRotDir(tcp_driver_local, mode, with_run); -} + Status HesaiHwInterface::SetRotDir(int mode, bool with_run) { if (with_run) { @@ -2667,16 +2458,7 @@ Status HesaiHwInterface::GetLidarMonitor( } return Status::WAITING_FOR_SENSOR_RESPONSE; } -Status HesaiHwInterface::GetLidarMonitor( - std::shared_ptr ctx, - std::function callback, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return GetLidarMonitor(tcp_driver_local, callback, with_run); -} + Status HesaiHwInterface::GetLidarMonitor( std::shared_ptr ctx, bool with_run) { From 3287abe0c5928147ee6f6bda8ffecfdf0a51c380 Mon Sep 17 00:00:00 2001 From: David Wong Date: Mon, 11 Mar 2024 17:30:53 +0900 Subject: [PATCH 18/31] fix: special handling for OT128 needing a different definition for PTP profile Signed-off-by: David Wong --- .../src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) 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 71ec7c4c3..87513bc51 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 @@ -2336,6 +2336,12 @@ Status HesaiHwInterface::SetPtpConfig( buf_vec.emplace_back((len >> 8) & 0xff); buf_vec.emplace_back((len >> 0) & 0xff); + if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR128_E4X) { + if (profile != static_cast(PtpProfile::IEEE_802_1AS_AUTO)) { + return Status::SENSOR_CONFIG_ERROR; + } + profile = 3; // OT128 has a different definition of PTP profile + } buf_vec.emplace_back((profile >> 0) & 0xff); buf_vec.emplace_back((domain >> 0) & 0xff); buf_vec.emplace_back((network >> 0) & 0xff); @@ -2344,7 +2350,7 @@ Status HesaiHwInterface::SetPtpConfig( buf_vec.emplace_back((logSyncInterval >> 0) & 0xff); buf_vec.emplace_back((logMinDelayReqInterval >> 0) & 0xff); } - if (profile == 3) { + else if (profile == 3) { buf_vec.emplace_back((switch_type >> 0) & 0xff); } From 9a28e3493b452158792ae83bf171c9a47efdefe3 Mon Sep 17 00:00:00 2001 From: David Wong Date: Mon, 11 Mar 2024 20:41:36 +0900 Subject: [PATCH 19/31] fix: move defaults in ptp config functions to header files Signed-off-by: David Wong --- .../nebula_hw_interfaces_hesai/hesai_hw_interface.hpp | 8 ++++---- .../src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp index 1413db869..01081e320 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 @@ -787,7 +787,7 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @return Resulting status Status SetPtpConfig( std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int profile, int domain, - int network, int switch_type, int logAnnounceInterval, int logSyncInterval, int logMinDelayReqInterval, + int network, int switch_type, int logAnnounceInterval = 1, int logSyncInterval = 1, int logMinDelayReqInterval = 0, bool with_run = true); /// @brief Setting values with PTC_COMMAND_SET_PTP_CONFIG /// @param ctx IO Context used @@ -805,7 +805,7 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @return Resulting status Status SetPtpConfig( std::shared_ptr ctx, int profile, int domain, int network, int switch_type, - int logAnnounceInterval, int logSyncInterval, int logMinDelayReqInterval, bool with_run = true); + int logAnnounceInterval = 1, int logSyncInterval = 1, int logMinDelayReqInterval = 0, bool with_run = true); /// @brief Setting values with PTC_COMMAND_SET_PTP_CONFIG /// @param profile IEEE timing and synchronization standard /// @param domain Domain attribute of the local clock @@ -820,8 +820,8 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status Status SetPtpConfig( - int profile, int domain, int network, int switch_type, int logAnnounceInterval, int logSyncInterval, - int logMinDelayReqInterval, bool with_run = true); + int profile, int domain, int network, int switch_type, int logAnnounceInterval = 1, int logSyncInterval = 1, + int logMinDelayReqInterval = 0, bool with_run = true); /// @brief Getting data with PTC_COMMAND_GET_PTP_CONFIG /// @param target_tcp_driver TcpDriver used /// @param with_run Automatically executes run() of TcpDriver 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 87513bc51..fec622a98 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 @@ -2316,8 +2316,8 @@ Status HesaiHwInterface::SetClockSource(int clock_source, bool with_run) Status HesaiHwInterface::SetPtpConfig( std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int profile, int domain, - int network, int switch_type, int logAnnounceInterval = 1, int logSyncInterval = 1, - int logMinDelayReqInterval = 0, bool with_run) + int network, int switch_type, int logAnnounceInterval, int logSyncInterval, + int logMinDelayReqInterval, bool with_run) { std::vector buf_vec; int len = 6; @@ -2376,8 +2376,8 @@ Status HesaiHwInterface::SetPtpConfig( } Status HesaiHwInterface::SetPtpConfig( std::shared_ptr ctx, int profile, int domain, int network, - int switch_type, int logAnnounceInterval = 1, int logSyncInterval = 1, - int logMinDelayReqInterval = 0, bool with_run) + int switch_type, int logAnnounceInterval, int logSyncInterval, + int logMinDelayReqInterval, bool with_run) { auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); tcp_driver_local->init_socket( From 37d7b0f4a1a6ec2d5bffc2a375489dfaae253830 Mon Sep 17 00:00:00 2001 From: David Wong Date: Mon, 11 Mar 2024 20:56:57 +0900 Subject: [PATCH 20/31] fix: correctly assign header size in PTP config Signed-off-by: David Wong --- .../hesai_hw_interface.cpp | 35 ++++++++++++------- 1 file changed, 22 insertions(+), 13 deletions(-) diff --git a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp index fec622a98..3b52b9b72 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 @@ -2319,14 +2319,30 @@ Status HesaiHwInterface::SetPtpConfig( int network, int switch_type, int logAnnounceInterval, int logSyncInterval, int logMinDelayReqInterval, bool with_run) { - std::vector buf_vec; - int len = 6; - if (profile == 0) { - } else if (profile >= 1) { - len = 3; - } else { + if (profile < 0 || profile > 3) { return Status::ERROR_1; } + // Handle the OT128 differently - it has TSN settings and defines the PTP profile + // for automotove as 0x03 instead of 0x02 for other sensors. + if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR128_E4X) { + if (profile != static_cast(PtpProfile::IEEE_802_1AS_AUTO)) { + return Status::SENSOR_CONFIG_ERROR; + } + profile = 3; + } + + std::vector buf_vec; + int len = 3; + switch (profile) { + case 0: + len = 6; + break; + case 3: + len = 4; + break; + default: + len = 3; + } buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); buf_vec.emplace_back(PTC_COMMAND_SET_PTP_CONFIG); // Cmd PTC_COMMAND_SET_PTP_CONFIG @@ -2335,13 +2351,6 @@ Status HesaiHwInterface::SetPtpConfig( buf_vec.emplace_back((len >> 16) & 0xff); buf_vec.emplace_back((len >> 8) & 0xff); buf_vec.emplace_back((len >> 0) & 0xff); - - if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR128_E4X) { - if (profile != static_cast(PtpProfile::IEEE_802_1AS_AUTO)) { - return Status::SENSOR_CONFIG_ERROR; - } - profile = 3; // OT128 has a different definition of PTP profile - } buf_vec.emplace_back((profile >> 0) & 0xff); buf_vec.emplace_back((domain >> 0) & 0xff); buf_vec.emplace_back((network >> 0) & 0xff); From 05990d1568e2f1d018fef3b595e0c3f4610f1b80 Mon Sep 17 00:00:00 2001 From: David Wong Date: Mon, 11 Mar 2024 21:00:09 +0900 Subject: [PATCH 21/31] fix: spelling miss Signed-off-by: David Wong --- .../src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp index 3b52b9b72..6be12fc22 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 @@ -2323,14 +2323,14 @@ Status HesaiHwInterface::SetPtpConfig( return Status::ERROR_1; } // Handle the OT128 differently - it has TSN settings and defines the PTP profile - // for automotove as 0x03 instead of 0x02 for other sensors. + // for automotive as 0x03 instead of 0x02 for other sensors. if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR128_E4X) { if (profile != static_cast(PtpProfile::IEEE_802_1AS_AUTO)) { return Status::SENSOR_CONFIG_ERROR; } profile = 3; } - + std::vector buf_vec; int len = 3; switch (profile) { From 021a3a51b8ed9ac45a715dbb8b708a6d5be004a2 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 13 Mar 2024 19:00:28 +0900 Subject: [PATCH 22/31] fix: [reword later] somehow this works --- .../hesai_cmd_response.hpp | 14 +- .../hesai_hw_interface.hpp | 49 +- .../hesai_hw_interface.cpp | 690 +++++++----------- .../hesai/hesai_hw_interface_ros_wrapper.cpp | 2 + 4 files changed, 293 insertions(+), 462 deletions(-) diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp index f8d9a4819..21989bf9b 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp @@ -235,40 +235,30 @@ struct HesaiInventory switch (model) { case 0: return "Pandar40P"; - break; case 2: return "Pandar64"; - break; case 3: return "Pandar128"; - break; case 15: return "PandarQT"; - break; case 17: return "Pandar40M"; - break; case 20: return "PandarMind(PM64)"; - break; case 25: return "PandarXT32"; - break; case 26: return "PandarXT16"; - break; case 32: return "QT128C2X"; - break; case 38: return "PandarXT32M"; - break; + case 42: + return "OT128"; case 48: return "PandarAT128"; - break; default: return "Unknown(" + std::to_string(model) + ")"; - break; } } }; diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp index 172dec22e..d15f4a28a 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp @@ -104,12 +104,6 @@ class HesaiHwInterface : NebulaHwInterfaceBase int prev_phase_{}; - int timeout_ = 2000; - std::timed_mutex tm_; - int tm_fail_cnt = 0; - int tm_fail_cnt_max = 0; - int tm_fail_cnt_max_sensor_setup = 3; - bool wl = false; bool is_solid_state = false; int target_model_no; @@ -130,18 +124,6 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @param str Received string void str_cb(const std::string & str); - /// @brief Lock function during TCP communication - /// @param tm Mutex - /// @param fail_cnt # of failures - /// @param fail_cnt_max # of times to accept failure - /// @param name Confirmation name used in PrintDebug - /// @return Locked - bool CheckLock(std::timed_mutex & tm, int & fail_cnt, const int & fail_cnt_max, std::string name); - /// @brief Unlock function during TCP communication - /// @param tm Mutex - /// @param name Confirmation name used in PrintDebug - void CheckUnlock(std::timed_mutex & tm, std::string name); - std::shared_ptr parent_node_logger; /// @brief Printing the string to RCLCPP_INFO_STREAM /// @param info Target string @@ -156,6 +138,11 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @param bytes Target byte vector void PrintDebug(const std::vector & bytes); + /// @brief Send a PTC request with an optional payload, and return the full response payload. Blocking. + /// @param command_id PTC command number. + /// @param payload Payload bytes of the PTC command. Not including the 8-byte PTC header. + /// @return The returned payload, if successful, or nullptr. + std::shared_ptr> SendReceive(const uint8_t command_id, const std::vector & payload = {}); public: /// @brief Constructor HesaiHwInterface(); @@ -422,31 +409,7 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @param callback Callback function for received HesaiConfig /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status GetConfig( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function callback, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_CONFIG_INFO - /// @param ctx IO Context used - /// @param callback Callback function for received HesaiConfig - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetConfig( - std::shared_ptr ctx, - std::function callback, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_CONFIG_INFO - /// @param ctx IO Context used - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetConfig(std::shared_ptr ctx, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_CONFIG_INFO - /// @param callback Callback function for received HesaiConfig - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetConfig(std::function callback, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_CONFIG_INFO - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetConfig(bool with_run = true); + std::shared_ptr GetConfig(); /// @brief Getting data with PTC_COMMAND_GET_LIDAR_STATUS /// @param target_tcp_driver TcpDriver used /// @param callback Callback function for received HesaiLidarStatus 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 21f310894..1b55fd6e9 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 @@ -7,6 +7,8 @@ #include #endif +#include + namespace nebula { namespace drivers @@ -48,6 +50,54 @@ HesaiHwInterface::~HesaiHwInterface() #endif } +std::shared_ptr> HesaiHwInterface::SendReceive(const uint8_t command_id, const std::vector & payload) { + uint32_t len = payload.size(); + + std::vector send_buf; + send_buf.emplace_back(PTC_COMMAND_HEADER_HIGH); + send_buf.emplace_back(PTC_COMMAND_HEADER_LOW); + send_buf.emplace_back(command_id); + send_buf.emplace_back(PTC_COMMAND_DUMMY_BYTE); + send_buf.emplace_back((len >> 24) & 0xff); + send_buf.emplace_back((len >> 16) & 0xff); + send_buf.emplace_back((len >> 8) & 0xff); + send_buf.emplace_back((len >> 0) & 0xff); + send_buf.insert(send_buf.end(), payload.begin(), payload.end()); + + auto recv_buf = std::make_shared>(); + bool success = false; + + std::timed_mutex tm; + tm.lock(); + + if (tcp_driver_->GetIOContext()->stopped()) { + tcp_driver_->GetIOContext()->restart(); + } + + tcp_driver_->asyncSendReceiveHeaderPayload(send_buf, + [](const std::vector & header_bytes){ + + }, + [&recv_buf, &success](const std::vector & payload_bytes){ + recv_buf->insert(recv_buf->end(), payload_bytes.begin(), payload_bytes.end()); + success = true; + }, + [&tm](){ tm.unlock(); } + ); + this->IOContextRun(); + if (!tm.try_lock_for(std::chrono::seconds(1))) { + PrintError("Request did not finish within 1s"); + return nullptr; + } + + if (!success) { + PrintError("Did not receive payload"); + return nullptr; + } + + return recv_buf; +} + Status HesaiHwInterface::SetSensorConfiguration( std::shared_ptr sensor_configuration) { @@ -234,9 +284,9 @@ Status HesaiHwInterface::InitializeTcpDriver(bool setup_sensor) #endif if (!tcp_driver_->open()) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "!tcp_driver_->open()" << std::endl; + std::cout << "!tcp_driver_->open()" << std::endl; #endif -// tcp_driver_->close(); + // tcp_driver_->close(); tcp_driver_->closeSync(); return Status::ERROR_1; } @@ -314,7 +364,7 @@ Status HesaiHwInterface::syncGetLidarCalibration( #endif bytes_callback(received_bytes); }, - [this]() { CheckUnlock(tm_, "GetLidarCalibration"); }); + [this]() { }); return Status::OK; } @@ -407,7 +457,7 @@ Status HesaiHwInterface::GetLidarCalibration( #endif bytes_callback(received_bytes); }, - [this]() { CheckUnlock(tm_, "GetLidarCalibration"); }); + [](){}); if (with_run) { boost::system::error_code ec = target_tcp_driver->run(); if (ec) { @@ -495,9 +545,6 @@ Status HesaiHwInterface::GetPtpDiagStatus( buf_vec.emplace_back(PTC_COMMAND_PTP_STATUS); // PTP STATUS - if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max, "GetPtpDiagStatus")) { - return GetPtpDiagStatus(target_tcp_driver, with_run); - } PrintDebug("GetPtpDiagStatus: start"); target_tcp_driver->asyncSendReceiveHeaderPayload( @@ -569,7 +616,7 @@ Status HesaiHwInterface::GetPtpDiagStatus( PrintInfo(ss.str()); } }, - [this]() { CheckUnlock(tm_, "GetPtpDiagStatus"); }); + [this]() { }); if (with_run) { boost::system::error_code ec = target_tcp_driver->run(); @@ -610,9 +657,6 @@ Status HesaiHwInterface::GetPtpDiagPort( buf_vec.emplace_back(PTC_COMMAND_PTP_PORT_DATA_SET); // PTP TLV PORT_DATA_SET - if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max, "GetPtpDiagPort")) { - return GetPtpDiagPort(target_tcp_driver, with_run); - } PrintDebug("GetPtpDiagPort: start"); target_tcp_driver->asyncSendReceiveHeaderPayload( @@ -690,7 +734,7 @@ Status HesaiHwInterface::GetPtpDiagPort( PrintInfo(ss.str()); } }, - [this]() { CheckUnlock(tm_, "GetPtpDiagPort"); }); + [this]() { }); if (with_run) { boost::system::error_code ec = target_tcp_driver->run(); @@ -730,9 +774,7 @@ Status HesaiHwInterface::GetPtpDiagTime( buf_vec.emplace_back((len >> 0) & 0xff); buf_vec.emplace_back(PTC_COMMAND_PTP_TIME_STATUS_NP); // PTP TLV TIME_STATUS_NP - if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max, "GetPtpDiagTime")) { - return GetPtpDiagTime(target_tcp_driver, with_run); - } + PrintDebug("GetPtpDiagTime: start"); target_tcp_driver->asyncSendReceiveHeaderPayload( @@ -847,7 +889,7 @@ Status HesaiHwInterface::GetPtpDiagTime( PrintInfo(ss.str()); } }, - [this]() { CheckUnlock(tm_, "GetPtpDiagTime"); }); + [this]() { }); if (with_run) { boost::system::error_code ec = target_tcp_driver->run(); @@ -887,9 +929,7 @@ Status HesaiHwInterface::GetPtpDiagGrandmaster( buf_vec.emplace_back((len >> 0) & 0xff); buf_vec.emplace_back(PTC_COMMAND_PTP_GRANDMASTER_SETTINGS_NP); // PTP TLV GRANDMASTER_SETTINGS_NP - if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max, "GetPtpDiagGrandmaster")) { - return GetPtpDiagGrandmaster(target_tcp_driver, with_run); - } + PrintDebug("GetPtpDiagGrandmaster: start"); target_tcp_driver->asyncSendReceiveHeaderPayload( @@ -944,7 +984,7 @@ Status HesaiHwInterface::GetPtpDiagGrandmaster( std::cout << hesai_ptp_diag_grandmaster << std::endl; } }, - [this]() { CheckUnlock(tm_, "GetPtpDiagGrandmaster"); }); + [this]() { }); if (with_run) { boost::system::error_code ec = target_tcp_driver->run(); @@ -985,9 +1025,6 @@ Status HesaiHwInterface::syncGetInventory( buf_vec.emplace_back((len >> 8) & 0xff); buf_vec.emplace_back((len >> 0) & 0xff); - if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max, "GetInventory")) { - return syncGetInventory(target_tcp_driver, callback); - } // It doesn't work even when the sensor is not connected... if(!target_tcp_driver){ @@ -1068,7 +1105,7 @@ Status HesaiHwInterface::syncGetInventory( callback(hesai_inventory); } }, - [this]() { CheckUnlock(tm_, "syncGetInventory"); }); + [this]() { }); return Status::OK; } @@ -1101,10 +1138,6 @@ Status HesaiHwInterface::GetInventory( buf_vec.emplace_back((len >> 16) & 0xff); buf_vec.emplace_back((len >> 8) & 0xff); buf_vec.emplace_back((len >> 0) & 0xff); - - if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max, "GetInventory")) { - return GetInventory(target_tcp_driver, callback, with_run); - } PrintDebug("GetInventory: start"); target_tcp_driver->asyncSendReceiveHeaderPayload( @@ -1174,7 +1207,7 @@ Status HesaiHwInterface::GetInventory( callback(hesai_inventory); } }, - [this]() { CheckUnlock(tm_, "GetInventory"); }); + []() {}); if (with_run) { boost::system::error_code ec = target_tcp_driver->run(); @@ -1210,145 +1243,70 @@ Status HesaiHwInterface::GetInventory(bool with_run) [this](HesaiInventory & result) { std::cout << result << std::endl; }, with_run); } -Status HesaiHwInterface::GetConfig( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function callback, bool with_run) -{ - std::vector buf_vec; - int len = 0; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_GET_CONFIG_INFO); // Cmd PTC_COMMAND_GET_CONFIG_INFO - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max, "GetConfig")) { - return GetConfig(target_tcp_driver, callback, with_run); - } - PrintDebug("GetConfig: start"); - - target_tcp_driver->asyncSendReceiveHeaderPayload( - buf_vec, - [this](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - }, - [this, target_tcp_driver, callback](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - - std::cout << "GetConfig getHeader: "; - for (const auto & b : target_tcp_driver->getHeader()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - std::cout << "GetConfig getPayload: "; - for (const auto & b : target_tcp_driver->getPayload()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - - auto response = target_tcp_driver->getPayload(); - HesaiConfig hesai_config; - if (8 < response.size()) { - int payload_pos = 8; - hesai_config.ipaddr[0] = static_cast(response[payload_pos++]); - hesai_config.ipaddr[1] = static_cast(response[payload_pos++]); - hesai_config.ipaddr[2] = static_cast(response[payload_pos++]); - hesai_config.ipaddr[3] = static_cast(response[payload_pos++]); - hesai_config.mask[0] = static_cast(response[payload_pos++]); - hesai_config.mask[1] = static_cast(response[payload_pos++]); - hesai_config.mask[2] = static_cast(response[payload_pos++]); - hesai_config.mask[3] = static_cast(response[payload_pos++]); - hesai_config.gateway[0] = static_cast(response[payload_pos++]); - hesai_config.gateway[1] = static_cast(response[payload_pos++]); - hesai_config.gateway[2] = static_cast(response[payload_pos++]); - hesai_config.gateway[3] = static_cast(response[payload_pos++]); - hesai_config.dest_ipaddr[0] = static_cast(response[payload_pos++]); - hesai_config.dest_ipaddr[1] = static_cast(response[payload_pos++]); - hesai_config.dest_ipaddr[2] = static_cast(response[payload_pos++]); - hesai_config.dest_ipaddr[3] = static_cast(response[payload_pos++]); - hesai_config.dest_LiDAR_udp_port = response[payload_pos++] << 8; - hesai_config.dest_LiDAR_udp_port = - hesai_config.dest_LiDAR_udp_port | response[payload_pos++]; - hesai_config.dest_gps_udp_port = response[payload_pos++] << 8; - hesai_config.dest_gps_udp_port = hesai_config.dest_gps_udp_port | response[payload_pos++]; - hesai_config.spin_rate = response[payload_pos++] << 8; - hesai_config.spin_rate = hesai_config.spin_rate | response[payload_pos++]; - hesai_config.sync = static_cast(response[payload_pos++]); - hesai_config.sync_angle = response[payload_pos++] << 8; - hesai_config.sync_angle = hesai_config.sync_angle | response[payload_pos++]; - hesai_config.start_angle = response[payload_pos++] << 8; - hesai_config.start_angle = hesai_config.start_angle | response[payload_pos++]; - hesai_config.stop_angle = response[payload_pos++] << 8; - hesai_config.stop_angle = hesai_config.stop_angle | response[payload_pos++]; - hesai_config.clock_source = static_cast(response[payload_pos++]); - hesai_config.udp_seq = static_cast(response[payload_pos++]); - hesai_config.trigger_method = static_cast(response[payload_pos++]); - hesai_config.return_mode = static_cast(response[payload_pos++]); - hesai_config.standby_mode = static_cast(response[payload_pos++]); - hesai_config.motor_status = static_cast(response[payload_pos++]); - hesai_config.vlan_flag = static_cast(response[payload_pos++]); - hesai_config.vlan_id = response[payload_pos++] << 8; - hesai_config.vlan_id = hesai_config.vlan_id | response[payload_pos++]; - hesai_config.clock_data_fmt = static_cast(response[payload_pos++]); - hesai_config.noise_filtering = static_cast(response[payload_pos++]); - hesai_config.reflectivity_mapping = static_cast(response[payload_pos++]); - hesai_config.reserved[0] = static_cast(response[payload_pos++]); - hesai_config.reserved[1] = static_cast(response[payload_pos++]); - hesai_config.reserved[2] = static_cast(response[payload_pos++]); - hesai_config.reserved[3] = static_cast(response[payload_pos++]); - hesai_config.reserved[4] = static_cast(response[payload_pos++]); - hesai_config.reserved[5] = static_cast(response[payload_pos++]); - - callback(hesai_config); - } - }, - [this]() { CheckUnlock(tm_, "GetConfig"); }); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::GetConfig: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): GetConfig" << std::endl; -#endif - } - - return Status::WAITING_FOR_SENSOR_RESPONSE; +std::shared_ptr HesaiHwInterface::GetConfig() +{ + auto hesai_config_ptr = std::make_shared(); + auto & hesai_config = *hesai_config_ptr; + auto response = *SendReceive(PTC_COMMAND_GET_CONFIG_INFO); + + PrintDebug(response); + + if (8 < response.size()) { + int payload_pos = 8; + hesai_config.ipaddr[0] = static_cast(response[payload_pos++]); + hesai_config.ipaddr[1] = static_cast(response[payload_pos++]); + hesai_config.ipaddr[2] = static_cast(response[payload_pos++]); + hesai_config.ipaddr[3] = static_cast(response[payload_pos++]); + hesai_config.mask[0] = static_cast(response[payload_pos++]); + hesai_config.mask[1] = static_cast(response[payload_pos++]); + hesai_config.mask[2] = static_cast(response[payload_pos++]); + hesai_config.mask[3] = static_cast(response[payload_pos++]); + hesai_config.gateway[0] = static_cast(response[payload_pos++]); + hesai_config.gateway[1] = static_cast(response[payload_pos++]); + hesai_config.gateway[2] = static_cast(response[payload_pos++]); + hesai_config.gateway[3] = static_cast(response[payload_pos++]); + hesai_config.dest_ipaddr[0] = static_cast(response[payload_pos++]); + hesai_config.dest_ipaddr[1] = static_cast(response[payload_pos++]); + hesai_config.dest_ipaddr[2] = static_cast(response[payload_pos++]); + hesai_config.dest_ipaddr[3] = static_cast(response[payload_pos++]); + hesai_config.dest_LiDAR_udp_port = response[payload_pos++] << 8; + hesai_config.dest_LiDAR_udp_port = + hesai_config.dest_LiDAR_udp_port | response[payload_pos++]; + hesai_config.dest_gps_udp_port = response[payload_pos++] << 8; + hesai_config.dest_gps_udp_port = hesai_config.dest_gps_udp_port | response[payload_pos++]; + hesai_config.spin_rate = response[payload_pos++] << 8; + hesai_config.spin_rate = hesai_config.spin_rate | response[payload_pos++]; + hesai_config.sync = static_cast(response[payload_pos++]); + hesai_config.sync_angle = response[payload_pos++] << 8; + hesai_config.sync_angle = hesai_config.sync_angle | response[payload_pos++]; + hesai_config.start_angle = response[payload_pos++] << 8; + hesai_config.start_angle = hesai_config.start_angle | response[payload_pos++]; + hesai_config.stop_angle = response[payload_pos++] << 8; + hesai_config.stop_angle = hesai_config.stop_angle | response[payload_pos++]; + hesai_config.clock_source = static_cast(response[payload_pos++]); + hesai_config.udp_seq = static_cast(response[payload_pos++]); + hesai_config.trigger_method = static_cast(response[payload_pos++]); + hesai_config.return_mode = static_cast(response[payload_pos++]); + hesai_config.standby_mode = static_cast(response[payload_pos++]); + hesai_config.motor_status = static_cast(response[payload_pos++]); + hesai_config.vlan_flag = static_cast(response[payload_pos++]); + hesai_config.vlan_id = response[payload_pos++] << 8; + hesai_config.vlan_id = hesai_config.vlan_id | response[payload_pos++]; + hesai_config.clock_data_fmt = static_cast(response[payload_pos++]); + hesai_config.noise_filtering = static_cast(response[payload_pos++]); + hesai_config.reflectivity_mapping = static_cast(response[payload_pos++]); + hesai_config.reserved[0] = static_cast(response[payload_pos++]); + hesai_config.reserved[1] = static_cast(response[payload_pos++]); + hesai_config.reserved[2] = static_cast(response[payload_pos++]); + hesai_config.reserved[3] = static_cast(response[payload_pos++]); + hesai_config.reserved[4] = static_cast(response[payload_pos++]); + hesai_config.reserved[5] = static_cast(response[payload_pos++]); + } + + std::cout << "Config: " << hesai_config << std::endl; + return hesai_config_ptr; } -Status HesaiHwInterface::GetConfig(std::shared_ptr ctx, bool with_run) -{ - return GetConfig( - ctx, [this](HesaiConfig & result) { std::cout << result << std::endl; }, with_run); -} -Status HesaiHwInterface::GetConfig( - std::function callback, bool with_run) -{ - if (with_run) { - if (tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } - } - return GetConfig(tcp_driver_, callback, with_run); -} -Status HesaiHwInterface::GetConfig(bool with_run) -{ - return GetConfig([this](HesaiConfig & result) { std::cout << result << std::endl; }, with_run); -} Status HesaiHwInterface::GetLidarStatus( std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, @@ -1365,9 +1323,6 @@ Status HesaiHwInterface::GetLidarStatus( buf_vec.emplace_back((len >> 8) & 0xff); buf_vec.emplace_back((len >> 0) & 0xff); - if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max, "GetLidarStatus")) { - return GetLidarStatus(target_tcp_driver, callback, with_run); - } PrintDebug("GetLidarStatus: start"); target_tcp_driver->asyncSendReceiveHeaderPayload( @@ -1438,7 +1393,7 @@ Status HesaiHwInterface::GetLidarStatus( callback(hesai_status); } }, - [this]() { CheckUnlock(tm_, "GetLidarStatus"); }); + [this]() { }); if (with_run) { boost::system::error_code ec = target_tcp_driver->run(); if (ec) { @@ -1494,12 +1449,9 @@ Status HesaiHwInterface::SetSpinRate( buf_vec.emplace_back((rpm >> 8) & 0xff); buf_vec.emplace_back((rpm >> 0) & 0xff); - if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max_sensor_setup, "SetSpinRate")) { - return SetSpinRate(target_tcp_driver, rpm, with_run); - } PrintDebug("SetSpinRate: start"); - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tm_, "SetSpinRate"); }); + target_tcp_driver->asyncSend(buf_vec, [this]() { }); if (with_run) { boost::system::error_code ec = target_tcp_driver->run(); if (ec) { @@ -1542,23 +1494,20 @@ Status HesaiHwInterface::SetSyncAngle( buf_vec.emplace_back((angle >> 8) & 0xff); buf_vec.emplace_back((angle >> 0) & 0xff); - if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max_sensor_setup, "SetSyncAngle")) { - return SetSyncAngle(target_tcp_driver, sync_angle, angle, with_run); - } PrintDebug("SetSyncAngle: start"); - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tm_, "SetSyncAngle"); }); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::SetSyncAngle: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): SetSyncAngle" << std::endl; -#endif - } +target_tcp_driver->syncSendReceiveHeaderPayload( + buf_vec, + []([[maybe_unused]]const std::vector & received_bytes) {}, + [this, target_tcp_driver]([[maybe_unused]]const std::vector & received_bytes) { + auto response = target_tcp_driver->getPayload(); + }, + [](){}); - return Status::WAITING_FOR_SENSOR_RESPONSE; + PrintDebug("SetSyncAngle: done"); + + + return Status::OK; } Status HesaiHwInterface::SetSyncAngle(int sync_angle, int angle, bool with_run) @@ -1588,12 +1537,9 @@ Status HesaiHwInterface::SetTriggerMethod( buf_vec.emplace_back((trigger_method >> 0) & 0xff); - if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max_sensor_setup, "SetTriggerMethod")) { - return SetTriggerMethod(target_tcp_driver, trigger_method, with_run); - } PrintDebug("SetTriggerMethod: start"); - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tm_, "SetTriggerMethod"); }); + target_tcp_driver->asyncSend(buf_vec, [this]() { }); if (with_run) { boost::system::error_code ec = target_tcp_driver->run(); if (ec) { @@ -1633,12 +1579,10 @@ Status HesaiHwInterface::SetStandbyMode( buf_vec.emplace_back((len >> 0) & 0xff); buf_vec.emplace_back((standby_mode >> 0) & 0xff); - if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max_sensor_setup, "SetStandbyMode")) { - return SetStandbyMode(target_tcp_driver, standby_mode, with_run); - } + std::cout << "start: SetStandbyMode" << std::endl; - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tm_, "SetStandbyMode"); }); + target_tcp_driver->asyncSend(buf_vec, [this]() { }); if (with_run) { boost::system::error_code ec = target_tcp_driver->run(); if (ec) { @@ -1679,22 +1623,15 @@ Status HesaiHwInterface::SetReturnMode( buf_vec.emplace_back((return_mode >> 0) & 0xff); PrintDebug("SetReturnMode: start" + std::to_string(return_mode)); - if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max_sensor_setup, "SetReturnMode")) { - return SetReturnMode(target_tcp_driver, return_mode, with_run); - } - PrintDebug("SetReturnMode: asyncSend"); - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tm_, "SetReturnMode"); }); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::SetReturnMode: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): SetReturnMode" << std::endl; -#endif - } + target_tcp_driver->syncSendReceiveHeaderPayload( + buf_vec, + []([[maybe_unused]]const std::vector & received_bytes) {}, + [this, target_tcp_driver]([[maybe_unused]]const std::vector & received_bytes) { + auto response = target_tcp_driver->getPayload(); + }, + [](){}); PrintDebug("SetReturnMode: done"); - return Status::WAITING_FOR_SENSOR_RESPONSE; + return Status::OK; } Status HesaiHwInterface::SetReturnMode(int return_mode, bool with_run) @@ -1733,24 +1670,20 @@ Status HesaiHwInterface::SetDestinationIp( buf_vec.emplace_back((gps_port >> 8) & 0xff); buf_vec.emplace_back((gps_port >> 0) & 0xff); - if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max_sensor_setup, "SetDestinationIp")) { - return SetDestinationIp( - target_tcp_driver, dest_ip_1, dest_ip_2, dest_ip_3, dest_ip_4, port, gps_port, with_run); - } PrintDebug("SetDestinationIp: start"); - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tm_, "SetDestinationIp"); }); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::SetDestinationIp: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): SetDestinationIp" << std::endl; -#endif - } +target_tcp_driver->syncSendReceiveHeaderPayload( + buf_vec, + []([[maybe_unused]]const std::vector & received_bytes) {}, + [this, target_tcp_driver]([[maybe_unused]]const std::vector & received_bytes) { + auto response = target_tcp_driver->getPayload(); + }, + [](){}); - return Status::WAITING_FOR_SENSOR_RESPONSE; + PrintDebug("SetDestinationIp: done"); + + + return Status::OK; } Status HesaiHwInterface::SetDestinationIp( @@ -1797,14 +1730,9 @@ Status HesaiHwInterface::SetControlPort( buf_vec.emplace_back((vlan_id >> 8) & 0xff); buf_vec.emplace_back((vlan_id >> 0) & 0xff); - if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max_sensor_setup, "SetControlPort")) { - return SetControlPort( - target_tcp_driver, ip_1, ip_2, ip_3, ip_4, mask_1, mask_2, mask_3, mask_4, gateway_1, - gateway_2, gateway_3, gateway_4, vlan_flg, vlan_id, with_run); - } PrintDebug("SetControlPort: start"); - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tm_, "SetControlPort"); }); + target_tcp_driver->asyncSend(buf_vec, [this]() { }); if (with_run) { boost::system::error_code ec = target_tcp_driver->run(); if (ec) { @@ -1856,12 +1784,9 @@ Status HesaiHwInterface::SetLidarRange( buf_vec.emplace_back(d); } - if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max_sensor_setup, "SetLidarRange")) { - return SetLidarRange(target_tcp_driver, method, data, with_run); - } PrintDebug("SetLidarRange: start"); - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tm_, "SetLidarRange"); }); + target_tcp_driver->asyncSend(buf_vec, [this]() { }); if (with_run) { boost::system::error_code ec = target_tcp_driver->run(); if (ec) { @@ -1910,12 +1835,9 @@ Status HesaiHwInterface::SetLidarRange( buf_vec.emplace_back((end >> 8) & 0xff); buf_vec.emplace_back((end >> 0) & 0xff); - if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max_sensor_setup, "SetLidarRange(All)")) { - return SetLidarRange(target_tcp_driver, start, end, with_run); - } PrintDebug("SetLidarRange(All): start"); - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tm_, "SetLidarRange(All)"); }); + target_tcp_driver->asyncSend(buf_vec, [this]() { }); if (with_run) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << "start ctx->run(): SetLidarRange(All)" << std::endl; @@ -1956,10 +1878,8 @@ Status HesaiHwInterface::GetLidarRange( buf_vec.emplace_back((len >> 16) & 0xff); buf_vec.emplace_back((len >> 8) & 0xff); buf_vec.emplace_back((len >> 0) & 0xff); - if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max, "GetLidarRange")) { - return GetLidarRange(target_tcp_driver, callback, with_run); - } - PrintDebug("SetLidarRange: start"); + + PrintDebug("GetLidarRange: start"); target_tcp_driver->asyncSendReceiveHeaderPayload( buf_vec, @@ -2021,7 +1941,7 @@ Status HesaiHwInterface::GetLidarRange( } } }, - [this]() { CheckUnlock(tm_, "GetLidarRange"); }); + [this]() { }); if (with_run) { boost::system::error_code ec = target_tcp_driver->run(); if (ec) { @@ -2073,12 +1993,9 @@ Status HesaiHwInterface::SetClockSource( buf_vec.emplace_back((clock_source >> 0) & 0xff); - if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max_sensor_setup, "SetClockSource")) { - return SetClockSource(target_tcp_driver, clock_source, with_run); - } PrintDebug("SetClockSource: start"); - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tm_, "SetClockSource"); }); + target_tcp_driver->asyncSend(buf_vec, [this]() { }); if (with_run) { boost::system::error_code ec = target_tcp_driver->run(); if (ec) { @@ -2106,7 +2023,7 @@ Status HesaiHwInterface::SetClockSource(int clock_source, bool with_run) Status HesaiHwInterface::SetPtpConfig( std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int profile, int domain, - int network, int switch_type, int logAnnounceInterval, int logSyncInterval, + int network, int switch_type, int logAnnounceInterval, int logSyncInterval, int logMinDelayReqInterval, bool with_run) { if (profile < 0 || profile > 3) { @@ -2153,25 +2070,21 @@ Status HesaiHwInterface::SetPtpConfig( buf_vec.emplace_back((switch_type >> 0) & 0xff); } - if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max_sensor_setup, "SetPtpConfig")) { - return SetPtpConfig( - target_tcp_driver, profile, domain, network, switch_type, logAnnounceInterval, - logSyncInterval, logMinDelayReqInterval, with_run); - } PrintDebug("SetPtpConfig: start"); - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tm_, "SetPtpConfig"); }); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::SetPtpConfig: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): SetPtpConfig" << std::endl; -#endif - } + target_tcp_driver->syncSendReceiveHeaderPayload( + buf_vec, + []([[maybe_unused]]const std::vector & received_bytes) {}, + [this, target_tcp_driver]([[maybe_unused]]const std::vector & received_bytes) { + auto response = target_tcp_driver->getPayload(); + PrintDebug("SetPtpConfig: recv'd"); - return Status::WAITING_FOR_SENSOR_RESPONSE; + }, + [](){}); + PrintDebug("SetPtpConfig: done"); + + + return Status::OK; } Status HesaiHwInterface::SetPtpConfig( std::shared_ptr ctx, int profile, int domain, int network, @@ -2214,9 +2127,6 @@ Status HesaiHwInterface::GetPtpConfig( buf_vec.emplace_back((len >> 8) & 0xff); buf_vec.emplace_back((len >> 0) & 0xff); - if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max, "GetPtpConfig")) { - return GetPtpConfig(target_tcp_driver, with_run); - } PrintDebug("GetPtpConfig: start"); target_tcp_driver->asyncSendReceiveHeaderPayload( @@ -2269,7 +2179,7 @@ Status HesaiHwInterface::GetPtpConfig( PrintInfo(ss.str()); } }, - [this]() { CheckUnlock(tm_, "GetPtpConfig"); }); + [this]() { }); if (with_run) { boost::system::error_code ec = target_tcp_driver->run(); if (ec) { @@ -2307,12 +2217,9 @@ Status HesaiHwInterface::SendReset( buf_vec.emplace_back((len >> 8) & 0xff); buf_vec.emplace_back((len >> 0) & 0xff); - if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max_sensor_setup, "SendReset")) { - return SendReset(target_tcp_driver, with_run); - } PrintDebug("SendReset: start"); - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tm_, "SendReset"); }); + target_tcp_driver->asyncSend(buf_vec, [this]() { }); if (with_run) { boost::system::error_code ec = target_tcp_driver->run(); if (ec) { @@ -2354,12 +2261,9 @@ Status HesaiHwInterface::SetRotDir( buf_vec.emplace_back((mode >> 0) & 0xff); - if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max_sensor_setup, "SetRotDir")) { - return SetRotDir(target_tcp_driver, mode, with_run); - } PrintDebug("SetRotDir: start"); - target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tm_, "SetRotDir"); }); + target_tcp_driver->asyncSend(buf_vec, [this]() { }); if (with_run) { boost::system::error_code ec = target_tcp_driver->run(); if (ec) { @@ -2398,9 +2302,6 @@ Status HesaiHwInterface::GetLidarMonitor( buf_vec.emplace_back((len >> 8) & 0xff); buf_vec.emplace_back((len >> 0) & 0xff); - if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max, "GetLidarMonitor")) { - return GetLidarMonitor(target_tcp_driver, callback, with_run); - } PrintDebug("GetLidarMonitor: start"); target_tcp_driver->asyncSendReceiveHeaderPayload( @@ -2465,7 +2366,7 @@ Status HesaiHwInterface::GetLidarMonitor( callback(hesai_lidar_monitor); } }, - [this]() { CheckUnlock(tm_, "GetLidarMonitor"); }); + [this]() { }); if (with_run) { boost::system::error_code ec = target_tcp_driver->run(); if (ec) { @@ -2587,14 +2488,14 @@ HesaiStatus HesaiHwInterface::SetPtpConfigSyncHttp( auto response = hcd->get( (boost::format("/pandar.cgi?action=set&object=lidar&key=ptp_configuration&value={" \ - "\"Profile\": %d," \ - "\"Domain\": %d," \ - "\"Network\": %d," \ - "\"LogAnnounceInterval\": %d," \ - "\"LogSyncInterval\": %d," \ - "\"LogMinDelayReqInterval\": %d," \ - "\"tsn_switch\": %d" \ - "}") + "\"Profile\": %d," \ + "\"Domain\": %d," \ + "\"Network\": %d," \ + "\"LogAnnounceInterval\": %d," \ + "\"LogSyncInterval\": %d," \ + "\"LogMinDelayReqInterval\": %d," \ + "\"tsn_switch\": %d" \ + "}") % profile % domain % network % logAnnounceInterval % logSyncInterval % logMinDelayReqInterval % 0 ).str()); ctx->run(); @@ -2607,14 +2508,14 @@ HesaiStatus HesaiHwInterface::SetPtpConfigSyncHttp(int profile, int network, int logAnnounceInterval, int logSyncInterval, - int logMinDelayReqInterval) + int logMinDelayReqInterval) { return SetPtpConfigSyncHttp(std::make_shared(), profile, domain, network, logAnnounceInterval, - logSyncInterval, + logSyncInterval, logMinDelayReqInterval); } @@ -2629,9 +2530,9 @@ HesaiStatus HesaiHwInterface::SetSyncAngleSyncHttp( return st; } auto tmp_str = (boost::format("/pandar.cgi?action=set&object=lidar_sync&key=sync_angle&value={" \ - "\"sync\": %d," \ - "\"syncAngle\": %d" \ - "}") % enable % angle).str(); + "\"sync\": %d," \ + "\"syncAngle\": %d" \ + "}") % enable % angle).str(); PrintInfo(tmp_str); auto response = hcd->get(tmp_str); ctx->run(); @@ -2691,13 +2592,16 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( std::stringstream ss2; ss2 << sensor_configuration->return_mode; PrintInfo("Current Configuration return_mode: " + ss2.str()); - auto return_mode_int = nebula::drivers::IntFromReturnModeHesai( - sensor_configuration->return_mode, sensor_configuration->sensor_model); - if (return_mode_int < 0) { - PrintError("Invalid Return Mode for this sensor. Please check your settings. Falling back to Dual mode."); - return_mode_int = 2; - } - SetReturnMode(return_mode_int); + std::thread t([this, sensor_configuration] { + auto return_mode_int = nebula::drivers::IntFromReturnModeHesai( + sensor_configuration->return_mode, sensor_configuration->sensor_model); + if (return_mode_int < 0) { + PrintError("Invalid Return Mode for this sensor. Please check your settings. Falling back to Dual mode."); + return_mode_int = 2; + } + SetReturnMode(return_mode_int); + }); + t.join(); std::this_thread::sleep_for(wait_time); } @@ -2711,7 +2615,9 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( SetSpinSpeedAsyncHttp(sensor_configuration->rotation_speed); } else { PrintInfo("Setting up spin rate via TCP." + std::to_string(sensor_configuration->rotation_speed) ); - SetSpinRate(sensor_configuration->rotation_speed); + std::thread t( + [this, sensor_configuration] { SetSpinRate(sensor_configuration->rotation_speed); }); + t.join(); } std::this_thread::sleep_for(wait_time); } @@ -2746,14 +2652,18 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( if (set_flg) { std::vector list_string; boost::split(list_string, sensor_configuration->host_ip, boost::is_any_of(".")); - SetDestinationIp( - 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); + std::thread t([this, sensor_configuration, list_string] { + SetDestinationIp( + 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); + }); + t.join(); std::this_thread::sleep_for(wait_time); } - if (sensor_configuration->sensor_model != SensorModel::HESAI_PANDARAT128) { + if (sensor_configuration->sensor_model != SensorModel::HESAI_PANDARAT128 + && sensor_configuration->sensor_model != SensorModel::HESAI_PANDARQT128) { set_flg = true; auto sync_angle = static_cast(hesai_config.sync_angle / 100); auto scan_phase = static_cast(sensor_configuration->scan_phase); @@ -2765,32 +2675,43 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( PrintInfo("current lidar sync: " + std::to_string(hesai_config.sync)); PrintInfo("current lidar sync_angle: " + std::to_string(sync_angle)); PrintInfo("current configuration scan_phase: " + std::to_string(scan_phase)); - SetSyncAngle(sync_flg, scan_phase); + std::thread t([this, sync_flg, scan_phase] { + SetSyncAngle(sync_flg, scan_phase); + }); + t.join(); std::this_thread::sleep_for(wait_time); } - if(sensor_configuration->sensor_model == SensorModel::HESAI_PANDAR40P - || sensor_configuration->sensor_model == SensorModel::HESAI_PANDAR64 - || sensor_configuration->sensor_model == SensorModel::HESAI_PANDARQT64 - || sensor_configuration->sensor_model == SensorModel::HESAI_PANDARXT32 - || sensor_configuration->sensor_model == SensorModel::HESAI_PANDARXT32M) { - PrintInfo("Trying to set Clock source to PTP"); - SetClockSource(HESAI_LIDAR_PTP_CLOCK_SOURCE); - } - 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(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 - ); + std::thread t([this, sensor_configuration] { + if(sensor_configuration->sensor_model == SensorModel::HESAI_PANDAR40P + || sensor_configuration->sensor_model == SensorModel::HESAI_PANDAR64 + || sensor_configuration->sensor_model == SensorModel::HESAI_PANDARQT64 + || sensor_configuration->sensor_model == SensorModel::HESAI_PANDARXT32 + || sensor_configuration->sensor_model == SensorModel::HESAI_PANDARXT32M) { + PrintInfo("Trying to set Clock source to PTP"); + SetClockSource(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(static_cast(sensor_configuration->ptp_profile), + sensor_configuration->ptp_domain, + static_cast(sensor_configuration->ptp_transport_type), + static_cast(sensor_configuration->ptp_switch_type), + PTP_LOG_ANNOUNCE_INTERVAL, + PTP_SYNC_INTERVAL, + PTP_LOG_MIN_DELAY_INTERVAL + ); + PrintDebug("Setting properties done"); + }); + PrintDebug("Waiting for thread to finish"); + + t.join(); + PrintDebug("Thread finished"); + std::this_thread::sleep_for(wait_time); } else { //AT128 only supports PTP setup via HTTP @@ -2799,12 +2720,12 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( static_cast(sensor_configuration->scan_phase)); std::ostringstream tmp_ostringstream; tmp_ostringstream << "Trying to set PTP Config: " << sensor_configuration->ptp_profile - << ", Domain: " << sensor_configuration->ptp_domain - << ", Transport: " << sensor_configuration->ptp_transport_type << " via HTTP"; + << ", Domain: " << sensor_configuration->ptp_domain + << ", Transport: " << sensor_configuration->ptp_transport_type << " via HTTP"; PrintInfo(tmp_ostringstream.str()); SetPtpConfigSyncHttp(static_cast(sensor_configuration->ptp_profile), sensor_configuration->ptp_domain, - static_cast(sensor_configuration->ptp_transport_type), + static_cast(sensor_configuration->ptp_transport_type), PTP_LOG_ANNOUNCE_INTERVAL, PTP_SYNC_INTERVAL, PTP_LOG_MIN_DELAY_INTERVAL); @@ -2814,7 +2735,9 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << "End CheckAndSetConfig(HesaiConfig)!!" << std::endl; #endif - return Status::WAITING_FOR_SENSOR_RESPONSE; + PrintDebug("GetAndCheckConfig(HesaiConfig) finished"); + + return Status::OK; } HesaiStatus HesaiHwInterface::CheckAndSetConfig( @@ -2854,11 +2777,14 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( } if (set_flg) { - SetLidarRange( - static_cast(sensor_configuration->cloud_min_angle * 10), - static_cast(sensor_configuration->cloud_max_angle * 10) //, - // false - ); + std::thread t([this, sensor_configuration] { + SetLidarRange( + static_cast(sensor_configuration->cloud_min_angle * 10), + static_cast(sensor_configuration->cloud_max_angle * 10) //, + // false + ); + }); + t.join(); } #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE @@ -2872,23 +2798,28 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig() #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << "Start CheckAndSetConfig!!" << std::endl; #endif -GetConfig( // ctx, - [this](HesaiConfig & result) { + std::thread t([this] { + auto result = GetConfig(); + std::stringstream ss; - ss << result; + ss << *result; PrintInfo(ss.str()); CheckAndSetConfig( - std::static_pointer_cast(sensor_configuration_), result); + std::static_pointer_cast(sensor_configuration_), *result); }); + t.join();//here -GetLidarRange( // ctx, - [this](HesaiLidarRangeAll & result) { - std::stringstream ss; - ss << result; - PrintInfo(ss.str()); - CheckAndSetConfig( - std::static_pointer_cast(sensor_configuration_), result); + std::thread t2([this] { + GetLidarRange( // ctx, + [this](HesaiLidarRangeAll & result) { + std::stringstream ss; + ss << result; + PrintInfo(ss.str()); + CheckAndSetConfig( + std::static_pointer_cast(sensor_configuration_), result); + }); }); + t2.join(); #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << "End CheckAndSetConfig!!" << std::endl; #endif @@ -2907,6 +2838,7 @@ GetLidarRange( // ctx, 32: QT128C2X 38: ? 40: AT128? +42: OT128 48: ? */ int HesaiHwInterface::NebulaModelToHesaiModelNo(nebula::drivers::SensorModel model) @@ -3026,62 +2958,6 @@ bool HesaiHwInterface::UseHttpGetLidarMonitor(int model) } bool HesaiHwInterface::UseHttpGetLidarMonitor() { return UseHttpGetLidarMonitor(target_model_no); } -bool HesaiHwInterface::CheckLock( - std::timed_mutex & tm, int & fail_cnt, const int & fail_cnt_max, std::string name) -{ - if (wl) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE -// std::chrono::time_point start_time = std::chrono::steady_clock::now(); - std::chrono::system_clock::time_point start_time = std::chrono::system_clock::now(); - std::time_t stt = std::chrono::system_clock::to_time_t(start_time); - const std::tm* system_local_time = std::localtime(&stt); - std::cout << "try_lock_for: start at " << std::put_time(system_local_time, "%c") << std::endl; -/* - std::chrono::system_clock::time_point test_time = std::chrono::system_clock::now(); - std::time_t tst = std::chrono::system_clock::to_time_t(test_time); - const std::tm* local_time = std::localtime(&tst); - std::cerr << "try_lock_for: test at " << std::put_time(local_time, "%c") << std::endl; - auto dur_test = test_time - start_time; - std::cerr << "test: " << name << " : " << std::chrono::duration_cast(dur_test).count() << std::endl; -*/ -#endif - if (!tm.try_lock_for(std::chrono::milliseconds(timeout_))) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "if (!tm.try_lock_for(std::chrono::milliseconds(" << timeout_ << "))) {" << std::endl; - std::chrono::system_clock::time_point end_time = std::chrono::system_clock::now(); - std::time_t edt = std::chrono::system_clock::to_time_t(end_time); - const std::tm* end_local_time = std::localtime(&edt); - std::cerr << "try_lock_for: end at " << std::put_time(end_local_time, "%c") << std::endl; - auto dur = end_time - start_time; - std::cerr << "timeout: " << name << " : " << std::chrono::duration_cast(dur).count() << std::endl; -#endif - PrintDebug("timeout: " + name); - fail_cnt++; - if (fail_cnt_max < fail_cnt) { - tm.unlock(); - if (tcp_driver_ && tcp_driver_->isOpen()) { - tcp_driver_->closeSync(); - tcp_driver_->open(); - } - } - else { - return true; - } - return false; - } - fail_cnt = 0; - } - return true; -} - -void HesaiHwInterface::CheckUnlock(std::timed_mutex & tm, std::string name) -{ - if (wl) { - tm.unlock(); - PrintDebug(name + ": finished"); - } -} - void HesaiHwInterface::SetLogger(std::shared_ptr logger) { parent_node_logger = logger; diff --git a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp index 8a6060d21..d7581b7f9 100644 --- a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp @@ -7,6 +7,8 @@ namespace ros HesaiHwInterfaceRosWrapper::HesaiHwInterfaceRosWrapper(const rclcpp::NodeOptions & options) : rclcpp::Node("hesai_hw_interface_ros_wrapper", options), hw_interface_() { + this->get_logger().set_level(rclcpp::Logger::Level::Debug); + if (mtx_config_.try_lock()) { interface_status_ = GetParameters(sensor_configuration_); mtx_config_.unlock(); From 80eb04272af0241e0d9b53a4b1c5dd8c9f3bf41d Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 14 Mar 2024 14:13:39 +0900 Subject: [PATCH 23/31] fix(hesai_hw_interface): tcp communication works with dummy server now --- .../hesai_hw_interface.hpp | 649 +--- .../hesai_hw_interface.cpp | 2759 ++++------------- .../src/hesai/hesai_decoder_ros_wrapper.cpp | 96 +- .../hesai/hesai_hw_interface_ros_wrapper.cpp | 9 +- .../hesai/hesai_hw_monitor_ros_wrapper.cpp | 43 +- 5 files changed, 782 insertions(+), 2774 deletions(-) diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp index d15f4a28a..2f1ced03a 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp @@ -10,13 +10,13 @@ #if (BOOST_VERSION / 100 == 1074) // Boost 1.74 #define BOOST_ALLOW_DEPRECATED_HEADERS #endif +#include "boost_tcp_driver/http_client_driver.hpp" +#include "boost_tcp_driver/tcp_driver.hpp" +#include "boost_udp_driver/udp_driver.hpp" #include "nebula_common/hesai/hesai_common.hpp" #include "nebula_common/hesai/hesai_status.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp" -#include "boost_tcp_driver/http_client_driver.hpp" -#include "boost_tcp_driver/tcp_driver.hpp" -#include "boost_udp_driver/udp_driver.hpp" #include @@ -77,9 +77,12 @@ const uint16_t PANDAR128_E4X_EXTENDED_PACKET_SIZE = 1117; const uint16_t MTU_SIZE = 1500; -const int PTP_LOG_ANNOUNCE_INTERVAL = 1; // Time interval between Announce messages, in units of log seconds (default: 1) -const int PTP_SYNC_INTERVAL = 1; //Time interval between Sync messages, in units of log seconds (default: 1) -const int PTP_LOG_MIN_DELAY_INTERVAL = 0; //Minimum permitted mean time between Delay_Req messages, in units of log seconds (default: 0) +const int PTP_LOG_ANNOUNCE_INTERVAL = + 1; // Time interval between Announce messages, in units of log seconds (default: 1) +const int PTP_SYNC_INTERVAL = + 1; // Time interval between Sync messages, in units of log seconds (default: 1) +const int PTP_LOG_MIN_DELAY_INTERVAL = 0; // Minimum permitted mean time between Delay_Req + // messages, in units of log seconds (default: 0) const int HESAI_LIDAR_GPS_CLOCK_SOURCE = 0; const int HESAI_LIDAR_PTP_CLOCK_SOURCE = 1; @@ -138,11 +141,14 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @param bytes Target byte vector void PrintDebug(const std::vector & bytes); - /// @brief Send a PTC request with an optional payload, and return the full response payload. Blocking. + /// @brief Send a PTC request with an optional payload, and return the full response payload. + /// Blocking. /// @param command_id PTC command number. /// @param payload Payload bytes of the PTC command. Not including the 8-byte PTC header. /// @return The returned payload, if successful, or nullptr. - std::shared_ptr> SendReceive(const uint8_t command_id, const std::vector & payload = {}); + std::shared_ptr> SendReceive( + const uint8_t command_id, const std::vector & payload = {}); + public: /// @brief Constructor HesaiHwInterface(); @@ -187,445 +193,64 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @return Resulting status Status RegisterScanCallback( std::function)> scan_callback); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION (sync) - /// @param target_tcp_driver TcpDriver used - /// @return Resulting status - Status syncGetLidarCalibration( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function & received_bytes)> bytes_callback); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION (sync) - /// @param bytes_callback callback - /// @param ctx IO Context used - /// @return Resulting status - Status syncGetLidarCalibration( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function str_callback); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION (sync) - /// @param str_callback callback - /// @param ctx IO Context used - /// @return Resulting status - Status syncGetLidarCalibration( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION (sync) - /// @param ctx IO Context used - /// @return Resulting status - Status syncGetLidarCalibration( - std::shared_ptr ctx, - std::function str_callback); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION (sync) - /// @param str_callback callback - /// @return Resulting status - Status syncGetLidarCalibration(std::shared_ptr ctx); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION (sync) - /// @return Resulting status - Status syncGetLidarCalibrationFromSensor( - std::function & received_bytes)> bytes_callback); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION (sync) - /// @param target_tcp_driver TcpDriver used - /// @param bytes_callback callback - /// @return Resulting status - Status syncGetLidarCalibrationFromSensor( - std::function str_callback); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION (sync) - /// @param target_tcp_driver TcpDriver used - /// @param str_callback callback - /// @return Resulting status - Status syncGetLidarCalibrationFromSensor(); /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION - /// @param target_tcp_driver TcpDriver used - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status GetLidarCalibration( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function & received_bytes)> bytes_callback, - bool with_run = true); + std::string GetLidarCalibrationString(); /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION - /// @param bytes_callback callback - /// @param ctx IO Context used - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status GetLidarCalibration( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function str_callback, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION - /// @param str_callback callback - /// @param ctx IO Context used - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetLidarCalibration( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION - /// @param ctx IO Context used - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetLidarCalibration( - std::shared_ptr ctx, - std::function str_callback, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION - /// @param str_callback callback - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetLidarCalibration(std::shared_ptr ctx, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetLidarCalibrationFromSensor( - std::function & received_bytes)> bytes_callback, - bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION - /// @param target_tcp_driver TcpDriver used - /// @param bytes_callback callback - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetLidarCalibrationFromSensor( - std::function str_callback, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION - /// @param target_tcp_driver TcpDriver used - /// @param str_callback callback - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetLidarCalibrationFromSensor(bool with_run = true); + std::vector GetLidarCalibrationBytes(); /// @brief Getting data with PTC_COMMAND_PTP_DIAGNOSTICS (PTP STATUS) - /// @param target_tcp_driver TcpDriver used - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status GetPtpDiagStatus( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_PTP_DIAGNOSTICS (PTP STATUS) - /// @param ctx IO Context used - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetPtpDiagStatus(std::shared_ptr ctx, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_PTP_DIAGNOSTICS (PTP STATUS) - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetPtpDiagStatus(bool with_run = true); + HesaiPtpDiagStatus GetPtpDiagStatus(); /// @brief Getting data with PTC_COMMAND_PTP_DIAGNOSTICS (PTP TLV PORT_DATA_SET) - /// @param target_tcp_driver TcpDriver used - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status GetPtpDiagPort( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_PTP_DIAGNOSTICS (PTP TLV PORT_DATA_SET) - /// @param ctx IO Context used - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetPtpDiagPort(std::shared_ptr ctx, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_PTP_DIAGNOSTICS (PTP TLV PORT_DATA_SET) - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetPtpDiagPort(bool with_run = true); + HesaiPtpDiagPort GetPtpDiagPort(); /// @brief Getting data with PTC_COMMAND_PTP_DIAGNOSTICS (PTP TLV TIME_STATUS_NP) - /// @param target_tcp_driver TcpDriver used - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status GetPtpDiagTime( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_PTP_DIAGNOSTICS (PTP TLV TIME_STATUS_NP) - /// @param ctx IO Context used - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetPtpDiagTime(std::shared_ptr ctx, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_PTP_DIAGNOSTICS (PTP TLV TIME_STATUS_NP) - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetPtpDiagTime(bool with_run = true); - /// @brief Getting data with PTC_COMMAND_PTP_DIAGNOSTICS (PTP TLV GRANDMASTER_SETTINGS_NP) - /// @param target_tcp_driver TcpDriver used - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetPtpDiagGrandmaster( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_PTP_DIAGNOSTICS (PTP TLV GRANDMASTER_SETTINGS_NP) - /// @param ctx IO Context used - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetPtpDiagGrandmaster(std::shared_ptr ctx, bool with_run = true); + HesaiPtpDiagTime GetPtpDiagTime(); /// @brief Getting data with PTC_COMMAND_PTP_DIAGNOSTICS (PTP TLV GRANDMASTER_SETTINGS_NP) - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetPtpDiagGrandmaster(bool with_run = true); - - /// @brief Getting data with PTC_COMMAND_GET_INVENTORY_INFO (sync) - /// @param target_tcp_driver TcpDriver used - /// @param callback Callback function for received HesaiInventory - /// @return Resulting status - Status syncGetInventory( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function callback); - /// @brief Getting data with PTC_COMMAND_GET_INVENTORY_INFO (sync) - /// @param target_tcp_driver TcpDriver used - /// @return Resulting status - Status syncGetInventory( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver); - /// @brief Getting data with PTC_COMMAND_GET_INVENTORY_INFO (sync) - /// @param ctx IO Context used - /// @param callback Callback function for received HesaiInventory - /// @return Resulting status - Status syncGetInventory( - std::shared_ptr ctx, - std::function callback); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION (sync) - /// @param ctx IO Context used /// @return Resulting status - Status syncGetInventory(std::shared_ptr ctx); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION (sync) - /// @param callback Callback function for received HesaiInventory - /// @return Resulting status - Status syncGetInventory(std::function callback); - + HesaiPtpDiagGrandmaster GetPtpDiagGrandmaster(); /// @brief Getting data with PTC_COMMAND_GET_INVENTORY_INFO - /// @param target_tcp_driver TcpDriver used - /// @param callback Callback function for received HesaiInventory - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status GetInventory( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function callback, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_INVENTORY_INFO - /// @param ctx IO Context used - /// @param callback Callback function for received HesaiInventory - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetInventory( - std::shared_ptr ctx, - std::function callback, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_INVENTORY_INFO - /// @param ctx IO Context used - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetInventory(std::shared_ptr ctx, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_INVENTORY_INFO - /// @param callback Callback function for received HesaiInventory - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetInventory(std::function callback, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_INVENTORY_INFO - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetInventory(bool with_run = true); + HesaiInventory GetInventory(); /// @brief Getting data with PTC_COMMAND_GET_CONFIG_INFO - /// @param target_tcp_driver TcpDriver used - /// @param callback Callback function for received HesaiConfig - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - std::shared_ptr GetConfig(); + HesaiConfig GetConfig(); /// @brief Getting data with PTC_COMMAND_GET_LIDAR_STATUS - /// @param target_tcp_driver TcpDriver used - /// @param callback Callback function for received HesaiLidarStatus - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status GetLidarStatus( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function callback, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_STATUS - /// @param ctx IO Context used - /// @param callback Callback function for received HesaiLidarStatus - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetLidarStatus( - std::shared_ptr ctx, - std::function callback, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_STATUS - /// @param ctx IO Context used - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetLidarStatus(std::shared_ptr ctx, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_STATUS - /// @param callback Callback function for received HesaiLidarStatus - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetLidarStatus( - std::function callback, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_LIDAR_STATUS - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetLidarStatus(bool with_run = true); - /// @brief Setting value with PTC_COMMAND_SET_SPIN_RATE - /// @param target_tcp_driver TcpDriver used - /// @param rpm Spin rate - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetSpinRate( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, uint16_t rpm, - bool with_run = true); - /// @brief Setting value with PTC_COMMAND_SET_SPIN_RATE - /// @param ctx IO Context used - /// @param rpm Spin rate - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetSpinRate( - std::shared_ptr ctx, uint16_t rpm, bool with_run = true); + HesaiLidarStatus GetLidarStatus(); /// @brief Setting value with PTC_COMMAND_SET_SPIN_RATE /// @param rpm Spin rate - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetSpinRate(uint16_t rpm, bool with_run = true); - /// @brief Setting value with PTC_COMMAND_SET_SYNC_ANGLE - /// @param target_tcp_driver TcpDriver used - /// @param sync_angle Sync angle enable flag - /// @param angle Angle value - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetSyncAngle( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int sync_angle, int angle, - bool with_run = true); - /// @brief Setting value with PTC_COMMAND_SET_SYNC_ANGLE - /// @param ctx IO Context used - /// @param sync_angle Sync angle enable flag - /// @param angle Angle value - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status SetSyncAngle( - std::shared_ptr ctx, int sync_angle, int angle, bool with_run = true); + Status SetSpinRate(uint16_t rpm); /// @brief Setting value with PTC_COMMAND_SET_SYNC_ANGLE /// @param sync_angle Sync angle enable flag /// @param angle Angle value - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status SetSyncAngle(int sync_angle, int angle, bool with_run = true); + Status SetSyncAngle(int sync_angle, int angle); /// @brief Setting mode with PTC_COMMAND_SET_TRIGGER_METHOD - /// @param target_tcp_driver TcpDriver used /// @param trigger_method Trigger method - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetTriggerMethod( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int trigger_method, - bool with_run = true); - /// @brief Setting mode with PTC_COMMAND_SET_TRIGGER_METHOD - /// @param ctx IO Context used - /// @param trigger_method Trigger method - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetTriggerMethod( - std::shared_ptr ctx, int trigger_method, bool with_run = true); - /// @brief Setting mode with PTC_COMMAND_SET_TRIGGER_METHOD - /// @param trigger_method Trigger method - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetTriggerMethod(int trigger_method, bool with_run = true); - /// @brief Setting mode with PTC_COMMAND_SET_STANDBY_MODE - /// @param target_tcp_driver TcpDriver used - /// @param standby_mode Standby mode - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetStandbyMode( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int standby_mode, - bool with_run = true); - /// @brief Setting mode with PTC_COMMAND_SET_STANDBY_MODE - /// @param ctx IO Context used - /// @param standby_mode Standby mode - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status SetStandbyMode( - std::shared_ptr ctx, int standby_mode, bool with_run = true); + Status SetTriggerMethod(int trigger_method); /// @brief Setting mode with PTC_COMMAND_SET_STANDBY_MODE /// @param standby_mode Standby mode - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status SetStandbyMode(int standby_mode, bool with_run = true); + Status SetStandbyMode(int standby_mode); /// @brief Setting mode with PTC_COMMAND_SET_RETURN_MODE - /// @param target_tcp_driver TcpDriver used /// @param return_mode Return mode - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status SetReturnMode( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int return_mode, - bool with_run = true); - /// @brief Setting mode with PTC_COMMAND_SET_RETURN_MODE - /// @param ctx IO Context used - /// @param return_mode Return mode - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetReturnMode( - std::shared_ptr ctx, int return_mode, bool with_run = true); - /// @brief Setting mode with PTC_COMMAND_SET_RETURN_MODE - /// @param return_mode Return mode - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetReturnMode(int return_mode, bool with_run = true); + Status SetReturnMode(int return_mode); /// @brief Setting IP with PTC_COMMAND_SET_DESTINATION_IP - /// @param target_tcp_driver TcpDriver used /// @param dest_ip_1 The 1st byte represents the 1st section /// @param dest_ip_2 The 2nd byte represents the 2nd section /// @param dest_ip_3 The 3rd byte represents the 3rd section /// @param dest_ip_4 The 4th byte represents the 4th section /// @param port LiDAR Destination Port /// @param gps_port GPS Destination Port - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status Status SetDestinationIp( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int dest_ip_1, - int dest_ip_2, int dest_ip_3, int dest_ip_4, int port, int gps_port, bool with_run = true); - /// @brief Setting IP with PTC_COMMAND_SET_DESTINATION_IP - /// @param ctx IO Context used - /// @param dest_ip_1 The 1st byte represents the 1st section - /// @param dest_ip_2 The 2nd byte represents the 2nd section - /// @param dest_ip_3 The 3rd byte represents the 3rd section - /// @param dest_ip_4 The 4th byte represents the 4th section - /// @param port LiDAR Destination Port - /// @param gps_port GPS Destination Port - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetDestinationIp( - std::shared_ptr ctx, int dest_ip_1, int dest_ip_2, int dest_ip_3, - int dest_ip_4, int port, int gps_port, bool with_run = true); - /// @brief Setting IP with PTC_COMMAND_SET_DESTINATION_IP - /// @param dest_ip_1 The 1st byte represents the 1st section - /// @param dest_ip_2 The 2nd byte represents the 2nd section - /// @param dest_ip_3 The 3rd byte represents the 3rd section - /// @param dest_ip_4 The 4th byte represents the 4th section - /// @param port LiDAR Destination Port - /// @param gps_port GPS Destination Port - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetDestinationIp( - int dest_ip_1, int dest_ip_2, int dest_ip_3, int dest_ip_4, int port, int gps_port, - bool with_run = true); - /// @brief Setting IP with PTC_COMMAND_SET_CONTROL_PORT - /// @param target_tcp_driver TcpDriver used - /// @param ip_1 Device IP of the 1st byte represents the 1st section - /// @param ip_2 Device IP of the 2nd byte represents the 2nd section - /// @param ip_3 Device IP of the 3rd byte represents the 3rd section - /// @param ip_4 Device IP of the 4th byte represents the 4th section - /// @param mask_1 Device subnet mask of the 1st byte represents the 1st section - /// @param mask_2 Device subnet mask of the 2nd byte represents the 2nd section - /// @param mask_3 Device subnet mask of the 3rd byte represents the 3rd section - /// @param mask_4 Device subnet mask of the 4th byte represents the 4th section - /// @param gateway_1 Device gateway of the 1st byte represents the 1st section - /// @param gateway_2 Device gateway of the 2nd byte represents the 2nd section - /// @param gateway_3 Device gateway of the 3rd byte represents the 3rd section - /// @param gateway_4 Device gateway of the 4th byte represents the 4th section - /// @param vlan_flg VLAN Status - /// @param vlan_id VLAN ID - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetControlPort( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int ip_1, int ip_2, - int ip_3, int ip_4, int mask_1, int mask_2, int mask_3, int mask_4, int gateway_1, - int gateway_2, int gateway_3, int gateway_4, int vlan_flg, int vlan_id, bool with_run = true); - /// @brief Setting IP with PTC_COMMAND_SET_CONTROL_PORT - /// @param ctx IO Context used - /// @param ip_1 Device IP of the 1st byte represents the 1st section - /// @param ip_2 Device IP of the 2nd byte represents the 2nd section - /// @param ip_3 Device IP of the 3rd byte represents the 3rd section - /// @param ip_4 Device IP of the 4th byte represents the 4th section - /// @param mask_1 Device subnet mask of the 1st byte represents the 1st section - /// @param mask_2 Device subnet mask of the 2nd byte represents the 2nd section - /// @param mask_3 Device subnet mask of the 3rd byte represents the 3rd section - /// @param mask_4 Device subnet mask of the 4th byte represents the 4th section - /// @param gateway_1 Device gateway of the 1st byte represents the 1st section - /// @param gateway_2 Device gateway of the 2nd byte represents the 2nd section - /// @param gateway_3 Device gateway of the 3rd byte represents the 3rd section - /// @param gateway_4 Device gateway of the 4th byte represents the 4th section - /// @param vlan_flg VLAN Status - /// @param vlan_id VLAN ID - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetControlPort( - std::shared_ptr ctx, int ip_1, int ip_2, int ip_3, int ip_4, - int mask_1, int mask_2, int mask_3, int mask_4, int gateway_1, int gateway_2, int gateway_3, - int gateway_4, int vlan_flg, int vlan_id, bool with_run = true); + 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 /// @param ip_2 Device IP of the 2nd byte represents the 2nd section @@ -641,130 +266,26 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @param gateway_4 Device gateway of the 4th byte represents the 4th section /// @param vlan_flg VLAN Status /// @param vlan_id VLAN ID - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status Status SetControlPort( int ip_1, int ip_2, int ip_3, int ip_4, int mask_1, int mask_2, int mask_3, int mask_4, - int gateway_1, int gateway_2, int gateway_3, int gateway_4, int vlan_flg, int vlan_id, - bool with_run = true); - /// @brief Setting values with PTC_COMMAND_SET_LIDAR_RANGE - /// @param target_tcp_driver TcpDriver used - /// @param method Method - /// @param data Set data - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetLidarRange( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int method, - std::vector data, bool with_run = true); + int gateway_1, int gateway_2, int gateway_3, int gateway_4, int vlan_flg, int vlan_id); /// @brief Setting values with PTC_COMMAND_SET_LIDAR_RANGE - /// @param ctx IO Context used /// @param method Method /// @param data Set data - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status SetLidarRange( - std::shared_ptr ctx, int method, std::vector data, - bool with_run = true); - /// @brief Setting values with PTC_COMMAND_SET_LIDAR_RANGE - /// @param method Method - /// @param data Set data - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetLidarRange(int method, std::vector data, bool with_run = true); - /// @brief Setting values with PTC_COMMAND_SET_LIDAR_RANGE - /// @param target_tcp_driver TcpDriver used - /// @param start Start angle - /// @param end End angle - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetLidarRange( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int start, int end, - bool with_run = true); - /// @brief Setting values with PTC_COMMAND_SET_LIDAR_RANGE - /// @param ctx IO Context used - /// @param start Start angle - /// @param end End angle - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetLidarRange( - std::shared_ptr ctx, int start, int end, bool with_run = true); + Status SetLidarRange(int method, std::vector data); /// @brief Setting values with PTC_COMMAND_SET_LIDAR_RANGE /// @param start Start angle /// @param end End angle - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetLidarRange(int start, int end, bool with_run = true); - /// @brief Getting values with PTC_COMMAND_GET_LIDAR_RANGE - /// @param target_tcp_driver TcpDriver used - /// @param callback Callback function for received HesaiLidarRangeAll - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetLidarRange( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function callback, bool with_run = true); - /// @brief Getting values with PTC_COMMAND_GET_LIDAR_RANGE - /// @param ctx IO Context used - /// @param callback Callback function for received HesaiLidarRangeAll - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status GetLidarRange( - std::shared_ptr ctx, - std::function callback, bool with_run = true); + Status SetLidarRange(int start, int end); /// @brief Getting values with PTC_COMMAND_GET_LIDAR_RANGE - /// @param ctx IO Context used - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status GetLidarRange(std::shared_ptr ctx, bool with_run = true); - /// @brief Getting values with PTC_COMMAND_GET_LIDAR_RANGE - /// @param callback Callback function for received HesaiLidarRangeAll - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetLidarRange( - std::function callback, bool with_run = true); - /// @brief Getting values with PTC_COMMAND_GET_LIDAR_RANGE - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetLidarRange(bool with_run = true); + HesaiLidarRangeAll GetLidarRange(); - Status SetClockSource(std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int clock_source, bool with_run); - Status SetClockSource(std::shared_ptr ctx, int clock_source, bool with_run); - Status SetClockSource(int clock_source, bool with_run = true); + Status SetClockSource(int clock_source); - /// @brief Setting values with PTC_COMMAND_SET_PTP_CONFIG - /// @param target_tcp_driver TcpDriver used - /// @param profile IEEE timing and synchronization standard - /// @param domain Domain attribute of the local clock - /// @param network Network transport type of 1588v2 - /// @param switch_type Switch type of 802.1AS Automotive - /// @param logAnnounceInterval Time interval between Announce messages, in units of log seconds - /// (default: 1) - /// @param logSyncInterval Time interval between Sync messages, in units of log seconds (default: - /// 1) - /// @param logMinDelayReqInterval Minimum permitted mean time between Delay_Req messages, in units - /// of log seconds (default: 0) - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetPtpConfig( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int profile, int domain, - int network, int switch_type, int logAnnounceInterval = 1, int logSyncInterval = 1, int logMinDelayReqInterval = 0, - bool with_run = true); - /// @brief Setting values with PTC_COMMAND_SET_PTP_CONFIG - /// @param ctx IO Context used - /// @param profile IEEE timing and synchronization standard - /// @param domain Domain attribute of the local clock - /// @param network Network transport type of 1588v2 - /// @param switch_type Switch type of 802.1AS Automotive - /// @param logAnnounceInterval Time interval between Announce messages, in units of log seconds - /// (default: 1) - /// @param logSyncInterval Time interval between Sync messages, in units of log seconds (default: - /// 1) - /// @param logMinDelayReqInterval Minimum permitted mean time between Delay_Req messages, in units - /// of log seconds (default: 0) - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetPtpConfig( - std::shared_ptr ctx, int profile, int domain, int network, int switch_type, - int logAnnounceInterval = 1, int logSyncInterval = 1, int logMinDelayReqInterval = 0, bool with_run = true); /// @brief Setting values with PTC_COMMAND_SET_PTP_CONFIG /// @param profile IEEE timing and synchronization standard /// @param domain Domain attribute of the local clock @@ -776,87 +297,23 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// 1) /// @param logMinDelayReqInterval Minimum permitted mean time between Delay_Req messages, in units /// of log seconds (default: 0) - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status Status SetPtpConfig( - int profile, int domain, int network, int switch_type, int logAnnounceInterval = 1, int logSyncInterval = 1, - int logMinDelayReqInterval = 0, bool with_run = true); + int profile, int domain, int network, int switch_type, int logAnnounceInterval = 1, + int logSyncInterval = 1, int logMinDelayReqInterval = 0); /// @brief Getting data with PTC_COMMAND_GET_PTP_CONFIG - /// @param target_tcp_driver TcpDriver used - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetPtpConfig( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_PTP_CONFIG - /// @param ctx IO Context used - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status GetPtpConfig(std::shared_ptr ctx, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_GET_PTP_CONFIG - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetPtpConfig(bool with_run = true); + HesaiPtpConfig GetPtpConfig(); /// @brief Sending command with PTC_COMMAND_RESET - /// @param target_tcp_driver TcpDriver used - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status SendReset( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, bool with_run = true); - /// @brief Sending command with PTC_COMMAND_RESET - /// @param ctx IO Context used - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SendReset(std::shared_ptr ctx, bool with_run = true); - /// @brief Sending command with PTC_COMMAND_RESET - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SendReset(bool with_run = true); - /// @brief Setting values with PTC_COMMAND_SET_ROTATE_DIRECTION - /// @param target_tcp_driver TcpDriver used - /// @param mode Rotation of the motor - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetRotDir( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int mode, - bool with_run = true); + Status SendReset(); /// @brief Setting values with PTC_COMMAND_SET_ROTATE_DIRECTION - /// @param ctx IO Context used /// @param mode Rotation of the motor - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status SetRotDir(std::shared_ptr ctx, int mode, bool with_run = true); - /// @brief Setting values with PTC_COMMAND_SET_ROTATE_DIRECTION - /// @param mode Rotation of the motor - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status SetRotDir(int mode, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_LIDAR_MONITOR - /// @param target_tcp_driver TcpDriver used - /// @param callback Callback function for received HesaiLidarMonitor - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetLidarMonitor( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function callback, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_LIDAR_MONITOR - /// @param ctx IO Context used - /// @param callback Callback function for received HesaiLidarMonitor - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetLidarMonitor( - std::shared_ptr ctx, - std::function callback, bool with_run = true); + Status SetRotDir(int mode); /// @brief Getting data with PTC_COMMAND_LIDAR_MONITOR - /// @param ctx IO Context used - /// @param with_run Automatically executes run() of TcpDriver - /// @return Resulting status - Status GetLidarMonitor(std::shared_ptr ctx, bool with_run = true); - Status GetLidarMonitor( - std::function callback, bool with_run = true); - /// @brief Getting data with PTC_COMMAND_LIDAR_MONITOR - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status - Status GetLidarMonitor(bool with_run = true); + HesaiLidarMonitor GetLidarMonitor(); /// @brief Call run() of IO Context void IOContextRun(); @@ -875,27 +332,15 @@ class HesaiHwInterface : NebulaHwInterfaceBase HesaiStatus SetSpinSpeedAsyncHttp(uint16_t rpm); HesaiStatus SetPtpConfigSyncHttp( - std::shared_ptr ctx, - int profile, - int domain, - int network, - int logAnnounceInterval, - int logSyncInterval, + std::shared_ptr ctx, int profile, int domain, int network, + int logAnnounceInterval, int logSyncInterval, int logMinDelayReqInterval); + HesaiStatus SetPtpConfigSyncHttp( + int profile, int domain, int network, int logAnnounceInterval, int logSyncInterval, int logMinDelayReqInterval); - HesaiStatus SetPtpConfigSyncHttp(int profile, - int domain, - int network, - int logAnnounceInterval, - int logSyncInterval, - int logMinDelayReqInterval); HesaiStatus SetSyncAngleSyncHttp( - std::shared_ptr ctx, - int enable, - int angle); + std::shared_ptr ctx, int enable, int angle); HesaiStatus SetSyncAngleSyncHttp(int enable, int angle); - - /// @brief Getting lidar_monitor via HTTP API /// @param ctx IO Context /// @param str_callback Callback function for received string @@ -926,8 +371,8 @@ class HesaiHwInterface : NebulaHwInterfaceBase HesaiStatus CheckAndSetConfig(); /// @brief Convert to model in Hesai protocol from nebula::drivers::SensorModel - /// @param model - /// @return + /// @param model + /// @return int NebulaModelToHesaiModelNo(nebula::drivers::SensorModel model); /// @brief Set target model number (for proper use of HTTP and TCP according to the support of the diff --git a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp index 1b55fd6e9..ab76f218e 100644 --- a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp @@ -1,6 +1,6 @@ #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" -//#define WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE +// #define WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE #include @@ -26,13 +26,11 @@ HesaiHwInterface::~HesaiHwInterface() #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << ".......................st: HesaiHwInterface::~HesaiHwInterface()" << std::endl; #endif - if(tcp_driver_) - { + if (tcp_driver_) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << ".......................tcp_driver_ is available" << std::endl; #endif - if(tcp_driver_ && tcp_driver_->isOpen()) - { + if (tcp_driver_ && tcp_driver_->isOpen()) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << ".......................st: tcp_driver_->close();" << std::endl; #endif @@ -50,7 +48,9 @@ HesaiHwInterface::~HesaiHwInterface() #endif } -std::shared_ptr> HesaiHwInterface::SendReceive(const uint8_t command_id, const std::vector & payload) { +std::shared_ptr> HesaiHwInterface::SendReceive( + const uint8_t command_id, const std::vector & payload) +{ uint32_t len = payload.size(); std::vector send_buf; @@ -61,40 +61,64 @@ std::shared_ptr> HesaiHwInterface::SendReceive(const uint8_ send_buf.emplace_back((len >> 24) & 0xff); send_buf.emplace_back((len >> 16) & 0xff); send_buf.emplace_back((len >> 8) & 0xff); - send_buf.emplace_back((len >> 0) & 0xff); + send_buf.emplace_back(len & 0xff); send_buf.insert(send_buf.end(), payload.begin(), payload.end()); auto recv_buf = std::make_shared>(); bool success = false; + std::stringstream ss; + ss << "0x" << std::setfill('0') << std::setw(2) << std::hex << static_cast(command_id) << " (" << len << ") "; + std::string log_tag = ss.str(); + + PrintDebug(log_tag + "Entering lock"); + std::timed_mutex tm; tm.lock(); if (tcp_driver_->GetIOContext()->stopped()) { + PrintDebug(log_tag + "IOContext was stopped"); tcp_driver_->GetIOContext()->restart(); } - tcp_driver_->asyncSendReceiveHeaderPayload(send_buf, - [](const std::vector & header_bytes){ + PrintDebug(log_tag + "Sending payload"); + tcp_driver_->asyncSendReceiveHeaderPayload( + send_buf, + [this, log_tag, &success](const std::vector & header_bytes) { + size_t payload_len = (header_bytes[4] << 24) | (header_bytes[5] << 16) | (header_bytes[6] << 8) | header_bytes[7]; + PrintDebug(log_tag + "Received header (expecting " + std::to_string(payload_len) + "B payload)"); + if (payload_len == 0) { success = true; } + }, + [this, log_tag, &recv_buf, &success](const std::vector & payload_bytes) { + PrintDebug(log_tag + "Received payload"); - }, - [&recv_buf, &success](const std::vector & payload_bytes){ - recv_buf->insert(recv_buf->end(), payload_bytes.begin(), payload_bytes.end()); - success = true; - }, - [&tm](){ tm.unlock(); } - ); + // Header had payload length 0 (thus, header callback processed request successfully already), + // but we still received a payload: invalid state + if (success == true) { + throw std::runtime_error("Received payload despite payload length 0 in header"); + } + + // Skip 8 header bytes + recv_buf->insert(recv_buf->end(), std::next(payload_bytes.begin(), 8), payload_bytes.end()); + success = true; + }, + [this, log_tag, &tm]() { + PrintDebug(log_tag + "Unlocking mutex"); + tm.unlock(); + PrintDebug(log_tag + "Unlocked mutex"); + }); this->IOContextRun(); if (!tm.try_lock_for(std::chrono::seconds(1))) { - PrintError("Request did not finish within 1s"); + PrintError(log_tag + "Request did not finish within 1s"); return nullptr; } if (!success) { - PrintError("Did not receive payload"); + PrintError(log_tag + "Did not receive response"); return nullptr; } - + + PrintDebug(log_tag + "Received response"); return recv_buf; } @@ -250,7 +274,10 @@ void HesaiHwInterface::ReceiveCloudPacketCallback(const std::vector & b } } } -Status HesaiHwInterface::CloudInterfaceStop() { return Status::ERROR_1; } +Status HesaiHwInterface::CloudInterfaceStop() +{ + return Status::ERROR_1; +} Status HesaiHwInterface::GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) { @@ -293,11 +320,11 @@ Status HesaiHwInterface::InitializeTcpDriver(bool setup_sensor) return Status::OK; } -Status HesaiHwInterface::FinalizeTcpDriver() { +Status HesaiHwInterface::FinalizeTcpDriver() +{ try { tcp_driver_->close(); - } - catch(std::exception &e) { + } catch (std::exception & e) { PrintError("Error while finalizing the TcpDriver"); return Status::UDP_CONNECTION_ERROR; } @@ -312,1723 +339,540 @@ boost::property_tree::ptree HesaiHwInterface::ParseJson(const std::string & str) } catch (boost::property_tree::json_parser_error & e) { std::cerr << e.what() << std::endl; } - return tree; -} - -Status HesaiHwInterface::syncGetLidarCalibration( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function & bytes)> bytes_callback) -{ - std::vector buf_vec; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_GET_LIDAR_CALIBRATION); - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - // if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max, "GetLidarCalibration")) { - // return GetLidarCalibration(target_tcp_driver, with_run); - // } - PrintDebug("syncGetLidarCalibration: start"); - - target_tcp_driver->syncSendReceiveHeaderPayload( - buf_vec, - [this]([[maybe_unused]]const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - PrintDebug(received_bytes); -#endif - }, - [this, target_tcp_driver, bytes_callback](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - - std::cout << "GetLidarCalib getHeader: "; - for (const auto & b : target_tcp_driver->getHeader()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - std::cout << "GetLidarCalib getPayload: "; - for (const auto & b : target_tcp_driver->getPayload()) { - std::cout << static_cast(b); - } - std::cout << std::endl; -#endif - bytes_callback(received_bytes); - }, - [this]() { }); - - return Status::OK; -} -Status HesaiHwInterface::syncGetLidarCalibration( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function str_callback) -{ - return syncGetLidarCalibration( - target_tcp_driver, [this, str_callback](const std::vector & received_bytes) { - std::string calib_string = - std::string(received_bytes.data(), received_bytes.data() + received_bytes.size()); - PrintInfo(calib_string); - str_callback(calib_string); - }); -} -Status HesaiHwInterface::syncGetLidarCalibration( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver) -{ - return syncGetLidarCalibration( - target_tcp_driver, [this](const std::vector & received_bytes) { - std::string calib_string = - std::string(received_bytes.data(), received_bytes.data() + received_bytes.size()); - PrintInfo(calib_string); - }); -} - -Status HesaiHwInterface::syncGetLidarCalibrationFromSensor( - std::function & received_bytes)> bytes_callback) -{ - return syncGetLidarCalibration(tcp_driver_, bytes_callback); -} -Status HesaiHwInterface::syncGetLidarCalibrationFromSensor( - std::function str_callback) -{ - return syncGetLidarCalibration( - tcp_driver_, [this, str_callback](const std::vector & received_bytes) { - std::string calib_string = - std::string(received_bytes.data(), received_bytes.data() + received_bytes.size()); - str_callback(calib_string); - }); -} -Status HesaiHwInterface::syncGetLidarCalibrationFromSensor() -{ - return syncGetLidarCalibrationFromSensor([this](const std::string & str) { PrintDebug(str); }); -} - -Status HesaiHwInterface::GetLidarCalibration( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function & bytes)> bytes_callback, bool with_run) -{ - std::vector buf_vec; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_GET_LIDAR_CALIBRATION); - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - PrintDebug("GetLidarCalibration: start"); - - target_tcp_driver->asyncSendReceiveHeaderPayload( - buf_vec, - [this]([[maybe_unused]]const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - PrintDebug(received_bytes); -#endif - }, - [this, target_tcp_driver, bytes_callback](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - - std::cout << "GetLidarCalib getHeader: "; - for (const auto & b : target_tcp_driver->getHeader()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - std::cout << "GetLidarCalib getPayload: "; - for (const auto & b : target_tcp_driver->getPayload()) { - std::cout << static_cast(b); - } - std::cout << std::endl; -#endif - bytes_callback(received_bytes); - }, - [](){}); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::GetLidarCalibration: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): GetLidarCalib" << std::endl; -#endif - } - - return Status::WAITING_FOR_SENSOR_RESPONSE; -} -Status HesaiHwInterface::GetLidarCalibration( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function str_callback, bool with_run) -{ - return GetLidarCalibration( - target_tcp_driver, - [this, str_callback](const std::vector & received_bytes) { - std::string calib_string = - std::string(received_bytes.data(), received_bytes.data() + received_bytes.size()); - PrintInfo(calib_string); - str_callback(calib_string); - }, - with_run); -} -Status HesaiHwInterface::GetLidarCalibration( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, bool with_run) -{ - return GetLidarCalibration( - target_tcp_driver, - [this](const std::vector & received_bytes) { - std::string calib_string = - std::string(received_bytes.data(), received_bytes.data() + received_bytes.size()); - PrintInfo(calib_string); - }, - with_run); -} - -Status HesaiHwInterface::GetLidarCalibrationFromSensor( - std::function & received_bytes)> bytes_callback, bool with_run) -{ - if (with_run) { - if (tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } - } - return GetLidarCalibration(tcp_driver_, bytes_callback, with_run); -} -Status HesaiHwInterface::GetLidarCalibrationFromSensor( - std::function str_callback, bool with_run) -{ - if (with_run) { - if (tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } - } - return GetLidarCalibration( - tcp_driver_, - [this, str_callback](const std::vector & received_bytes) { - std::string calib_string = - std::string(received_bytes.data(), received_bytes.data() + received_bytes.size()); - str_callback(calib_string); - }, - with_run); -} -Status HesaiHwInterface::GetLidarCalibrationFromSensor(bool with_run) -{ - return GetLidarCalibrationFromSensor( - [this](const std::string & str) { PrintDebug(str); }, with_run); -} - -Status HesaiHwInterface::GetPtpDiagStatus( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, bool with_run) -{ - std::vector buf_vec; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_PTP_DIAGNOSTICS); // Cmd PTC_COMMAND_PTP_DIAGNOSTICS - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - - buf_vec.emplace_back(PTC_COMMAND_PTP_STATUS); // PTP STATUS - - PrintDebug("GetPtpDiagStatus: start"); - - target_tcp_driver->asyncSendReceiveHeaderPayload( - buf_vec, - [this](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - }, - [this, target_tcp_driver](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - - std::cout << "GetPtpDiagStatus getHeader: "; - for (const auto & b : target_tcp_driver->getHeader()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - std::cout << "GetPtpDiagStatus getPayload: "; - for (const auto & b : target_tcp_driver->getPayload()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - - auto response = target_tcp_driver->getPayload(); - HesaiPtpDiagStatus hesai_ptp_diag_status{}; - if (8 < response.size()) { - int payload_pos = 8; - hesai_ptp_diag_status.master_offset = static_cast(response[payload_pos++]) << 56; - hesai_ptp_diag_status.master_offset = hesai_ptp_diag_status.master_offset | - static_cast(response[payload_pos++]) << 48; - hesai_ptp_diag_status.master_offset = hesai_ptp_diag_status.master_offset | - static_cast(response[payload_pos++]) << 40; - hesai_ptp_diag_status.master_offset = hesai_ptp_diag_status.master_offset | - static_cast(response[payload_pos++]) << 32; - hesai_ptp_diag_status.master_offset = hesai_ptp_diag_status.master_offset | - static_cast(response[payload_pos++]) << 24; - hesai_ptp_diag_status.master_offset = hesai_ptp_diag_status.master_offset | - static_cast(response[payload_pos++]) << 16; - hesai_ptp_diag_status.master_offset = hesai_ptp_diag_status.master_offset | - static_cast(response[payload_pos++]) << 8; - hesai_ptp_diag_status.master_offset = - hesai_ptp_diag_status.master_offset | static_cast(response[payload_pos++]); - hesai_ptp_diag_status.ptp_state = response[payload_pos++] << 24; - hesai_ptp_diag_status.ptp_state = hesai_ptp_diag_status.ptp_state | response[payload_pos++] - << 16; - hesai_ptp_diag_status.ptp_state = hesai_ptp_diag_status.ptp_state | response[payload_pos++] - << 8; - hesai_ptp_diag_status.ptp_state = hesai_ptp_diag_status.ptp_state | response[payload_pos++]; - hesai_ptp_diag_status.elapsed_millisec = response[payload_pos++] << 24; - hesai_ptp_diag_status.elapsed_millisec = - hesai_ptp_diag_status.elapsed_millisec | response[payload_pos++] << 16; - hesai_ptp_diag_status.elapsed_millisec = - hesai_ptp_diag_status.elapsed_millisec | response[payload_pos++] << 8; - hesai_ptp_diag_status.elapsed_millisec = - hesai_ptp_diag_status.elapsed_millisec | response[payload_pos++]; - - std::stringstream ss; - ss << "HesaiHwInterface::GetPtpDiagStatus: " << hesai_ptp_diag_status; - PrintInfo(ss.str()); - } - }, - [this]() { }); - - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::GetPtpDiagStatus: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): GetPtpDiagStatus" << std::endl; -#endif - } - - return Status::WAITING_FOR_SENSOR_RESPONSE; -} - -Status HesaiHwInterface::GetPtpDiagStatus(bool with_run) -{ - if (with_run) { - if (tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } - } - return GetPtpDiagStatus(tcp_driver_, with_run); -} - -Status HesaiHwInterface::GetPtpDiagPort( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, bool with_run) -{ - std::vector buf_vec; - int len = 1; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_PTP_DIAGNOSTICS); // Cmd PTC_COMMAND_PTP_DIAGNOSTICS - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - - buf_vec.emplace_back(PTC_COMMAND_PTP_PORT_DATA_SET); // PTP TLV PORT_DATA_SET - - PrintDebug("GetPtpDiagPort: start"); - - target_tcp_driver->asyncSendReceiveHeaderPayload( - buf_vec, - [this](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - }, - [this, target_tcp_driver](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - - std::cout << "GetPtpDiagPort getHeader: "; - for (const auto & b : target_tcp_driver->getHeader()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - std::cout << "GetPtpDiagPort getPayload: "; - for (const auto & b : target_tcp_driver->getPayload()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - - auto response = target_tcp_driver->getPayload(); - HesaiPtpDiagPort hesai_ptp_diag_port; - if (8 < response.size()) { - int payload_pos = 8; - - for (size_t i = 0; i < hesai_ptp_diag_port.portIdentity.size(); i++) { - hesai_ptp_diag_port.portIdentity[i] = response[payload_pos++]; - } - hesai_ptp_diag_port.portState = static_cast(response[payload_pos++]); - hesai_ptp_diag_port.logMinDelayReqInterval = static_cast(response[payload_pos++]); - hesai_ptp_diag_port.peerMeanPathDelay = static_cast(response[payload_pos++]) - << 56; - hesai_ptp_diag_port.peerMeanPathDelay = - hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) - << 48; - hesai_ptp_diag_port.peerMeanPathDelay = - hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) - << 40; - hesai_ptp_diag_port.peerMeanPathDelay = - hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) - << 32; - hesai_ptp_diag_port.peerMeanPathDelay = - hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) - << 24; - hesai_ptp_diag_port.peerMeanPathDelay = - hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) - << 16; - hesai_ptp_diag_port.peerMeanPathDelay = - hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) - << 8; - hesai_ptp_diag_port.peerMeanPathDelay = - hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]); - hesai_ptp_diag_port.logAnnounceInterval = static_cast(response[payload_pos++]); - hesai_ptp_diag_port.announceReceiptTimeout = static_cast(response[payload_pos++]); - hesai_ptp_diag_port.logSyncInterval = static_cast(response[payload_pos++]); - hesai_ptp_diag_port.delayMechanism = static_cast(response[payload_pos++]); - hesai_ptp_diag_port.logMinPdelayReqInterval = static_cast(response[payload_pos++]); - hesai_ptp_diag_port.versionNumber = static_cast(response[payload_pos++]); - - std::stringstream ss; - ss << "HesaiHwInterface::GetPtpDiagPort: " << hesai_ptp_diag_port; - PrintInfo(ss.str()); - } - }, - [this]() { }); - - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::GetPtpDiagPort: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): GetPtpDiagPort" << std::endl; -#endif - } - - return Status::WAITING_FOR_SENSOR_RESPONSE; -} - -Status HesaiHwInterface::GetPtpDiagPort(bool with_run) -{ - if (with_run) { - if (tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } - } - return GetPtpDiagPort(tcp_driver_, with_run); -} - -Status HesaiHwInterface::GetPtpDiagTime( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, bool with_run) -{ - std::vector buf_vec; - int len = 1; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_PTP_DIAGNOSTICS); // Cmd PTC_COMMAND_PTP_DIAGNOSTICS - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - - buf_vec.emplace_back(PTC_COMMAND_PTP_TIME_STATUS_NP); // PTP TLV TIME_STATUS_NP - - PrintDebug("GetPtpDiagTime: start"); - - target_tcp_driver->asyncSendReceiveHeaderPayload( - buf_vec, - [this](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - }, - [this, target_tcp_driver](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - - std::cout << "GetPtpDiagTime getHeader: "; - for (const auto & b : target_tcp_driver->getHeader()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - std::cout << "GetPtpDiagTime getPayload: "; - for (const auto & b : target_tcp_driver->getPayload()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - - auto response = target_tcp_driver->getPayload(); - HesaiPtpDiagTime hesai_ptp_diag_time; - if (8 < response.size()) { - int payload_pos = 8; - hesai_ptp_diag_time.master_offset = static_cast(response[payload_pos++]) << 56; - hesai_ptp_diag_time.master_offset = - hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 48; - hesai_ptp_diag_time.master_offset = - hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 40; - hesai_ptp_diag_time.master_offset = - hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 32; - hesai_ptp_diag_time.master_offset = - hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 24; - hesai_ptp_diag_time.master_offset = - hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 16; - hesai_ptp_diag_time.master_offset = - hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 8; - hesai_ptp_diag_time.master_offset = - hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]); - hesai_ptp_diag_time.ingress_time = static_cast(response[payload_pos++]) << 56; - hesai_ptp_diag_time.ingress_time = - hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 48; - hesai_ptp_diag_time.ingress_time = - hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 40; - hesai_ptp_diag_time.ingress_time = - hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 32; - hesai_ptp_diag_time.ingress_time = - hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 24; - hesai_ptp_diag_time.ingress_time = - hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 16; - hesai_ptp_diag_time.ingress_time = - hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 8; - hesai_ptp_diag_time.ingress_time = - hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]); - hesai_ptp_diag_time.cumulativeScaledRateOffset = response[payload_pos++] << 24; - hesai_ptp_diag_time.cumulativeScaledRateOffset = - hesai_ptp_diag_time.cumulativeScaledRateOffset | response[payload_pos++] << 16; - hesai_ptp_diag_time.cumulativeScaledRateOffset = - hesai_ptp_diag_time.cumulativeScaledRateOffset | response[payload_pos++] << 8; - hesai_ptp_diag_time.cumulativeScaledRateOffset = - hesai_ptp_diag_time.cumulativeScaledRateOffset | response[payload_pos++]; - hesai_ptp_diag_time.scaledLastGmPhaseChange = response[payload_pos++] << 24; - hesai_ptp_diag_time.scaledLastGmPhaseChange = - hesai_ptp_diag_time.scaledLastGmPhaseChange | response[payload_pos++] << 16; - hesai_ptp_diag_time.scaledLastGmPhaseChange = - hesai_ptp_diag_time.scaledLastGmPhaseChange | response[payload_pos++] << 8; - hesai_ptp_diag_time.scaledLastGmPhaseChange = - hesai_ptp_diag_time.scaledLastGmPhaseChange | response[payload_pos++]; - hesai_ptp_diag_time.gmTimeBaseIndicator = response[payload_pos++] << 8; - hesai_ptp_diag_time.gmTimeBaseIndicator = - hesai_ptp_diag_time.gmTimeBaseIndicator | response[payload_pos++]; - for (size_t i = 0; i < hesai_ptp_diag_time.lastGmPhaseChange.size(); i++) { - hesai_ptp_diag_time.lastGmPhaseChange[i] = response[payload_pos++]; - } - hesai_ptp_diag_time.gmPresent = response[payload_pos++] << 24; - hesai_ptp_diag_time.gmPresent = hesai_ptp_diag_time.gmPresent | response[payload_pos++] - << 16; - hesai_ptp_diag_time.gmPresent = hesai_ptp_diag_time.gmPresent | response[payload_pos++] - << 8; - hesai_ptp_diag_time.gmPresent = hesai_ptp_diag_time.gmPresent | response[payload_pos++]; - hesai_ptp_diag_time.gmIdentity = static_cast(response[payload_pos++]) << 56; - hesai_ptp_diag_time.gmIdentity = - hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 48; - hesai_ptp_diag_time.gmIdentity = - hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 40; - hesai_ptp_diag_time.gmIdentity = - hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 32; - hesai_ptp_diag_time.gmIdentity = - hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 24; - hesai_ptp_diag_time.gmIdentity = - hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 16; - hesai_ptp_diag_time.gmIdentity = - hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 8; - hesai_ptp_diag_time.gmIdentity = - hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]); - - std::stringstream ss; - ss << "HesaiHwInterface::GetPtpDiagTime: " << hesai_ptp_diag_time; - PrintInfo(ss.str()); - } - }, - [this]() { }); - - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::GetPtpDiagTime: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): GetPtpDiagTime" << std::endl; -#endif - } - - return Status::WAITING_FOR_SENSOR_RESPONSE; -} - -Status HesaiHwInterface::GetPtpDiagTime(bool with_run) -{ - if (with_run) { - if (tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } - } - return GetPtpDiagTime(tcp_driver_, with_run); -} - -Status HesaiHwInterface::GetPtpDiagGrandmaster( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, bool with_run) -{ - std::vector buf_vec; - int len = 1; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_PTP_DIAGNOSTICS); // Cmd PTC_COMMAND_PTP_DIAGNOSTICS - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - - buf_vec.emplace_back(PTC_COMMAND_PTP_GRANDMASTER_SETTINGS_NP); // PTP TLV GRANDMASTER_SETTINGS_NP - - PrintDebug("GetPtpDiagGrandmaster: start"); - - target_tcp_driver->asyncSendReceiveHeaderPayload( - buf_vec, - [this](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - }, - [this, target_tcp_driver](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - - std::cout << "GetPtpDiagGrandmaster getHeader: "; - for (const auto & b : target_tcp_driver->getHeader()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - std::cout << "GetPtpDiagGrandmaster getPayload: "; - for (const auto & b : target_tcp_driver->getPayload()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - - auto response = target_tcp_driver->getPayload(); - HesaiPtpDiagGrandmaster hesai_ptp_diag_grandmaster; - if (8 < response.size()) { - int payload_pos = 8; - - hesai_ptp_diag_grandmaster.clockQuality = response[payload_pos++] << 24; - hesai_ptp_diag_grandmaster.clockQuality = - hesai_ptp_diag_grandmaster.clockQuality | response[payload_pos++] << 16; - hesai_ptp_diag_grandmaster.clockQuality = - hesai_ptp_diag_grandmaster.clockQuality | response[payload_pos++] << 8; - hesai_ptp_diag_grandmaster.clockQuality = - hesai_ptp_diag_grandmaster.clockQuality | response[payload_pos++]; - hesai_ptp_diag_grandmaster.utc_offset = response[payload_pos++] << 8; - hesai_ptp_diag_grandmaster.utc_offset = - hesai_ptp_diag_grandmaster.utc_offset | response[payload_pos++]; - hesai_ptp_diag_grandmaster.time_flags = static_cast(response[payload_pos++]); - hesai_ptp_diag_grandmaster.time_source = static_cast(response[payload_pos++]); - - std::cout << hesai_ptp_diag_grandmaster << std::endl; - } - }, - [this]() { }); - - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::GetPtpDiagGrandmaster: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): GetPtpDiagGrandmaster" << std::endl; -#endif - } - - return Status::WAITING_FOR_SENSOR_RESPONSE; -} - -Status HesaiHwInterface::GetPtpDiagGrandmaster(bool with_run) -{ - if (with_run) { - if (tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } - } - return GetPtpDiagGrandmaster(tcp_driver_, with_run); -} - -Status HesaiHwInterface::syncGetInventory( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function callback) -{ - std::cout << "Status HesaiHwInterface::syncGetInventory(std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, std::function callback)" << std::endl; - std::vector buf_vec; - int len = 0; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_GET_INVENTORY_INFO); // Cmd PTC_COMMAND_GET_INVENTORY_INFO - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - - - // It doesn't work even when the sensor is not connected... - if(!target_tcp_driver){ - PrintError("!target_tcp_driver"); - return Status::ERROR_1; - } - - // It doesn't work even when the sensor is not connected... - if(!target_tcp_driver->isOpen()){ - PrintError("!target_tcp_driver->isOpen()"); - return Status::ERROR_1; - } - - PrintDebug("syncGetInventory: start"); - - target_tcp_driver->syncSendReceiveHeaderPayload( - buf_vec, - [this](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - }, - [this, target_tcp_driver, callback]([[maybe_unused]]const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - - std::cout << "syncGetInventory getHeader: "; - for (const auto & b : target_tcp_driver->getHeader()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - std::cout << "syncGetInventory getPayload: "; - for (const auto & b : target_tcp_driver->getPayload()) { - std::cout << static_cast(b); - } - std::cout << std::endl; -#endif - auto response = target_tcp_driver->getPayload(); - HesaiInventory hesai_inventory; - if (8 < response.size()) { - int payload_pos = 8; - for (size_t i = 0; i < hesai_inventory.sn.size(); i++) { - hesai_inventory.sn[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.date_of_manufacture.size(); i++) { - hesai_inventory.date_of_manufacture[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.mac.size(); i++) { - hesai_inventory.mac[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.sw_ver.size(); i++) { - hesai_inventory.sw_ver[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.hw_ver.size(); i++) { - hesai_inventory.hw_ver[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.control_fw_ver.size(); i++) { - hesai_inventory.control_fw_ver[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.sensor_fw_ver.size(); i++) { - hesai_inventory.sensor_fw_ver[i] = response[payload_pos++]; - } - hesai_inventory.angle_offset = response[payload_pos++] << 8; - hesai_inventory.angle_offset = hesai_inventory.angle_offset | response[payload_pos++]; - hesai_inventory.model = static_cast(response[payload_pos++]); - hesai_inventory.motor_type = static_cast(response[payload_pos++]); - hesai_inventory.num_of_lines = static_cast(response[payload_pos++]); - for (size_t i = 0; i < hesai_inventory.reserved.size(); i++) { - hesai_inventory.reserved[i] = static_cast(response[payload_pos++]); - } - callback(hesai_inventory); - } - }, - [this]() { }); - - return Status::OK; -} -Status HesaiHwInterface::syncGetInventory( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver) -{ - return syncGetInventory(target_tcp_driver, - [this](HesaiInventory & result) { std::cout << result << std::endl; }); -} - -Status HesaiHwInterface::syncGetInventory(std::function callback) -{ - return syncGetInventory( - tcp_driver_, [this, callback](HesaiInventory & result) { - callback(result); - }); -} - -Status HesaiHwInterface::GetInventory( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function callback, bool with_run) -{ - std::vector buf_vec; - int len = 0; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_GET_INVENTORY_INFO); // Cmd PTC_COMMAND_GET_INVENTORY_INFO - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - PrintDebug("GetInventory: start"); - - target_tcp_driver->asyncSendReceiveHeaderPayload( - buf_vec, - [this](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - }, - [this, target_tcp_driver, callback](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - - std::cout << "GetInventory getHeader: "; - for (const auto & b : target_tcp_driver->getHeader()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - std::cout << "GetInventory getPayload: "; - for (const auto & b : target_tcp_driver->getPayload()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - - auto response = target_tcp_driver->getPayload(); - HesaiInventory hesai_inventory; - if (8 < response.size()) { - int payload_pos = 8; - for (size_t i = 0; i < hesai_inventory.sn.size(); i++) { - hesai_inventory.sn[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.date_of_manufacture.size(); i++) { - hesai_inventory.date_of_manufacture[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.mac.size(); i++) { - hesai_inventory.mac[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.sw_ver.size(); i++) { - hesai_inventory.sw_ver[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.hw_ver.size(); i++) { - hesai_inventory.hw_ver[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.control_fw_ver.size(); i++) { - hesai_inventory.control_fw_ver[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.sensor_fw_ver.size(); i++) { - hesai_inventory.sensor_fw_ver[i] = response[payload_pos++]; - } - hesai_inventory.angle_offset = response[payload_pos++] << 8; - hesai_inventory.angle_offset = hesai_inventory.angle_offset | response[payload_pos++]; - hesai_inventory.model = static_cast(response[payload_pos++]); - hesai_inventory.motor_type = static_cast(response[payload_pos++]); - hesai_inventory.num_of_lines = static_cast(response[payload_pos++]); - for (size_t i = 0; i < hesai_inventory.reserved.size(); i++) { - hesai_inventory.reserved[i] = static_cast(response[payload_pos++]); - } - callback(hesai_inventory); - } - }, - []() {}); - - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::GetInventory: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): GetInventory" << std::endl; -#endif - } - - return Status::WAITING_FOR_SENSOR_RESPONSE; -} - -Status HesaiHwInterface::GetInventory(std::shared_ptr ctx, bool with_run) -{ - return GetInventory( - ctx, [this](HesaiInventory & result) { std::cout << result << std::endl; }, with_run); -} -Status HesaiHwInterface::GetInventory( - std::function callback, bool with_run) -{ - if (with_run) { - if (tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } - } - return GetInventory(tcp_driver_, callback, with_run); -} -Status HesaiHwInterface::GetInventory(bool with_run) -{ - return GetInventory( - [this](HesaiInventory & result) { std::cout << result << std::endl; }, with_run); -} - -std::shared_ptr HesaiHwInterface::GetConfig() -{ - auto hesai_config_ptr = std::make_shared(); - auto & hesai_config = *hesai_config_ptr; - auto response = *SendReceive(PTC_COMMAND_GET_CONFIG_INFO); - - PrintDebug(response); - - if (8 < response.size()) { - int payload_pos = 8; - hesai_config.ipaddr[0] = static_cast(response[payload_pos++]); - hesai_config.ipaddr[1] = static_cast(response[payload_pos++]); - hesai_config.ipaddr[2] = static_cast(response[payload_pos++]); - hesai_config.ipaddr[3] = static_cast(response[payload_pos++]); - hesai_config.mask[0] = static_cast(response[payload_pos++]); - hesai_config.mask[1] = static_cast(response[payload_pos++]); - hesai_config.mask[2] = static_cast(response[payload_pos++]); - hesai_config.mask[3] = static_cast(response[payload_pos++]); - hesai_config.gateway[0] = static_cast(response[payload_pos++]); - hesai_config.gateway[1] = static_cast(response[payload_pos++]); - hesai_config.gateway[2] = static_cast(response[payload_pos++]); - hesai_config.gateway[3] = static_cast(response[payload_pos++]); - hesai_config.dest_ipaddr[0] = static_cast(response[payload_pos++]); - hesai_config.dest_ipaddr[1] = static_cast(response[payload_pos++]); - hesai_config.dest_ipaddr[2] = static_cast(response[payload_pos++]); - hesai_config.dest_ipaddr[3] = static_cast(response[payload_pos++]); - hesai_config.dest_LiDAR_udp_port = response[payload_pos++] << 8; - hesai_config.dest_LiDAR_udp_port = - hesai_config.dest_LiDAR_udp_port | response[payload_pos++]; - hesai_config.dest_gps_udp_port = response[payload_pos++] << 8; - hesai_config.dest_gps_udp_port = hesai_config.dest_gps_udp_port | response[payload_pos++]; - hesai_config.spin_rate = response[payload_pos++] << 8; - hesai_config.spin_rate = hesai_config.spin_rate | response[payload_pos++]; - hesai_config.sync = static_cast(response[payload_pos++]); - hesai_config.sync_angle = response[payload_pos++] << 8; - hesai_config.sync_angle = hesai_config.sync_angle | response[payload_pos++]; - hesai_config.start_angle = response[payload_pos++] << 8; - hesai_config.start_angle = hesai_config.start_angle | response[payload_pos++]; - hesai_config.stop_angle = response[payload_pos++] << 8; - hesai_config.stop_angle = hesai_config.stop_angle | response[payload_pos++]; - hesai_config.clock_source = static_cast(response[payload_pos++]); - hesai_config.udp_seq = static_cast(response[payload_pos++]); - hesai_config.trigger_method = static_cast(response[payload_pos++]); - hesai_config.return_mode = static_cast(response[payload_pos++]); - hesai_config.standby_mode = static_cast(response[payload_pos++]); - hesai_config.motor_status = static_cast(response[payload_pos++]); - hesai_config.vlan_flag = static_cast(response[payload_pos++]); - hesai_config.vlan_id = response[payload_pos++] << 8; - hesai_config.vlan_id = hesai_config.vlan_id | response[payload_pos++]; - hesai_config.clock_data_fmt = static_cast(response[payload_pos++]); - hesai_config.noise_filtering = static_cast(response[payload_pos++]); - hesai_config.reflectivity_mapping = static_cast(response[payload_pos++]); - hesai_config.reserved[0] = static_cast(response[payload_pos++]); - hesai_config.reserved[1] = static_cast(response[payload_pos++]); - hesai_config.reserved[2] = static_cast(response[payload_pos++]); - hesai_config.reserved[3] = static_cast(response[payload_pos++]); - hesai_config.reserved[4] = static_cast(response[payload_pos++]); - hesai_config.reserved[5] = static_cast(response[payload_pos++]); - } - - std::cout << "Config: " << hesai_config << std::endl; - return hesai_config_ptr; -} - - -Status HesaiHwInterface::GetLidarStatus( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function callback, bool with_run) -{ - std::vector buf_vec; - int len = 0; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_GET_LIDAR_STATUS); // Cmd PTC_COMMAND_GET_LIDAR_STATUS - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - - PrintDebug("GetLidarStatus: start"); - - target_tcp_driver->asyncSendReceiveHeaderPayload( - buf_vec, - [this](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - }, - [this, target_tcp_driver, callback](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - - std::cout << "GetLidarStatus getHeader: "; - for (const auto & b : target_tcp_driver->getHeader()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - std::cout << "GetLidarStatus getPayload: "; - for (const auto & b : target_tcp_driver->getPayload()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - - auto response = target_tcp_driver->getPayload(); - HesaiLidarStatus hesai_status; - if (8 < response.size()) { - int payload_pos = 8; - hesai_status.system_uptime = response[payload_pos++] << 24; - hesai_status.system_uptime = hesai_status.system_uptime | response[payload_pos++] << 16; - hesai_status.system_uptime = hesai_status.system_uptime | response[payload_pos++] << 8; - hesai_status.system_uptime = hesai_status.system_uptime | response[payload_pos++]; - hesai_status.motor_speed = response[payload_pos++] << 8; - hesai_status.motor_speed = hesai_status.motor_speed | response[payload_pos++]; - for (size_t i = 0; i < hesai_status.temperature.size(); i++) { - hesai_status.temperature[i] = response[payload_pos++] << 24; - hesai_status.temperature[i] = hesai_status.temperature[i] | response[payload_pos++] << 16; - hesai_status.temperature[i] = hesai_status.temperature[i] | response[payload_pos++] << 8; - hesai_status.temperature[i] = hesai_status.temperature[i] | response[payload_pos++]; - } - hesai_status.gps_pps_lock = static_cast(response[payload_pos++]); - hesai_status.gps_gprmc_status = static_cast(response[payload_pos++]); - hesai_status.startup_times = response[payload_pos++] << 24; - hesai_status.startup_times = hesai_status.startup_times | response[payload_pos++] << 16; - hesai_status.startup_times = hesai_status.startup_times | response[payload_pos++] << 8; - hesai_status.startup_times = hesai_status.startup_times | response[payload_pos++]; - hesai_status.total_operation_time = response[payload_pos++] << 24; - hesai_status.total_operation_time = - hesai_status.total_operation_time | response[payload_pos++] << 16; - hesai_status.total_operation_time = - hesai_status.total_operation_time | response[payload_pos++] << 8; - hesai_status.total_operation_time = - hesai_status.total_operation_time | response[payload_pos++]; - hesai_status.ptp_clock_status = static_cast(response[payload_pos++]); - for (size_t i = 0; i < hesai_status.reserved.size(); i++) { - hesai_status.reserved[i] = static_cast(response[payload_pos++]); - } - - callback(hesai_status); - } - }, - [this]() { }); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::GetLidarStatus: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): GetLidarStatus" << std::endl; -#endif - } - - return Status::WAITING_FOR_SENSOR_RESPONSE; -} - -Status HesaiHwInterface::GetLidarStatus(std::shared_ptr ctx, bool with_run) -{ - return GetLidarStatus( - ctx, [this](HesaiLidarStatus & result) { std::cout << result << std::endl; }, with_run); -} -Status HesaiHwInterface::GetLidarStatus( - std::function callback, bool with_run) -{ -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "GetLidarStatus tcp_driver_->GetIOContext()->stopped()=" - << tcp_driver_->GetIOContext()->stopped() << std::endl; -#endif - if (with_run) { - if (tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } - } - return GetLidarStatus(tcp_driver_, callback, with_run); -} -Status HesaiHwInterface::GetLidarStatus(bool with_run) -{ - return GetLidarStatus( - [this](HesaiLidarStatus & result) { std::cout << result << std::endl; }, with_run); -} - -Status HesaiHwInterface::SetSpinRate( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, uint16_t rpm, bool with_run) -{ - std::vector buf_vec; - int len = 2; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_SET_SPIN_RATE); // Cmd PTC_COMMAND_SET_SPIN_RATE - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - - buf_vec.emplace_back((rpm >> 8) & 0xff); - buf_vec.emplace_back((rpm >> 0) & 0xff); - - PrintDebug("SetSpinRate: start"); - - target_tcp_driver->asyncSend(buf_vec, [this]() { }); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::SetSpinRate: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): SetSpinRate" << std::endl; -#endif - } - - return Status::WAITING_FOR_SENSOR_RESPONSE; -} - -Status HesaiHwInterface::SetSpinRate(uint16_t rpm, bool with_run) -{ - if (with_run) { - if (tcp_driver_ && tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } - } - return SetSpinRate(tcp_driver_, rpm, with_run); -} - -Status HesaiHwInterface::SetSyncAngle( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int sync_angle, int angle, - bool with_run) -{ - std::vector buf_vec; - int len = 3; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_SET_SYNC_ANGLE); // Cmd PTC_COMMAND_SET_SYNC_ANGLE - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - - buf_vec.emplace_back((sync_angle >> 0) & 0xff); - buf_vec.emplace_back((angle >> 8) & 0xff); - buf_vec.emplace_back((angle >> 0) & 0xff); - - PrintDebug("SetSyncAngle: start"); - -target_tcp_driver->syncSendReceiveHeaderPayload( - buf_vec, - []([[maybe_unused]]const std::vector & received_bytes) {}, - [this, target_tcp_driver]([[maybe_unused]]const std::vector & received_bytes) { - auto response = target_tcp_driver->getPayload(); - }, - [](){}); + return tree; +} - PrintDebug("SetSyncAngle: done"); +std::vector HesaiHwInterface::GetLidarCalibrationBytes() +{ + auto response_ptr = SendReceive(PTC_COMMAND_GET_LIDAR_CALIBRATION); + return std::vector(*response_ptr); +} + +std::string HesaiHwInterface::GetLidarCalibrationString() +{ + auto response_ptr = SendReceive(PTC_COMMAND_GET_LIDAR_CALIBRATION); + std::string calib_string(response_ptr->begin(), response_ptr->end()); + return calib_string; +} + +HesaiPtpDiagStatus HesaiHwInterface::GetPtpDiagStatus() +{ + auto response_ptr = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_STATUS}); + auto & response = *response_ptr; + + HesaiPtpDiagStatus hesai_ptp_diag_status{}; + int payload_pos = 0; + hesai_ptp_diag_status.master_offset = static_cast(response[payload_pos++]) << 56; + hesai_ptp_diag_status.master_offset = + hesai_ptp_diag_status.master_offset | static_cast(response[payload_pos++]) << 48; + hesai_ptp_diag_status.master_offset = + hesai_ptp_diag_status.master_offset | static_cast(response[payload_pos++]) << 40; + hesai_ptp_diag_status.master_offset = + hesai_ptp_diag_status.master_offset | static_cast(response[payload_pos++]) << 32; + hesai_ptp_diag_status.master_offset = + hesai_ptp_diag_status.master_offset | static_cast(response[payload_pos++]) << 24; + hesai_ptp_diag_status.master_offset = + hesai_ptp_diag_status.master_offset | static_cast(response[payload_pos++]) << 16; + hesai_ptp_diag_status.master_offset = + hesai_ptp_diag_status.master_offset | static_cast(response[payload_pos++]) << 8; + hesai_ptp_diag_status.master_offset = + hesai_ptp_diag_status.master_offset | static_cast(response[payload_pos++]); + hesai_ptp_diag_status.ptp_state = response[payload_pos++] << 24; + hesai_ptp_diag_status.ptp_state = hesai_ptp_diag_status.ptp_state | response[payload_pos++] << 16; + hesai_ptp_diag_status.ptp_state = hesai_ptp_diag_status.ptp_state | response[payload_pos++] << 8; + hesai_ptp_diag_status.ptp_state = hesai_ptp_diag_status.ptp_state | response[payload_pos++]; + hesai_ptp_diag_status.elapsed_millisec = response[payload_pos++] << 24; + hesai_ptp_diag_status.elapsed_millisec = + hesai_ptp_diag_status.elapsed_millisec | response[payload_pos++] << 16; + hesai_ptp_diag_status.elapsed_millisec = + hesai_ptp_diag_status.elapsed_millisec | response[payload_pos++] << 8; + hesai_ptp_diag_status.elapsed_millisec = + hesai_ptp_diag_status.elapsed_millisec | response[payload_pos++]; + std::stringstream ss; + ss << "HesaiHwInterface::GetPtpDiagStatus: " << hesai_ptp_diag_status; + PrintInfo(ss.str()); + + return hesai_ptp_diag_status; +} + +HesaiPtpDiagPort HesaiHwInterface::GetPtpDiagPort() +{ + auto response_ptr = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_PORT_DATA_SET}); + auto & response = *response_ptr; + + HesaiPtpDiagPort hesai_ptp_diag_port; + int payload_pos = 0; + + for (size_t i = 0; i < hesai_ptp_diag_port.portIdentity.size(); i++) { + hesai_ptp_diag_port.portIdentity[i] = response[payload_pos++]; + } + hesai_ptp_diag_port.portState = static_cast(response[payload_pos++]); + hesai_ptp_diag_port.logMinDelayReqInterval = static_cast(response[payload_pos++]); + hesai_ptp_diag_port.peerMeanPathDelay = static_cast(response[payload_pos++]) << 56; + hesai_ptp_diag_port.peerMeanPathDelay = + hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) << 48; + hesai_ptp_diag_port.peerMeanPathDelay = + hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) << 40; + hesai_ptp_diag_port.peerMeanPathDelay = + hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) << 32; + hesai_ptp_diag_port.peerMeanPathDelay = + hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) << 24; + hesai_ptp_diag_port.peerMeanPathDelay = + hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) << 16; + hesai_ptp_diag_port.peerMeanPathDelay = + hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) << 8; + hesai_ptp_diag_port.peerMeanPathDelay = + hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]); + hesai_ptp_diag_port.logAnnounceInterval = static_cast(response[payload_pos++]); + hesai_ptp_diag_port.announceReceiptTimeout = static_cast(response[payload_pos++]); + hesai_ptp_diag_port.logSyncInterval = static_cast(response[payload_pos++]); + hesai_ptp_diag_port.delayMechanism = static_cast(response[payload_pos++]); + hesai_ptp_diag_port.logMinPdelayReqInterval = static_cast(response[payload_pos++]); + hesai_ptp_diag_port.versionNumber = static_cast(response[payload_pos++]); - return Status::OK; -} + std::stringstream ss; + ss << "HesaiHwInterface::GetPtpDiagPort: " << hesai_ptp_diag_port; + PrintInfo(ss.str()); + + return hesai_ptp_diag_port; +} + +HesaiPtpDiagTime HesaiHwInterface::GetPtpDiagTime() +{ + auto response_ptr = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_TIME_STATUS_NP}); + auto & response = *response_ptr; + + HesaiPtpDiagTime hesai_ptp_diag_time; + int payload_pos = 0; + hesai_ptp_diag_time.master_offset = static_cast(response[payload_pos++]) << 56; + hesai_ptp_diag_time.master_offset = + hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 48; + hesai_ptp_diag_time.master_offset = + hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 40; + hesai_ptp_diag_time.master_offset = + hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 32; + hesai_ptp_diag_time.master_offset = + hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 24; + hesai_ptp_diag_time.master_offset = + hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 16; + hesai_ptp_diag_time.master_offset = + hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 8; + hesai_ptp_diag_time.master_offset = + hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]); + hesai_ptp_diag_time.ingress_time = static_cast(response[payload_pos++]) << 56; + hesai_ptp_diag_time.ingress_time = + hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 48; + hesai_ptp_diag_time.ingress_time = + hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 40; + hesai_ptp_diag_time.ingress_time = + hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 32; + hesai_ptp_diag_time.ingress_time = + hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 24; + hesai_ptp_diag_time.ingress_time = + hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 16; + hesai_ptp_diag_time.ingress_time = + hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 8; + hesai_ptp_diag_time.ingress_time = + hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]); + hesai_ptp_diag_time.cumulativeScaledRateOffset = response[payload_pos++] << 24; + hesai_ptp_diag_time.cumulativeScaledRateOffset = + hesai_ptp_diag_time.cumulativeScaledRateOffset | response[payload_pos++] << 16; + hesai_ptp_diag_time.cumulativeScaledRateOffset = + hesai_ptp_diag_time.cumulativeScaledRateOffset | response[payload_pos++] << 8; + hesai_ptp_diag_time.cumulativeScaledRateOffset = + hesai_ptp_diag_time.cumulativeScaledRateOffset | response[payload_pos++]; + hesai_ptp_diag_time.scaledLastGmPhaseChange = response[payload_pos++] << 24; + hesai_ptp_diag_time.scaledLastGmPhaseChange = + hesai_ptp_diag_time.scaledLastGmPhaseChange | response[payload_pos++] << 16; + hesai_ptp_diag_time.scaledLastGmPhaseChange = + hesai_ptp_diag_time.scaledLastGmPhaseChange | response[payload_pos++] << 8; + hesai_ptp_diag_time.scaledLastGmPhaseChange = + hesai_ptp_diag_time.scaledLastGmPhaseChange | response[payload_pos++]; + hesai_ptp_diag_time.gmTimeBaseIndicator = response[payload_pos++] << 8; + hesai_ptp_diag_time.gmTimeBaseIndicator = + hesai_ptp_diag_time.gmTimeBaseIndicator | response[payload_pos++]; + for (size_t i = 0; i < hesai_ptp_diag_time.lastGmPhaseChange.size(); i++) { + hesai_ptp_diag_time.lastGmPhaseChange[i] = response[payload_pos++]; + } + hesai_ptp_diag_time.gmPresent = response[payload_pos++] << 24; + hesai_ptp_diag_time.gmPresent = hesai_ptp_diag_time.gmPresent | response[payload_pos++] << 16; + hesai_ptp_diag_time.gmPresent = hesai_ptp_diag_time.gmPresent | response[payload_pos++] << 8; + hesai_ptp_diag_time.gmPresent = hesai_ptp_diag_time.gmPresent | response[payload_pos++]; + hesai_ptp_diag_time.gmIdentity = static_cast(response[payload_pos++]) << 56; + hesai_ptp_diag_time.gmIdentity = + hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 48; + hesai_ptp_diag_time.gmIdentity = + hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 40; + hesai_ptp_diag_time.gmIdentity = + hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 32; + hesai_ptp_diag_time.gmIdentity = + hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 24; + hesai_ptp_diag_time.gmIdentity = + hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 16; + hesai_ptp_diag_time.gmIdentity = + hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 8; + hesai_ptp_diag_time.gmIdentity = + hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]); -Status HesaiHwInterface::SetSyncAngle(int sync_angle, int angle, bool with_run) -{ - if (with_run) { - if (tcp_driver_ && tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } - } - return SetSyncAngle(tcp_driver_, sync_angle, angle, with_run); + std::stringstream ss; + ss << "HesaiHwInterface::GetPtpDiagTime: " << hesai_ptp_diag_time; + PrintInfo(ss.str()); + + return hesai_ptp_diag_time; } -Status HesaiHwInterface::SetTriggerMethod( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int trigger_method, - bool with_run) +HesaiPtpDiagGrandmaster HesaiHwInterface::GetPtpDiagGrandmaster() { - std::vector buf_vec; - int len = 1; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_SET_TRIGGER_METHOD); // Cmd PTC_COMMAND_SET_TRIGGER_METHOD - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); + auto response_ptr = + SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_GRANDMASTER_SETTINGS_NP}); + auto & response = *response_ptr; - buf_vec.emplace_back((trigger_method >> 0) & 0xff); + HesaiPtpDiagGrandmaster hesai_ptp_diag_grandmaster; + int payload_pos = 0; - PrintDebug("SetTriggerMethod: start"); + hesai_ptp_diag_grandmaster.clockQuality = response[payload_pos++] << 24; + hesai_ptp_diag_grandmaster.clockQuality = + hesai_ptp_diag_grandmaster.clockQuality | response[payload_pos++] << 16; + hesai_ptp_diag_grandmaster.clockQuality = + hesai_ptp_diag_grandmaster.clockQuality | response[payload_pos++] << 8; + hesai_ptp_diag_grandmaster.clockQuality = + hesai_ptp_diag_grandmaster.clockQuality | response[payload_pos++]; + hesai_ptp_diag_grandmaster.utc_offset = response[payload_pos++] << 8; + hesai_ptp_diag_grandmaster.utc_offset = + hesai_ptp_diag_grandmaster.utc_offset | response[payload_pos++]; + hesai_ptp_diag_grandmaster.time_flags = static_cast(response[payload_pos++]); + hesai_ptp_diag_grandmaster.time_source = static_cast(response[payload_pos++]); - target_tcp_driver->asyncSend(buf_vec, [this]() { }); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::SetTriggerMethod: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): SetTriggerMethod" << std::endl; -#endif - } + std::stringstream ss; + ss << "HesaiHwInterface::GetPtpDiagGrandmaster: " << hesai_ptp_diag_grandmaster; + PrintInfo(ss.str()); + + return hesai_ptp_diag_grandmaster; +} + +HesaiInventory HesaiHwInterface::GetInventory() +{ + auto response_ptr = SendReceive(PTC_COMMAND_GET_INVENTORY_INFO); + auto & response = *response_ptr; + + HesaiInventory hesai_inventory; + int payload_pos = 0; + for (size_t i = 0; i < hesai_inventory.sn.size(); i++) { + hesai_inventory.sn[i] = response[payload_pos++]; + } + for (size_t i = 0; i < hesai_inventory.date_of_manufacture.size(); i++) { + hesai_inventory.date_of_manufacture[i] = response[payload_pos++]; + } + for (size_t i = 0; i < hesai_inventory.mac.size(); i++) { + hesai_inventory.mac[i] = response[payload_pos++]; + } + for (size_t i = 0; i < hesai_inventory.sw_ver.size(); i++) { + hesai_inventory.sw_ver[i] = response[payload_pos++]; + } + for (size_t i = 0; i < hesai_inventory.hw_ver.size(); i++) { + hesai_inventory.hw_ver[i] = response[payload_pos++]; + } + for (size_t i = 0; i < hesai_inventory.control_fw_ver.size(); i++) { + hesai_inventory.control_fw_ver[i] = response[payload_pos++]; + } + for (size_t i = 0; i < hesai_inventory.sensor_fw_ver.size(); i++) { + hesai_inventory.sensor_fw_ver[i] = response[payload_pos++]; + } + hesai_inventory.angle_offset = response[payload_pos++] << 8; + hesai_inventory.angle_offset = hesai_inventory.angle_offset | response[payload_pos++]; + hesai_inventory.model = static_cast(response[payload_pos++]); + hesai_inventory.motor_type = static_cast(response[payload_pos++]); + hesai_inventory.num_of_lines = static_cast(response[payload_pos++]); + for (size_t i = 0; i < hesai_inventory.reserved.size(); i++) { + hesai_inventory.reserved[i] = static_cast(response[payload_pos++]); + } + + return hesai_inventory; +} + +HesaiConfig HesaiHwInterface::GetConfig() +{ + auto response_ptr = SendReceive(PTC_COMMAND_GET_CONFIG_INFO); + auto & response = *response_ptr; + + HesaiConfig hesai_config{}; + int payload_pos = 0; + hesai_config.ipaddr[0] = static_cast(response[payload_pos++]); + hesai_config.ipaddr[1] = static_cast(response[payload_pos++]); + hesai_config.ipaddr[2] = static_cast(response[payload_pos++]); + hesai_config.ipaddr[3] = static_cast(response[payload_pos++]); + hesai_config.mask[0] = static_cast(response[payload_pos++]); + hesai_config.mask[1] = static_cast(response[payload_pos++]); + hesai_config.mask[2] = static_cast(response[payload_pos++]); + hesai_config.mask[3] = static_cast(response[payload_pos++]); + hesai_config.gateway[0] = static_cast(response[payload_pos++]); + hesai_config.gateway[1] = static_cast(response[payload_pos++]); + hesai_config.gateway[2] = static_cast(response[payload_pos++]); + hesai_config.gateway[3] = static_cast(response[payload_pos++]); + hesai_config.dest_ipaddr[0] = static_cast(response[payload_pos++]); + hesai_config.dest_ipaddr[1] = static_cast(response[payload_pos++]); + hesai_config.dest_ipaddr[2] = static_cast(response[payload_pos++]); + hesai_config.dest_ipaddr[3] = static_cast(response[payload_pos++]); + hesai_config.dest_LiDAR_udp_port = response[payload_pos++] << 8; + hesai_config.dest_LiDAR_udp_port = hesai_config.dest_LiDAR_udp_port | response[payload_pos++]; + hesai_config.dest_gps_udp_port = response[payload_pos++] << 8; + hesai_config.dest_gps_udp_port = hesai_config.dest_gps_udp_port | response[payload_pos++]; + hesai_config.spin_rate = response[payload_pos++] << 8; + hesai_config.spin_rate = hesai_config.spin_rate | response[payload_pos++]; + hesai_config.sync = static_cast(response[payload_pos++]); + hesai_config.sync_angle = response[payload_pos++] << 8; + hesai_config.sync_angle = hesai_config.sync_angle | response[payload_pos++]; + hesai_config.start_angle = response[payload_pos++] << 8; + hesai_config.start_angle = hesai_config.start_angle | response[payload_pos++]; + hesai_config.stop_angle = response[payload_pos++] << 8; + hesai_config.stop_angle = hesai_config.stop_angle | response[payload_pos++]; + hesai_config.clock_source = static_cast(response[payload_pos++]); + hesai_config.udp_seq = static_cast(response[payload_pos++]); + hesai_config.trigger_method = static_cast(response[payload_pos++]); + hesai_config.return_mode = static_cast(response[payload_pos++]); + hesai_config.standby_mode = static_cast(response[payload_pos++]); + hesai_config.motor_status = static_cast(response[payload_pos++]); + hesai_config.vlan_flag = static_cast(response[payload_pos++]); + hesai_config.vlan_id = response[payload_pos++] << 8; + hesai_config.vlan_id = hesai_config.vlan_id | response[payload_pos++]; + hesai_config.clock_data_fmt = static_cast(response[payload_pos++]); + hesai_config.noise_filtering = static_cast(response[payload_pos++]); + hesai_config.reflectivity_mapping = static_cast(response[payload_pos++]); + hesai_config.reserved[0] = static_cast(response[payload_pos++]); + hesai_config.reserved[1] = static_cast(response[payload_pos++]); + hesai_config.reserved[2] = static_cast(response[payload_pos++]); + hesai_config.reserved[3] = static_cast(response[payload_pos++]); + hesai_config.reserved[4] = static_cast(response[payload_pos++]); + hesai_config.reserved[5] = static_cast(response[payload_pos++]); - return Status::WAITING_FOR_SENSOR_RESPONSE; + std::cout << "Config: " << hesai_config << std::endl; + return hesai_config; } -Status HesaiHwInterface::SetTriggerMethod(int trigger_method, bool with_run) +HesaiLidarStatus HesaiHwInterface::GetLidarStatus() { - if (with_run) { - if (tcp_driver_ && tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } + auto response_ptr = SendReceive(PTC_COMMAND_GET_LIDAR_STATUS); + auto & response = *response_ptr; + + HesaiLidarStatus hesai_status; + int payload_pos = 0; + hesai_status.system_uptime = response[payload_pos++] << 24; + hesai_status.system_uptime = hesai_status.system_uptime | response[payload_pos++] << 16; + hesai_status.system_uptime = hesai_status.system_uptime | response[payload_pos++] << 8; + hesai_status.system_uptime = hesai_status.system_uptime | response[payload_pos++]; + hesai_status.motor_speed = response[payload_pos++] << 8; + hesai_status.motor_speed = hesai_status.motor_speed | response[payload_pos++]; + for (size_t i = 0; i < hesai_status.temperature.size(); i++) { + hesai_status.temperature[i] = response[payload_pos++] << 24; + hesai_status.temperature[i] = hesai_status.temperature[i] | response[payload_pos++] << 16; + hesai_status.temperature[i] = hesai_status.temperature[i] | response[payload_pos++] << 8; + hesai_status.temperature[i] = hesai_status.temperature[i] | response[payload_pos++]; } - return SetTriggerMethod(tcp_driver_, trigger_method, with_run); + hesai_status.gps_pps_lock = static_cast(response[payload_pos++]); + hesai_status.gps_gprmc_status = static_cast(response[payload_pos++]); + hesai_status.startup_times = response[payload_pos++] << 24; + hesai_status.startup_times = hesai_status.startup_times | response[payload_pos++] << 16; + hesai_status.startup_times = hesai_status.startup_times | response[payload_pos++] << 8; + hesai_status.startup_times = hesai_status.startup_times | response[payload_pos++]; + hesai_status.total_operation_time = response[payload_pos++] << 24; + hesai_status.total_operation_time = hesai_status.total_operation_time | response[payload_pos++] + << 16; + hesai_status.total_operation_time = hesai_status.total_operation_time | response[payload_pos++] + << 8; + hesai_status.total_operation_time = hesai_status.total_operation_time | response[payload_pos++]; + hesai_status.ptp_clock_status = static_cast(response[payload_pos++]); + for (size_t i = 0; i < hesai_status.reserved.size(); i++) { + hesai_status.reserved[i] = static_cast(response[payload_pos++]); + } + + return hesai_status; } -Status HesaiHwInterface::SetStandbyMode( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int standby_mode, - bool with_run) +Status HesaiHwInterface::SetSpinRate(uint16_t rpm) { - std::vector buf_vec; - int len = 1; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_SET_STANDBY_MODE); // Cmd PTC_COMMAND_SET_STANDBY_MODE - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); + std::vector request_payload; + request_payload.emplace_back((rpm >> 8) & 0xff); + request_payload.emplace_back(rpm & 0xff); - buf_vec.emplace_back((standby_mode >> 0) & 0xff); + auto response_ptr = SendReceive(PTC_COMMAND_SET_SPIN_RATE, request_payload); + auto & response = *response_ptr; + + return Status::OK; +} - std::cout << "start: SetStandbyMode" << std::endl; +Status HesaiHwInterface::SetSyncAngle(int sync_angle, int angle) +{ + std::vector request_payload; + request_payload.emplace_back(sync_angle & 0xff); + request_payload.emplace_back((angle >> 8) & 0xff); + request_payload.emplace_back(angle & 0xff); - target_tcp_driver->asyncSend(buf_vec, [this]() { }); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::SetStandbyMode: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): SetStandbyMode" << std::endl; -#endif - } + auto response_ptr = SendReceive(PTC_COMMAND_SET_SYNC_ANGLE, request_payload); + auto & response = *response_ptr; - return Status::WAITING_FOR_SENSOR_RESPONSE; + return Status::OK; } -Status HesaiHwInterface::SetStandbyMode(int standby_mode, bool with_run) +Status HesaiHwInterface::SetTriggerMethod(int trigger_method) { - if (with_run) { - if (tcp_driver_ && tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } - } - return SetStandbyMode(tcp_driver_, standby_mode, with_run); -} - -Status HesaiHwInterface::SetReturnMode( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int return_mode, - bool with_run) -{ - std::vector buf_vec; - int len = 1; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_SET_RETURN_MODE); // Cmd PTC_COMMAND_SET_RETURN_MODE - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - - buf_vec.emplace_back((return_mode >> 0) & 0xff); - PrintDebug("SetReturnMode: start" + std::to_string(return_mode)); - target_tcp_driver->syncSendReceiveHeaderPayload( - buf_vec, - []([[maybe_unused]]const std::vector & received_bytes) {}, - [this, target_tcp_driver]([[maybe_unused]]const std::vector & received_bytes) { - auto response = target_tcp_driver->getPayload(); - }, - [](){}); - PrintDebug("SetReturnMode: done"); + std::vector request_payload; + request_payload.emplace_back(trigger_method & 0xff); + + auto response_ptr = SendReceive(PTC_COMMAND_SET_TRIGGER_METHOD, request_payload); + auto & response = *response_ptr; + return Status::OK; } -Status HesaiHwInterface::SetReturnMode(int return_mode, bool with_run) +Status HesaiHwInterface::SetStandbyMode(int standby_mode) { - //* - if (with_run) { - if (tcp_driver_ && tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } - } - //*/ - return SetReturnMode(tcp_driver_, return_mode, with_run); -} + std::vector request_payload; + request_payload.emplace_back(standby_mode & 0xff); -Status HesaiHwInterface::SetDestinationIp( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int dest_ip_1, int dest_ip_2, - int dest_ip_3, int dest_ip_4, int port, int gps_port, bool with_run) -{ - std::vector buf_vec; - int len = 8; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_SET_DESTINATION_IP); // Cmd PTC_COMMAND_SET_DESTINATION_IP - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - - buf_vec.emplace_back((dest_ip_1 >> 0) & 0xff); - buf_vec.emplace_back((dest_ip_2 >> 0) & 0xff); - buf_vec.emplace_back((dest_ip_3 >> 0) & 0xff); - buf_vec.emplace_back((dest_ip_4 >> 0) & 0xff); - buf_vec.emplace_back((port >> 8) & 0xff); - buf_vec.emplace_back((port >> 0) & 0xff); - buf_vec.emplace_back((gps_port >> 8) & 0xff); - buf_vec.emplace_back((gps_port >> 0) & 0xff); - - PrintDebug("SetDestinationIp: start"); - -target_tcp_driver->syncSendReceiveHeaderPayload( - buf_vec, - []([[maybe_unused]]const std::vector & received_bytes) {}, - [this, target_tcp_driver]([[maybe_unused]]const std::vector & received_bytes) { - auto response = target_tcp_driver->getPayload(); - }, - [](){}); + auto response_ptr = SendReceive(PTC_COMMAND_SET_STANDBY_MODE, request_payload); + auto & response = *response_ptr; + + return Status::OK; +} - PrintDebug("SetDestinationIp: done"); +Status HesaiHwInterface::SetReturnMode(int return_mode) +{ + std::vector request_payload; + request_payload.emplace_back(return_mode & 0xff); + auto response_ptr = SendReceive(PTC_COMMAND_SET_RETURN_MODE, request_payload); + auto & response = *response_ptr; return Status::OK; } Status HesaiHwInterface::SetDestinationIp( - int dest_ip_1, int dest_ip_2, int dest_ip_3, int dest_ip_4, int port, int gps_port, bool with_run) + int dest_ip_1, int dest_ip_2, int dest_ip_3, int dest_ip_4, int port, int gps_port) { - if (with_run) { - if (tcp_driver_ && tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } - } - return SetDestinationIp( - tcp_driver_, dest_ip_1, dest_ip_2, dest_ip_3, dest_ip_4, port, gps_port, with_run); -} + std::vector request_payload; + request_payload.emplace_back(dest_ip_1 & 0xff); + request_payload.emplace_back(dest_ip_2 & 0xff); + request_payload.emplace_back(dest_ip_3 & 0xff); + request_payload.emplace_back(dest_ip_4 & 0xff); + request_payload.emplace_back((port >> 8) & 0xff); + request_payload.emplace_back(port & 0xff); + request_payload.emplace_back((gps_port >> 8) & 0xff); + request_payload.emplace_back(gps_port & 0xff); -Status HesaiHwInterface::SetControlPort( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int ip_1, int ip_2, int ip_3, - int ip_4, int mask_1, int mask_2, int mask_3, int mask_4, int gateway_1, int gateway_2, - int gateway_3, int gateway_4, int vlan_flg, int vlan_id, bool with_run) -{ - std::vector buf_vec; - int len = 15; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_SET_CONTROL_PORT); // Cmd PTC_COMMAND_SET_CONTROL_PORT - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - - buf_vec.emplace_back((ip_1 >> 0) & 0xff); - buf_vec.emplace_back((ip_2 >> 0) & 0xff); - buf_vec.emplace_back((ip_3 >> 0) & 0xff); - buf_vec.emplace_back((ip_4 >> 0) & 0xff); - buf_vec.emplace_back((mask_1 >> 0) & 0xff); - buf_vec.emplace_back((mask_2 >> 0) & 0xff); - buf_vec.emplace_back((mask_3 >> 0) & 0xff); - buf_vec.emplace_back((mask_4 >> 0) & 0xff); - buf_vec.emplace_back((gateway_1 >> 0) & 0xff); - buf_vec.emplace_back((gateway_2 >> 0) & 0xff); - buf_vec.emplace_back((gateway_3 >> 0) & 0xff); - buf_vec.emplace_back((gateway_4 >> 0) & 0xff); - buf_vec.emplace_back((vlan_flg >> 0) & 0xff); - buf_vec.emplace_back((vlan_id >> 8) & 0xff); - buf_vec.emplace_back((vlan_id >> 0) & 0xff); - - PrintDebug("SetControlPort: start"); - - target_tcp_driver->asyncSend(buf_vec, [this]() { }); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::SetControlPort: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): SetControlPort" << std::endl; -#endif - } + auto response_ptr = SendReceive(PTC_COMMAND_SET_DESTINATION_IP, request_payload); + auto & response = *response_ptr; - return Status::WAITING_FOR_SENSOR_RESPONSE; + return Status::OK; } Status HesaiHwInterface::SetControlPort( int ip_1, int ip_2, int ip_3, int ip_4, int mask_1, int mask_2, int mask_3, int mask_4, - int gateway_1, int gateway_2, int gateway_3, int gateway_4, int vlan_flg, int vlan_id, - bool with_run) -{ - if (with_run) { - if (tcp_driver_ && tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } - } - return SetControlPort( - tcp_driver_, ip_1, ip_2, ip_3, ip_4, mask_1, mask_2, mask_3, mask_4, gateway_1, gateway_2, - gateway_3, gateway_4, vlan_flg, vlan_id, with_run); -} - -Status HesaiHwInterface::SetLidarRange( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int method, - std::vector data, bool with_run) -{ - std::vector buf_vec; - int len = 1 + data.size(); - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_SET_LIDAR_RANGE); // Cmd PTC_COMMAND_SET_LIDAR_RANGE - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); + int gateway_1, int gateway_2, int gateway_3, int gateway_4, int vlan_flg, int vlan_id) +{ + std::vector request_payload; + request_payload.emplace_back(ip_1 & 0xff); + request_payload.emplace_back(ip_2 & 0xff); + request_payload.emplace_back(ip_3 & 0xff); + request_payload.emplace_back(ip_4 & 0xff); + request_payload.emplace_back(mask_1 & 0xff); + request_payload.emplace_back(mask_2 & 0xff); + request_payload.emplace_back(mask_3 & 0xff); + request_payload.emplace_back(mask_4 & 0xff); + request_payload.emplace_back(gateway_1 & 0xff); + request_payload.emplace_back(gateway_2 & 0xff); + request_payload.emplace_back(gateway_3 & 0xff); + request_payload.emplace_back(gateway_4 & 0xff); + request_payload.emplace_back(vlan_flg & 0xff); + request_payload.emplace_back((vlan_id >> 8) & 0xff); + request_payload.emplace_back(vlan_id & 0xff); + + auto response_ptr = SendReceive(PTC_COMMAND_SET_CONTROL_PORT, request_payload); + auto & response = *response_ptr; + return Status::OK; +} + +Status HesaiHwInterface::SetLidarRange(int method, std::vector data) +{ // 0 - for all channels : 5-1 bytes // 1 - for each channel : 323-1 bytes // 2 - multi-section FOV : 1347-1 bytes - buf_vec.emplace_back((method >> 0) & 0xff); - for (int d : data) { - buf_vec.emplace_back(d); - } - - PrintDebug("SetLidarRange: start"); - - target_tcp_driver->asyncSend(buf_vec, [this]() { }); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::SetLidarRange: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): SetLidarRange" << std::endl; -#endif - } + std::vector request_payload; + request_payload.emplace_back(method & 0xff); + request_payload.insert(request_payload.end(), data.begin(), data.end()); - return Status::WAITING_FOR_SENSOR_RESPONSE; -} + auto response_ptr = SendReceive(PTC_COMMAND_SET_LIDAR_RANGE, request_payload); + auto & response = *response_ptr; -Status HesaiHwInterface::SetLidarRange(int method, std::vector data, bool with_run) -{ - if (with_run) { - if (tcp_driver_ && tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } - } - return SetLidarRange(tcp_driver_, method, data, with_run); + return Status::OK; } -Status HesaiHwInterface::SetLidarRange( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int start, int end, - bool with_run) +Status HesaiHwInterface::SetLidarRange(int start, int end) { - std::vector buf_vec; - int len = 5; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_SET_LIDAR_RANGE); // Cmd PTC_COMMAND_SET_LIDAR_RANGE - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - // 0 - for all channels : 5-1 bytes // 1 - for each channel : 323-1 bytes // 2 - multi-section FOV : 1347-1 bytes + std::vector request_payload; int method = 0; - buf_vec.emplace_back((method >> 0) & 0xff); - buf_vec.emplace_back((start >> 8) & 0xff); - buf_vec.emplace_back((start >> 0) & 0xff); - buf_vec.emplace_back((end >> 8) & 0xff); - buf_vec.emplace_back((end >> 0) & 0xff); - - PrintDebug("SetLidarRange(All): start"); - - target_tcp_driver->asyncSend(buf_vec, [this]() { }); - if (with_run) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "start ctx->run(): SetLidarRange(All)" << std::endl; -#endif - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::SetLidarRange(All): " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): SetLidarRange(All)" << std::endl; -#endif - } + request_payload.emplace_back(method & 0xff); + request_payload.emplace_back((start >> 8) & 0xff); + request_payload.emplace_back(start & 0xff); + request_payload.emplace_back((end >> 8) & 0xff); + request_payload.emplace_back(end & 0xff); - return Status::WAITING_FOR_SENSOR_RESPONSE; -} + auto response_ptr = SendReceive(PTC_COMMAND_SET_LIDAR_RANGE, request_payload); + auto & response = *response_ptr; -Status HesaiHwInterface::SetLidarRange(int start, int end, bool with_run) -{ - if (with_run) { - if (tcp_driver_ && tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } - } - return SetLidarRange(tcp_driver_, start, end, with_run); + return Status::OK; } -Status HesaiHwInterface::GetLidarRange( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function callback, bool with_run) +HesaiLidarRangeAll HesaiHwInterface::GetLidarRange() { - std::vector buf_vec; - int len = 0; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_GET_LIDAR_RANGE); // Cmd PTC_COMMAND_GET_LIDAR_RANGE - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - - PrintDebug("GetLidarRange: start"); - - target_tcp_driver->asyncSendReceiveHeaderPayload( - buf_vec, - [this](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - }, - [this, target_tcp_driver, callback](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - - std::cout << "GetLidarRange getHeader: "; - for (const auto & b : target_tcp_driver->getHeader()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - std::cout << "GetLidarRange getPayload: "; - for (const auto & b : target_tcp_driver->getPayload()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); + auto response_ptr = SendReceive(PTC_COMMAND_GET_LIDAR_RANGE); + auto & response = *response_ptr; - auto response = target_tcp_driver->getPayload(); - if (8 < response.size()) { - int payload_pos = 8; - int method = static_cast(response[payload_pos++]); -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "GetLidarRange method: " << method << std::endl; -#endif - if (method == 0) // for all channels - { - HesaiLidarRangeAll hesai_range_all; - hesai_range_all.method = method; - hesai_range_all.start = response[payload_pos++] << 8; - hesai_range_all.start = hesai_range_all.start | response[payload_pos++]; - hesai_range_all.end = response[payload_pos++] << 8; - hesai_range_all.end = hesai_range_all.end | response[payload_pos++]; - callback(hesai_range_all); - } else if (method == 1) // for each channel - { - HesaiLidarRangeAll hesai_range_all; - hesai_range_all.method = method; - callback(hesai_range_all); - } else if (method == 2) // multi-section FOV - { - HesaiLidarRangeAll hesai_range_all; - hesai_range_all.method = method; - callback(hesai_range_all); - } - } - }, - [this]() { }); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::GetLidarRange: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): GetLidarRange" << std::endl; -#endif + HesaiLidarRangeAll hesai_range_all; + int payload_pos = 0; + int method = static_cast(response[payload_pos++]); + switch (method) { + case 0: // for all channels + hesai_range_all.method = method; + hesai_range_all.start = response[payload_pos++] << 8; + hesai_range_all.start = hesai_range_all.start | response[payload_pos++]; + hesai_range_all.end = response[payload_pos++] << 8; + hesai_range_all.end = hesai_range_all.end | response[payload_pos++]; + break; + case 1: // for each channel + hesai_range_all.method = method; + break; + case 2: // multi-section FOV + hesai_range_all.method = method; + break; } - return Status::WAITING_FOR_SENSOR_RESPONSE; -} - -Status HesaiHwInterface::GetLidarRange(std::shared_ptr ctx, bool with_run) -{ - return GetLidarRange( - ctx, [this](HesaiLidarRangeAll & result) { std::cout << result << std::endl; }, with_run); -} -Status HesaiHwInterface::GetLidarRange( - std::function callback, bool with_run) -{ - if (with_run) { - if (tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } - } - return GetLidarRange(tcp_driver_, callback, with_run); -} -Status HesaiHwInterface::GetLidarRange(bool with_run) -{ - return GetLidarRange( - [this](HesaiLidarRangeAll & result) { std::cout << result << std::endl; }, with_run); + return hesai_range_all; } -Status HesaiHwInterface::SetClockSource( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int clock_source, - bool with_run) +Status HesaiHwInterface::SetClockSource(int clock_source) { - std::vector buf_vec; - int len = 1; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_SET_CLOCK_SOURCE); - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - - buf_vec.emplace_back((clock_source >> 0) & 0xff); - - PrintDebug("SetClockSource: start"); - - target_tcp_driver->asyncSend(buf_vec, [this]() { }); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::SetClockSource: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): SetReturnMode" << std::endl; -#endif - } + std::vector request_payload; + request_payload.emplace_back(clock_source & 0xff); - return Status::WAITING_FOR_SENSOR_RESPONSE; -} + auto response_ptr = SendReceive(PTC_COMMAND_SET_CLOCK_SOURCE, request_payload); + auto & response = *response_ptr; -Status HesaiHwInterface::SetClockSource(int clock_source, bool with_run) -{ - //* - if (with_run) { - if (tcp_driver_ && tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } - } - //*/ - return SetClockSource(tcp_driver_, clock_source, with_run); + return Status::OK; } Status HesaiHwInterface::SetPtpConfig( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int profile, int domain, - int network, int switch_type, int logAnnounceInterval, int logSyncInterval, - int logMinDelayReqInterval, bool with_run) + int profile, int domain, int network, int switch_type, int logAnnounceInterval, + int logSyncInterval, int logMinDelayReqInterval) { if (profile < 0 || profile > 3) { return Status::ERROR_1; } + // Handle the OT128 differently - it has TSN settings and defines the PTP profile // for automotive as 0x03 instead of 0x02 for other sensors. if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR128_E4X) { @@ -2038,373 +882,109 @@ Status HesaiHwInterface::SetPtpConfig( profile = 3; } - std::vector buf_vec; - int len = 3; - switch (profile) { - case 0: - len = 6; - break; - case 3: - len = 4; - break; - default: - len = 3; - } - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_SET_PTP_CONFIG); // Cmd PTC_COMMAND_SET_PTP_CONFIG - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - buf_vec.emplace_back((profile >> 0) & 0xff); - buf_vec.emplace_back((domain >> 0) & 0xff); - buf_vec.emplace_back((network >> 0) & 0xff); + std::vector request_payload; + request_payload.emplace_back(profile & 0xff); + request_payload.emplace_back(domain & 0xff); + request_payload.emplace_back(network & 0xff); if (profile == 0) { - buf_vec.emplace_back((logAnnounceInterval >> 0) & 0xff); - buf_vec.emplace_back((logSyncInterval >> 0) & 0xff); - buf_vec.emplace_back((logMinDelayReqInterval >> 0) & 0xff); + request_payload.emplace_back(logAnnounceInterval & 0xff); + request_payload.emplace_back(logSyncInterval & 0xff); + request_payload.emplace_back(logMinDelayReqInterval & 0xff); + } else if (profile == 3) { + request_payload.emplace_back(switch_type & 0xff); } - else if (profile == 3) { - buf_vec.emplace_back((switch_type >> 0) & 0xff); - } - - PrintDebug("SetPtpConfig: start"); - - target_tcp_driver->syncSendReceiveHeaderPayload( - buf_vec, - []([[maybe_unused]]const std::vector & received_bytes) {}, - [this, target_tcp_driver]([[maybe_unused]]const std::vector & received_bytes) { - auto response = target_tcp_driver->getPayload(); - PrintDebug("SetPtpConfig: recv'd"); - - }, - [](){}); - PrintDebug("SetPtpConfig: done"); + auto response_ptr = SendReceive(PTC_COMMAND_SET_PTP_CONFIG, request_payload); + auto & response = *response_ptr; return Status::OK; } -Status HesaiHwInterface::SetPtpConfig( - std::shared_ptr ctx, int profile, int domain, int network, - int switch_type, int logAnnounceInterval, int logSyncInterval, - int logMinDelayReqInterval, bool with_run) -{ - auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); - tcp_driver_local->init_socket( - sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, - PandarTcpCommandPort); - return SetPtpConfig( - tcp_driver_local, profile, domain, network, switch_type, logAnnounceInterval, - logSyncInterval, logMinDelayReqInterval, with_run); -} -Status HesaiHwInterface::SetPtpConfig( - int profile, int domain, int network, int switch_type, int logAnnounceInterval, - int logSyncInterval, int logMinDelayReqInterval, bool with_run) -{ - if (with_run) { - if (tcp_driver_ && tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } - } - return SetPtpConfig( - tcp_driver_, profile, domain, network, switch_type, logAnnounceInterval, - logSyncInterval, logMinDelayReqInterval, with_run); -} - -Status HesaiHwInterface::GetPtpConfig( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, bool with_run) -{ - std::vector buf_vec; - int len = 0; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_GET_PTP_CONFIG); // Cmd PTC_COMMAND_GET_PTP_CONFIG - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - - PrintDebug("GetPtpConfig: start"); - - target_tcp_driver->asyncSendReceiveHeaderPayload( - buf_vec, - [this](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - }, - [this, target_tcp_driver](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - std::cout << "GetPtpConfig getHeader: "; - for (const auto & b : target_tcp_driver->getHeader()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - std::cout << "GetPtpConfig getPayload: "; - for (const auto & b : target_tcp_driver->getPayload()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - - auto response = target_tcp_driver->getPayload(); - HesaiPtpConfig hesai_ptp_config{}; - if (8 < response.size()) { - int payload_pos = 8; - hesai_ptp_config.status = static_cast(response[payload_pos++]); - hesai_ptp_config.profile = static_cast(response[payload_pos++]); - hesai_ptp_config.domain = static_cast(response[payload_pos++]); - hesai_ptp_config.network = static_cast(response[payload_pos++]); - if (hesai_ptp_config.status == 0) { - hesai_ptp_config.logAnnounceInterval = static_cast(response[payload_pos++]); - hesai_ptp_config.logSyncInterval = static_cast(response[payload_pos++]); - hesai_ptp_config.logMinDelayReqInterval = static_cast(response[payload_pos++]); - } - - std::stringstream ss; - ss << "HesaiHwInterface::GetPtpConfig: " << hesai_ptp_config; - PrintInfo(ss.str()); - } - }, - [this]() { }); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::GetPtpConfig: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): GetPtpConfig" << std::endl; -#endif - } +HesaiPtpConfig HesaiHwInterface::GetPtpConfig() +{ + auto response_ptr = SendReceive(PTC_COMMAND_GET_PTP_CONFIG); + auto & response = *response_ptr; - return Status::WAITING_FOR_SENSOR_RESPONSE; -} + HesaiPtpConfig hesai_ptp_config{}; -Status HesaiHwInterface::GetPtpConfig(bool with_run) -{ - if (with_run) { - if (tcp_driver_ && tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } - } - return GetPtpConfig(tcp_driver_, with_run); -} - -Status HesaiHwInterface::SendReset( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, bool with_run) -{ - std::vector buf_vec; - int len = 0; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_RESET); // Cmd PTC_COMMAND_RESET - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - - PrintDebug("SendReset: start"); - - target_tcp_driver->asyncSend(buf_vec, [this]() { }); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::SendReset: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): SendReset" << std::endl; -#endif + int payload_pos = 0; + hesai_ptp_config.status = static_cast(response[payload_pos++]); + hesai_ptp_config.profile = static_cast(response[payload_pos++]); + hesai_ptp_config.domain = static_cast(response[payload_pos++]); + hesai_ptp_config.network = static_cast(response[payload_pos++]); + if (hesai_ptp_config.status == 0) { + hesai_ptp_config.logAnnounceInterval = static_cast(response[payload_pos++]); + hesai_ptp_config.logSyncInterval = static_cast(response[payload_pos++]); + hesai_ptp_config.logMinDelayReqInterval = static_cast(response[payload_pos++]); } - return Status::WAITING_FOR_SENSOR_RESPONSE; -} + std::stringstream ss; + ss << "HesaiHwInterface::GetPtpConfig: " << hesai_ptp_config; + PrintInfo(ss.str()); -Status HesaiHwInterface::SendReset(bool with_run) -{ - //* - if (with_run) { - if (tcp_driver_ && tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } - } - //*/ - return SendReset(tcp_driver_, with_run); + return hesai_ptp_config; } -Status HesaiHwInterface::SetRotDir( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int mode, bool with_run) +Status HesaiHwInterface::SendReset() { - std::vector buf_vec; - int len = 1; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_SET_ROTATE_DIRECTION); // Cmd PTC_COMMAND_SET_ROTATE_DIRECTION - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - - buf_vec.emplace_back((mode >> 0) & 0xff); - - PrintDebug("SetRotDir: start"); - - target_tcp_driver->asyncSend(buf_vec, [this]() { }); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::SetRotDir: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): SetRotDir" << std::endl; -#endif - } + auto response_ptr = SendReceive(PTC_COMMAND_RESET); + auto & response = *response_ptr; - return Status::WAITING_FOR_SENSOR_RESPONSE; + return Status::OK; } -Status HesaiHwInterface::SetRotDir(int mode, bool with_run) +Status HesaiHwInterface::SetRotDir(int mode) { - if (with_run) { - if (tcp_driver_ && tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } - } - return SetRotDir(tcp_driver_, mode, with_run); + std::vector request_payload; + request_payload.emplace_back(mode & 0xff); + + auto response_ptr = SendReceive(PTC_COMMAND_SET_ROTATE_DIRECTION, request_payload); + auto & response = *response_ptr; + + return Status::OK; } -Status HesaiHwInterface::GetLidarMonitor( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, - std::function callback, bool with_run) +HesaiLidarMonitor HesaiHwInterface::GetLidarMonitor() { - std::vector buf_vec; - int len = 0; - buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); - buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); - buf_vec.emplace_back(PTC_COMMAND_LIDAR_MONITOR); // Cmd PTC_COMMAND_LIDAR_MONITOR - buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); - buf_vec.emplace_back((len >> 24) & 0xff); - buf_vec.emplace_back((len >> 16) & 0xff); - buf_vec.emplace_back((len >> 8) & 0xff); - buf_vec.emplace_back((len >> 0) & 0xff); - - PrintDebug("GetLidarMonitor: start"); + auto response_ptr = SendReceive(PTC_COMMAND_LIDAR_MONITOR); + auto & response = *response_ptr; - target_tcp_driver->asyncSendReceiveHeaderPayload( - buf_vec, - [this](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - }, - [this, target_tcp_driver, callback](const std::vector & received_bytes) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - for (const auto & b : received_bytes) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; + HesaiLidarMonitor hesai_lidar_monitor; + int payload_pos = 0; + hesai_lidar_monitor.input_voltage = response[payload_pos++] << 24; + hesai_lidar_monitor.input_voltage = hesai_lidar_monitor.input_voltage | response[payload_pos++] + << 16; + hesai_lidar_monitor.input_voltage = hesai_lidar_monitor.input_voltage | response[payload_pos++] + << 8; + hesai_lidar_monitor.input_voltage = hesai_lidar_monitor.input_voltage | response[payload_pos++]; + hesai_lidar_monitor.input_current = response[payload_pos++] << 24; + hesai_lidar_monitor.input_current = hesai_lidar_monitor.input_current | response[payload_pos++] + << 16; + hesai_lidar_monitor.input_current = hesai_lidar_monitor.input_current | response[payload_pos++] + << 8; + hesai_lidar_monitor.input_current = hesai_lidar_monitor.input_current | response[payload_pos++]; + hesai_lidar_monitor.input_power = response[payload_pos++] << 24; + hesai_lidar_monitor.input_power = hesai_lidar_monitor.input_power | response[payload_pos++] << 16; + hesai_lidar_monitor.input_power = hesai_lidar_monitor.input_power | response[payload_pos++] << 8; + hesai_lidar_monitor.input_power = hesai_lidar_monitor.input_power | response[payload_pos++]; - std::cout << "GetLidarMonitor getHeader: "; - for (const auto & b : target_tcp_driver->getHeader()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; - std::cout << "GetLidarMonitor getPayload: "; - for (const auto & b : target_tcp_driver->getPayload()) { - std::cout << static_cast(b) << ", "; - } - std::cout << std::endl; -#endif - PrintDebug(received_bytes); - - auto response = target_tcp_driver->getPayload(); - HesaiLidarMonitor hesai_lidar_monitor; - if (8 < response.size()) { - int payload_pos = 8; - hesai_lidar_monitor.input_voltage = response[payload_pos++] << 24; - hesai_lidar_monitor.input_voltage = - hesai_lidar_monitor.input_voltage | response[payload_pos++] << 16; - hesai_lidar_monitor.input_voltage = - hesai_lidar_monitor.input_voltage | response[payload_pos++] << 8; - hesai_lidar_monitor.input_voltage = - hesai_lidar_monitor.input_voltage | response[payload_pos++]; - hesai_lidar_monitor.input_current = response[payload_pos++] << 24; - hesai_lidar_monitor.input_current = - hesai_lidar_monitor.input_current | response[payload_pos++] << 16; - hesai_lidar_monitor.input_current = - hesai_lidar_monitor.input_current | response[payload_pos++] << 8; - hesai_lidar_monitor.input_current = - hesai_lidar_monitor.input_current | response[payload_pos++]; - hesai_lidar_monitor.input_power = response[payload_pos++] << 24; - hesai_lidar_monitor.input_power = hesai_lidar_monitor.input_power | response[payload_pos++] - << 16; - hesai_lidar_monitor.input_power = hesai_lidar_monitor.input_power | response[payload_pos++] - << 8; - hesai_lidar_monitor.input_power = hesai_lidar_monitor.input_power | response[payload_pos++]; - - for (size_t i = 0; i < hesai_lidar_monitor.reserved.size(); i++) { - hesai_lidar_monitor.reserved[i] = static_cast(response[payload_pos++]); - } - callback(hesai_lidar_monitor); - } - }, - [this]() { }); - if (with_run) { - boost::system::error_code ec = target_tcp_driver->run(); - if (ec) { - PrintError("HesaiHwInterface::GetLidarMonitor: " + ec.message()); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ctx->run(): GetLidarMonitor" << std::endl; -#endif + for (size_t i = 0; i < hesai_lidar_monitor.reserved.size(); i++) { + hesai_lidar_monitor.reserved[i] = static_cast(response[payload_pos++]); } - return Status::WAITING_FOR_SENSOR_RESPONSE; -} -Status HesaiHwInterface::GetLidarMonitor( - std::shared_ptr ctx, bool with_run) -{ - return GetLidarMonitor( - ctx, [this](HesaiLidarMonitor & result) { std::cout << result << std::endl; }, with_run); + return hesai_lidar_monitor; } -Status HesaiHwInterface::GetLidarMonitor( - std::function callback, bool with_run) + +void HesaiHwInterface::IOContextRun() { - if (with_run) { - if (tcp_driver_->GetIOContext()->stopped()) { - tcp_driver_->GetIOContext()->restart(); - } - } - return GetLidarMonitor(tcp_driver_, callback, with_run); + m_owned_ctx->run(); } -Status HesaiHwInterface::GetLidarMonitor(bool with_run) + +std::shared_ptr HesaiHwInterface::GetIOContext() { - return GetLidarMonitor( - [this](HesaiLidarMonitor & result) { std::cout << result << std::endl; }, with_run); + return m_owned_ctx; } -void HesaiHwInterface::IOContextRun() { m_owned_ctx->run(); } - -std::shared_ptr HesaiHwInterface::GetIOContext() { return m_owned_ctx; } - HesaiStatus HesaiHwInterface::GetHttpClientDriverOnce( std::shared_ptr ctx, std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> & hcd) @@ -2433,7 +1013,10 @@ HesaiStatus HesaiHwInterface::GetHttpClientDriverOnce( return st; } -void HesaiHwInterface::str_cb(const std::string & str) { PrintInfo(str); } +void HesaiHwInterface::str_cb(const std::string & str) +{ + PrintInfo(str); +} HesaiStatus HesaiHwInterface::SetSpinSpeedAsyncHttp( std::shared_ptr ctx, uint16_t rpm) @@ -2472,13 +1055,8 @@ HesaiStatus HesaiHwInterface::SetSpinSpeedAsyncHttp(uint16_t rpm) } HesaiStatus HesaiHwInterface::SetPtpConfigSyncHttp( - std::shared_ptr ctx, - int profile, - int domain, - int network, - int logAnnounceInterval, - int logSyncInterval, - int logMinDelayReqInterval) + std::shared_ptr ctx, int profile, int domain, int network, + int logAnnounceInterval, int logSyncInterval, int logMinDelayReqInterval) { std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; auto st = GetHttpClientDriverOnce(ctx, hcd); @@ -2486,53 +1064,47 @@ HesaiStatus HesaiHwInterface::SetPtpConfigSyncHttp( return st; } - auto response = hcd->get( - (boost::format("/pandar.cgi?action=set&object=lidar&key=ptp_configuration&value={" \ - "\"Profile\": %d," \ - "\"Domain\": %d," \ - "\"Network\": %d," \ - "\"LogAnnounceInterval\": %d," \ - "\"LogSyncInterval\": %d," \ - "\"LogMinDelayReqInterval\": %d," \ - "\"tsn_switch\": %d" \ - "}") - % profile % domain % network % logAnnounceInterval % logSyncInterval % logMinDelayReqInterval % 0 - ).str()); + auto response = + hcd->get((boost::format("/pandar.cgi?action=set&object=lidar&key=ptp_configuration&value={" + "\"Profile\": %d," + "\"Domain\": %d," + "\"Network\": %d," + "\"LogAnnounceInterval\": %d," + "\"LogSyncInterval\": %d," + "\"LogMinDelayReqInterval\": %d," + "\"tsn_switch\": %d" + "}") % + profile % domain % network % logAnnounceInterval % logSyncInterval % + logMinDelayReqInterval % 0) + .str()); ctx->run(); PrintInfo(response); return Status::OK; } -HesaiStatus HesaiHwInterface::SetPtpConfigSyncHttp(int profile, - int domain, - int network, - int logAnnounceInterval, - int logSyncInterval, +HesaiStatus HesaiHwInterface::SetPtpConfigSyncHttp( + int profile, int domain, int network, int logAnnounceInterval, int logSyncInterval, int logMinDelayReqInterval) { - return SetPtpConfigSyncHttp(std::make_shared(), - profile, - domain, - network, - logAnnounceInterval, - logSyncInterval, - logMinDelayReqInterval); + return SetPtpConfigSyncHttp( + std::make_shared(), profile, domain, network, logAnnounceInterval, + logSyncInterval, logMinDelayReqInterval); } HesaiStatus HesaiHwInterface::SetSyncAngleSyncHttp( - std::shared_ptr ctx, - int enable, - int angle) + std::shared_ptr ctx, int enable, int angle) { std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; auto st = GetHttpClientDriverOnce(ctx, hcd); if (st != Status::OK) { return st; } - auto tmp_str = (boost::format("/pandar.cgi?action=set&object=lidar_sync&key=sync_angle&value={" \ - "\"sync\": %d," \ - "\"syncAngle\": %d" \ - "}") % enable % angle).str(); + auto tmp_str = (boost::format("/pandar.cgi?action=set&object=lidar_sync&key=sync_angle&value={" + "\"sync\": %d," + "\"syncAngle\": %d" + "}") % + enable % angle) + .str(); PrintInfo(tmp_str); auto response = hcd->get(tmp_str); ctx->run(); @@ -2542,9 +1114,7 @@ HesaiStatus HesaiHwInterface::SetSyncAngleSyncHttp( HesaiStatus HesaiHwInterface::SetSyncAngleSyncHttp(int enable, int angle) { - return SetSyncAngleSyncHttp(std::make_shared(), - enable, - angle); + return SetSyncAngleSyncHttp(std::make_shared(), enable, angle); } HesaiStatus HesaiHwInterface::GetLidarMonitorAsyncHttp( @@ -2584,7 +1154,8 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( #endif auto current_return_mode = nebula::drivers::ReturnModeFromIntHesai( hesai_config.return_mode, sensor_configuration->sensor_model); - auto wait_time = 100ms; // Avoids spamming the sensor, which leads to failure when configuring it. + auto wait_time = + 100ms; // Avoids spamming the sensor, which leads to failure when configuring it. if (sensor_configuration->return_mode != current_return_mode) { std::stringstream ss; ss << current_return_mode; @@ -2596,7 +1167,9 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( auto return_mode_int = nebula::drivers::IntFromReturnModeHesai( sensor_configuration->return_mode, sensor_configuration->sensor_model); if (return_mode_int < 0) { - PrintError("Invalid Return Mode for this sensor. Please check your settings. Falling back to Dual mode."); + PrintError( + "Invalid Return Mode for this sensor. Please check your settings. Falling back to Dual " + "mode."); return_mode_int = 2; } SetReturnMode(return_mode_int); @@ -2614,7 +1187,8 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( if (UseHttpSetSpinRate()) { SetSpinSpeedAsyncHttp(sensor_configuration->rotation_speed); } else { - PrintInfo("Setting up spin rate via TCP." + std::to_string(sensor_configuration->rotation_speed) ); + PrintInfo( + "Setting up spin rate via TCP." + std::to_string(sensor_configuration->rotation_speed)); std::thread t( [this, sensor_configuration] { SetSpinRate(sensor_configuration->rotation_speed); }); t.join(); @@ -2662,8 +1236,9 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( std::this_thread::sleep_for(wait_time); } - if (sensor_configuration->sensor_model != SensorModel::HESAI_PANDARAT128 - && sensor_configuration->sensor_model != SensorModel::HESAI_PANDARQT128) { + if ( + sensor_configuration->sensor_model != SensorModel::HESAI_PANDARAT128 && + sensor_configuration->sensor_model != SensorModel::HESAI_PANDARQT128) { set_flg = true; auto sync_angle = static_cast(hesai_config.sync_angle / 100); auto scan_phase = static_cast(sensor_configuration->scan_phase); @@ -2675,19 +1250,18 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( PrintInfo("current lidar sync: " + std::to_string(hesai_config.sync)); PrintInfo("current lidar sync_angle: " + std::to_string(sync_angle)); PrintInfo("current configuration scan_phase: " + std::to_string(scan_phase)); - std::thread t([this, sync_flg, scan_phase] { - SetSyncAngle(sync_flg, scan_phase); - }); + std::thread t([this, sync_flg, scan_phase] { SetSyncAngle(sync_flg, scan_phase); }); t.join(); std::this_thread::sleep_for(wait_time); } std::thread t([this, sensor_configuration] { - if(sensor_configuration->sensor_model == SensorModel::HESAI_PANDAR40P - || sensor_configuration->sensor_model == SensorModel::HESAI_PANDAR64 - || sensor_configuration->sensor_model == SensorModel::HESAI_PANDARQT64 - || sensor_configuration->sensor_model == SensorModel::HESAI_PANDARXT32 - || sensor_configuration->sensor_model == SensorModel::HESAI_PANDARXT32M) { + if ( + sensor_configuration->sensor_model == SensorModel::HESAI_PANDAR40P || + sensor_configuration->sensor_model == SensorModel::HESAI_PANDAR64 || + sensor_configuration->sensor_model == SensorModel::HESAI_PANDARQT64 || + sensor_configuration->sensor_model == SensorModel::HESAI_PANDARXT32 || + sensor_configuration->sensor_model == SensorModel::HESAI_PANDARXT32M) { PrintInfo("Trying to set Clock source to PTP"); SetClockSource(HESAI_LIDAR_PTP_CLOCK_SOURCE); } @@ -2697,14 +1271,11 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( << ", Transport: " << sensor_configuration->ptp_transport_type << ", Switch Type: " << sensor_configuration->ptp_switch_type << " via TCP"; PrintInfo(tmp_ostringstream.str()); - SetPtpConfig(static_cast(sensor_configuration->ptp_profile), - sensor_configuration->ptp_domain, + SetPtpConfig( + static_cast(sensor_configuration->ptp_profile), sensor_configuration->ptp_domain, static_cast(sensor_configuration->ptp_transport_type), - static_cast(sensor_configuration->ptp_switch_type), - PTP_LOG_ANNOUNCE_INTERVAL, - PTP_SYNC_INTERVAL, - PTP_LOG_MIN_DELAY_INTERVAL - ); + static_cast(sensor_configuration->ptp_switch_type), PTP_LOG_ANNOUNCE_INTERVAL, + PTP_SYNC_INTERVAL, PTP_LOG_MIN_DELAY_INTERVAL); PrintDebug("Setting properties done"); }); PrintDebug("Waiting for thread to finish"); @@ -2713,29 +1284,24 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( PrintDebug("Thread finished"); std::this_thread::sleep_for(wait_time); - } - else { //AT128 only supports PTP setup via HTTP + } else { // AT128 only supports PTP setup via HTTP PrintInfo("Trying to set SyncAngle via HTTP"); - SetSyncAngleSyncHttp(1, - static_cast(sensor_configuration->scan_phase)); + SetSyncAngleSyncHttp(1, static_cast(sensor_configuration->scan_phase)); std::ostringstream tmp_ostringstream; tmp_ostringstream << "Trying to set PTP Config: " << sensor_configuration->ptp_profile << ", Domain: " << sensor_configuration->ptp_domain << ", Transport: " << sensor_configuration->ptp_transport_type << " via HTTP"; PrintInfo(tmp_ostringstream.str()); - SetPtpConfigSyncHttp(static_cast(sensor_configuration->ptp_profile), - sensor_configuration->ptp_domain, - static_cast(sensor_configuration->ptp_transport_type), - PTP_LOG_ANNOUNCE_INTERVAL, - PTP_SYNC_INTERVAL, - PTP_LOG_MIN_DELAY_INTERVAL); - + SetPtpConfigSyncHttp( + static_cast(sensor_configuration->ptp_profile), sensor_configuration->ptp_domain, + static_cast(sensor_configuration->ptp_transport_type), PTP_LOG_ANNOUNCE_INTERVAL, + PTP_SYNC_INTERVAL, PTP_LOG_MIN_DELAY_INTERVAL); } #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << "End CheckAndSetConfig(HesaiConfig)!!" << std::endl; #endif - PrintDebug("GetAndCheckConfig(HesaiConfig) finished"); + PrintDebug("GetAndCheckConfig(HesaiConfig) finished"); return Status::OK; } @@ -2802,28 +1368,26 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig() auto result = GetConfig(); std::stringstream ss; - ss << *result; + ss << result; PrintInfo(ss.str()); CheckAndSetConfig( - std::static_pointer_cast(sensor_configuration_), *result); + std::static_pointer_cast(sensor_configuration_), result); }); - t.join();//here + t.join(); // here std::thread t2([this] { - GetLidarRange( // ctx, - [this](HesaiLidarRangeAll & result) { - std::stringstream ss; - ss << result; - PrintInfo(ss.str()); - CheckAndSetConfig( - std::static_pointer_cast(sensor_configuration_), result); - }); + auto result = GetLidarRange(); + std::stringstream ss; + ss << result; + PrintInfo(ss.str()); + CheckAndSetConfig( + std::static_pointer_cast(sensor_configuration_), result); }); t2.join(); #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << "End CheckAndSetConfig!!" << std::endl; #endif - return Status::WAITING_FOR_SENSOR_RESPONSE; + return Status::OK; } /* @@ -2843,13 +1407,12 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig() */ int HesaiHwInterface::NebulaModelToHesaiModelNo(nebula::drivers::SensorModel model) { - switch (model) { case SensorModel::HESAI_PANDAR40P: return 0; case SensorModel::HESAI_PANDAR64: return 2; - case SensorModel::HESAI_PANDARQT64://check required + case SensorModel::HESAI_PANDARQT64: // check required return 15; case SensorModel::HESAI_PANDAR40M: return 17; @@ -2859,9 +1422,9 @@ int HesaiHwInterface::NebulaModelToHesaiModelNo(nebula::drivers::SensorModel mod return 32; case SensorModel::HESAI_PANDARXT32M: return 38; - case SensorModel::HESAI_PANDAR128_E3X://check required + case SensorModel::HESAI_PANDAR128_E3X: // check required return 40; - case SensorModel::HESAI_PANDAR128_E4X://OT128 + case SensorModel::HESAI_PANDAR128_E4X: // OT128 return 42; case SensorModel::HESAI_PANDARAT128: return 48; @@ -2870,8 +1433,14 @@ int HesaiHwInterface::NebulaModelToHesaiModelNo(nebula::drivers::SensorModel mod return -1; } } -void HesaiHwInterface::SetTargetModel(int model) { target_model_no = model; } -void HesaiHwInterface::SetTargetModel(nebula::drivers::SensorModel model) { target_model_no = NebulaModelToHesaiModelNo(model); } +void HesaiHwInterface::SetTargetModel(int model) +{ + target_model_no = model; +} +void HesaiHwInterface::SetTargetModel(nebula::drivers::SensorModel model) +{ + target_model_no = NebulaModelToHesaiModelNo(model); +} bool HesaiHwInterface::UseHttpSetSpinRate(int model) { @@ -2914,7 +1483,10 @@ bool HesaiHwInterface::UseHttpSetSpinRate(int model) break; } } -bool HesaiHwInterface::UseHttpSetSpinRate() { return UseHttpSetSpinRate(target_model_no); } +bool HesaiHwInterface::UseHttpSetSpinRate() +{ + return UseHttpSetSpinRate(target_model_no); +} bool HesaiHwInterface::UseHttpGetLidarMonitor(int model) { switch (model) { @@ -2956,7 +1528,10 @@ bool HesaiHwInterface::UseHttpGetLidarMonitor(int model) break; } } -bool HesaiHwInterface::UseHttpGetLidarMonitor() { return UseHttpGetLidarMonitor(target_model_no); } +bool HesaiHwInterface::UseHttpGetLidarMonitor() +{ + return UseHttpGetLidarMonitor(target_model_no); +} void HesaiHwInterface::SetLogger(std::shared_ptr logger) { diff --git a/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp index 0152fa772..bfad771b8 100644 --- a/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp @@ -306,29 +306,25 @@ Status HesaiDriverRosWrapper::GetParameters( std::future future = std::async(std::launch::async, [this, &calibration_configuration, &calibration_file_path_from_sensor, &run_local]() { if (hw_interface_.InitializeTcpDriver(false) == Status::OK) { - hw_interface_.GetLidarCalibrationFromSensor( - [this, &calibration_configuration, &calibration_file_path_from_sensor]( - const std::string &str) { - auto rt = calibration_configuration.SaveFileFromString( - calibration_file_path_from_sensor, str); - RCLCPP_ERROR_STREAM(get_logger(), str); - if (rt == Status::OK) { - RCLCPP_INFO_STREAM(get_logger(), "SaveFileFromString success:" - << calibration_file_path_from_sensor << "\n"); - } else { - RCLCPP_ERROR_STREAM(get_logger(), "SaveFileFromString failed:" - << calibration_file_path_from_sensor << "\n"); - } - rt = calibration_configuration.LoadFromString(str); - if (rt == Status::OK) { - RCLCPP_INFO_STREAM(get_logger(), - "LoadFromString success:" << str << "\n"); - } else { - RCLCPP_ERROR_STREAM(get_logger(), - "LoadFromString failed:" << str << "\n"); - } - }, - true); + auto str = hw_interface_.GetLidarCalibrationString(); + auto rt = calibration_configuration.SaveFileFromString( + calibration_file_path_from_sensor, str); + RCLCPP_ERROR_STREAM(get_logger(), str); + if (rt == Status::OK) { + RCLCPP_INFO_STREAM(get_logger(), "SaveFileFromString success:" + << calibration_file_path_from_sensor << "\n"); + } else { + RCLCPP_ERROR_STREAM(get_logger(), "SaveFileFromString failed:" + << calibration_file_path_from_sensor << "\n"); + } + rt = calibration_configuration.LoadFromString(str); + if (rt == Status::OK) { + RCLCPP_INFO_STREAM(get_logger(), + "LoadFromString success:" << str << "\n"); + } else { + RCLCPP_ERROR_STREAM(get_logger(), + "LoadFromString failed:" << str << "\n"); + } } else { run_local = true; } @@ -398,34 +394,32 @@ Status HesaiDriverRosWrapper::GetParameters( correction_file_path_from_sensor += correction_file_path.substr(ext_pos, correction_file_path.size() - ext_pos); } std::future future = std::async(std::launch::async, [this, &correction_configuration, &correction_file_path_from_sensor, &run_local, &launch_hw]() { - if (launch_hw && hw_interface_.InitializeTcpDriver(false) == Status::OK) { - RCLCPP_INFO_STREAM( - this->get_logger(), "Trying to acquire calibration data from sensor"); - hw_interface_.GetLidarCalibrationFromSensor( - [this, &correction_configuration, &correction_file_path_from_sensor, &run_local](const std::vector & received_bytes) { - RCLCPP_INFO_STREAM(get_logger(), "AT128 calibration size:" << received_bytes.size() << "\n"); - auto rt = correction_configuration.SaveFileFromBinary(correction_file_path_from_sensor, received_bytes); - if(rt == Status::OK) - { - RCLCPP_INFO_STREAM(get_logger(), "SaveFileFromBinary success:" << correction_file_path_from_sensor << "\n"); - } - else - { - RCLCPP_ERROR_STREAM(get_logger(), "SaveFileFromBinary failed:" << correction_file_path_from_sensor << ". Falling back to offline calibration file."); - run_local = true; - } - rt = correction_configuration.LoadFromBinary(received_bytes); - if(rt == Status::OK) - { - RCLCPP_INFO_STREAM(get_logger(), "LoadFromBinary success" << "\n"); - run_local = false; - } - else - { - RCLCPP_ERROR_STREAM(get_logger(), "LoadFromBinary failed" << ". Falling back to offline calibration file."); - run_local = true; - } - }); + if (launch_hw && hw_interface_.InitializeTcpDriver(false) == Status::OK) { + RCLCPP_INFO_STREAM( + this->get_logger(), "Trying to acquire calibration data from sensor"); + auto received_bytes = hw_interface_.GetLidarCalibrationBytes(); + RCLCPP_INFO_STREAM(get_logger(), "AT128 calibration size:" << received_bytes.size() << "\n"); + auto rt = correction_configuration.SaveFileFromBinary(correction_file_path_from_sensor, received_bytes); + if(rt == Status::OK) + { + RCLCPP_INFO_STREAM(get_logger(), "SaveFileFromBinary success:" << correction_file_path_from_sensor << "\n"); + } + else + { + RCLCPP_ERROR_STREAM(get_logger(), "SaveFileFromBinary failed:" << correction_file_path_from_sensor << ". Falling back to offline calibration file."); + run_local = true; + } + rt = correction_configuration.LoadFromBinary(received_bytes); + if(rt == Status::OK) + { + RCLCPP_INFO_STREAM(get_logger(), "LoadFromBinary success" << "\n"); + run_local = false; + } + else + { + RCLCPP_ERROR_STREAM(get_logger(), "LoadFromBinary failed" << ". Falling back to offline calibration file."); + run_local = true; + } }else{ RCLCPP_ERROR_STREAM(get_logger(), "Falling back to offline calibration file."); run_local = true; diff --git a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp index d7581b7f9..68aa71319 100644 --- a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp @@ -42,11 +42,9 @@ HesaiHwInterfaceRosWrapper::HesaiHwInterfaceRosWrapper(const rclcpp::NodeOptions try{ std::vector thread_pool{}; thread_pool.emplace_back([this] { - hw_interface_.GetInventory( // ios, - [this](HesaiInventory & result) { - RCLCPP_INFO_STREAM(get_logger(), result); - hw_interface_.SetTargetModel(result.model); - }); + auto result = hw_interface_.GetInventory(); + RCLCPP_INFO_STREAM(get_logger(), result); + hw_interface_.SetTargetModel(result.model); }); for (std::thread & th : thread_pool) { th.join(); @@ -146,6 +144,7 @@ HesaiHwInterfaceRosWrapper::HesaiHwInterfaceRosWrapper(const rclcpp::NodeOptions } #endif + RCLCPP_DEBUG(this->get_logger(), "Starting stream"); StreamStart(); } diff --git a/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp index 82be03022..626d6888b 100644 --- a/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp @@ -67,19 +67,17 @@ HesaiHwMonitorRosWrapper::HesaiHwMonitorRosWrapper(const rclcpp::NodeOptions & o } std::vector thread_pool{}; thread_pool.emplace_back([this] { - hw_interface_.GetInventory( // ios, - [this](HesaiInventory & result) { - current_inventory.reset(new HesaiInventory(result)); - current_inventory_time.reset(new rclcpp::Time(this->get_clock()->now())); - std::cout << "HesaiInventory" << std::endl; - std::cout << result << std::endl; - info_model = result.get_str_model(); - info_serial = std::string(result.sn.begin(), result.sn.end()); - hw_interface_.SetTargetModel(result.model); - RCLCPP_INFO_STREAM(this->get_logger(), "Model:" << info_model); - RCLCPP_INFO_STREAM(this->get_logger(), "Serial:" << info_serial); - InitializeHesaiDiagnostics(); - }); + auto result = hw_interface_.GetInventory(); + current_inventory.reset(new HesaiInventory(result)); + current_inventory_time.reset(new rclcpp::Time(this->get_clock()->now())); + std::cout << "HesaiInventory" << std::endl; + std::cout << result << std::endl; + info_model = result.get_str_model(); + info_serial = std::string(result.sn.begin(), result.sn.end()); + hw_interface_.SetTargetModel(result.model); + RCLCPP_INFO_STREAM(this->get_logger(), "Model:" << info_model); + RCLCPP_INFO_STREAM(this->get_logger(), "Serial:" << info_serial); + InitializeHesaiDiagnostics(); }); for (std::thread & th : thread_pool) { th.join(); @@ -413,12 +411,10 @@ void HesaiHwMonitorRosWrapper::OnHesaiStatusTimer() RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiStatusTimer" << std::endl); try { auto ios = std::make_shared(); - hw_interface_.GetLidarStatus( // ios, - [this](HesaiLidarStatus & result) { - std::scoped_lock lock(mtx_status); - current_status_time.reset(new rclcpp::Time(this->get_clock()->now())); - current_status.reset(new HesaiLidarStatus(result)); - }); + auto result = hw_interface_.GetLidarStatus(); + std::scoped_lock lock(mtx_status); + current_status_time.reset(new rclcpp::Time(this->get_clock()->now())); + current_status.reset(new HesaiLidarStatus(result)); } catch (const std::system_error & error) { RCLCPP_ERROR_STREAM( rclcpp::get_logger("HesaiHwMonitorRosWrapper::OnHesaiStatusTimer(std::system_error)"), @@ -459,11 +455,10 @@ void HesaiHwMonitorRosWrapper::OnHesaiLidarMonitorTimer() RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiLidarMonitorTimer"); try { auto ios = std::make_shared(); - hw_interface_.GetLidarMonitor([this](HesaiLidarMonitor & result) { - std::scoped_lock lock(mtx_lidar_monitor); - current_lidar_monitor_time.reset(new rclcpp::Time(this->get_clock()->now())); - current_monitor.reset(new HesaiLidarMonitor(result)); - }); + auto result = hw_interface_.GetLidarMonitor(); + std::scoped_lock lock(mtx_lidar_monitor); + current_lidar_monitor_time.reset(new rclcpp::Time(this->get_clock()->now())); + current_monitor.reset(new HesaiLidarMonitor(result)); } catch (const std::system_error & error) { RCLCPP_ERROR_STREAM( rclcpp::get_logger("HesaiHwMonitorRosWrapper::OnHesaiLidarMonitorTimer(std::system_error)"), From 44364e9b19bfcf64cf2d9f4f2b12b9f624418f51 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 14 Mar 2024 19:05:52 +0900 Subject: [PATCH 24/31] fix(qt128): correctly send PTP switch_mode --- .../src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp index ab76f218e..7f1189622 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 @@ -890,7 +890,7 @@ Status HesaiHwInterface::SetPtpConfig( request_payload.emplace_back(logAnnounceInterval & 0xff); request_payload.emplace_back(logSyncInterval & 0xff); request_payload.emplace_back(logMinDelayReqInterval & 0xff); - } else if (profile == 3) { + } else if (profile == 2 || profile == 3) { request_payload.emplace_back(switch_type & 0xff); } @@ -1237,8 +1237,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( } if ( - sensor_configuration->sensor_model != SensorModel::HESAI_PANDARAT128 && - sensor_configuration->sensor_model != SensorModel::HESAI_PANDARQT128) { + sensor_configuration->sensor_model != SensorModel::HESAI_PANDARAT128) { set_flg = true; auto sync_angle = static_cast(hesai_config.sync_angle / 100); auto scan_phase = static_cast(sensor_configuration->scan_phase); From 12b12b1c46d83f013b20783e4ec8f6c33cd90517 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 14 Mar 2024 19:23:25 +0900 Subject: [PATCH 25/31] fix(ot128): revert timing correction to old datasheet --- .../decoders/pandar_128e4x.hpp | 140 ++++++++---------- 1 file changed, 60 insertions(+), 80 deletions(-) diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp index 412c8aac7..7108cad34 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp @@ -20,70 +20,50 @@ typedef Packet128E3X Packet128E4X; } // namespace hesai_packet +// FIXME(mojomex): +// The OT128 datasheet has entierly different numbers (and more azimuth states). +// With the current sensor version, the numbers from the new datasheet are incorrect +// (clouds do not sync to ToS but ToS+.052s) class Pandar128E4X : public HesaiSensor { private: -enum OperationalState { HIGH_RESOLUTION = 0, SHUTDOWN = 1, STANDARD = 2 }; - - static constexpr int hires_as0_time_offset_ns_[128] = { - -1, -1, -1, -1, 21185, 14896, 8607, 2318, -1, -1, -1, -1, 21185, - 14896, 8607, 2318, -1, -1, -1, -1, 21185, 14896, 8607, 2318, 0, 18867, - 12578, 18867, 0, 6289, 16549, 10260, 16549, 10260, 12578, 3971, 3971, 6289, 22838, - 22838, 16549, 18867, 0, 10260, 12578, 18867, 3971, 6289, 12578, 22838, 16549, 6289, - 0, 10260, 16549, 18867, 3971, 10260, 12578, 22838, 3971, 6289, 0, 22838, 16549, - 18867, 0, 10260, 12578, 18867, 3971, 6289, 12578, 22838, 16549, 6289, 0, 10260, - 16549, 18867, 3971, 10260, 12578, 22838, 3971, 6289, 0, 22838, -1, -1, -1, - -1, 21185, 14896, 8607, 2318, -1, -1, -1, -1, 21185, 14896, 8607, 2318, - -1, -1, -1, -1, 21185, 14896, 8607, 2318, -1, -1, -1, -1, 21185, - 14896, 8607, 2318, -1, -1, -1, -1, 21185, 14896, 8607, 2318}; - - static constexpr int hires_as1_time_offset_ns_[128] = { - 21185, 14896, 8607, 2318, -1, -1, -1, -1, 21185, 14896, 8607, 2318, -1, - -1, -1, -1, 21185, 14896, 8607, 2318, -1, -1, -1, -1, 0, 18867, - 12578, 18867, 0, 6289, 16549, 10260, 16549, 10260, 12578, 3971, 3971, 6289, 22838, - 22838, 16549, 18867, 0, 10260, 12578, 18867, 3971, 6289, 12578, 22838, 16549, 6289, - 0, 10260, 16549, 18867, 3971, 10260, 12578, 22838, 3971, 6289, 0, 22838, 16549, - 18867, 0, 10260, 12578, 18867, 3971, 6289, 12578, 22838, 16549, 6289, 0, 10260, - 16549, 18867, 3971, 10260, 12578, 22838, 3971, 6289, 0, 22838, 21185, 14896, 8607, - 2318, -1, -1, -1, -1, 21185, 14896, 8607, 2318, -1, -1, -1, -1, - 21185, 14896, 8607, 2318, -1, -1, -1, -1, 21185, 14896, 8607, 2318, -1, - -1, -1, -1, 21185, 14896, 8607, 2318, -1, -1, -1, -1}; - - static constexpr int hires_as2_time_offset_ns_[128] = { - -1, -1, -1, -1, 21185, 14896, 8607, 2318, -1, -1, -1, -1, 21185, - 14896, 8607, 2318, -1, -1, -1, -1, 21185, 14896, 8607, 2318, 0, 18867, - 12578, 18867, 0, 6289, 16549, 10260, 16549, 10260, 12578, 3971, 3971, 6289, 22838, - 22838, 16549, 18867, 0, 10260, 12578, 18867, 3971, 6289, 12578, 22838, 16549, 6289, - 0, 10260, 16549, 18867, 3971, 10260, 12578, 22838, 3971, 6289, 0, 22838, 16549, - 18867, 0, 10260, 12578, 18867, 3971, 6289, 12578, 22838, 16549, 6289, 0, 10260, - 16549, 18867, 3971, 10260, 12578, 22838, 3971, 6289, 0, 22838, -1, -1, -1, - -1, 21185, 14896, 8607, 2318, -1, -1, -1, -1, 21185, 14896, 8607, 2318, - -1, -1, -1, -1, 21185, 14896, 8607, 2318, -1, -1, -1, -1, 21185, - 14896, 8607, 2318, -1, -1, -1, -1, 21185, 14896, 8607, 2318}; - - static constexpr int hires_as3_time_offset_ns_[128] = { - 21185, 14896, 8607, 2318, -1, -1, -1, -1, 21185, 14896, 8607, 2318, -1, - -1, -1, -1, 21185, 14896, 8607, 2318, -1, -1, -1, -1, 0, 18867, - 12578, 18867, 0, 6289, 16549, 10260, 16549, 10260, 12578, 3971, 3971, 6289, 22838, - 22838, 16549, 18867, 0, 10260, 12578, 18867, 3971, 6289, 12578, 22838, 16549, 6289, - 0, 10260, 16549, 18867, 3971, 10260, 12578, 22838, 3971, 6289, 0, 22838, 16549, - 18867, 0, 10260, 12578, 18867, 3971, 6289, 12578, 22838, 16549, 6289, 0, 10260, - 16549, 18867, 3971, 10260, 12578, 22838, 3971, 6289, 0, 22838, 21185, 14896, 8607, - 2318, -1, -1, -1, -1, 21185, 14896, 8607, 2318, -1, -1, -1, -1, - 21185, 14896, 8607, 2318, -1, -1, -1, -1, 21185, 14896, 8607, 2318, -1, - -1, -1, -1, 21185, 14896, 8607, 2318, -1, -1, -1, -1}; - - static constexpr int standard_time_offset_ns_[128] = { - 48963, 42674, 36385, 30096, 21185, 14896, 8607, 2318, 48963, 42674, 36385, 30096, 21185, - 14896, 8607, 2318, 48963, 42674, 36385, 30096, 21185, 14896, 8607, 2318, 27778, 46645, - 40356, 46645, 27778, 34067, 44327, 38038, 44327, 38038, 40356, 31749, 31749, 34067, 50616, - 50616, 44327, 46645, 27778, 38038, 40356, 46645, 31749, 34067, 40356, 50616, 44327, 34067, - 27778, 38038, 44327, 46645, 31749, 38038, 40356, 50616, 31749, 34067, 27778, 50616, 44327, - 46645, 27778, 38038, 40356, 46645, 31749, 34067, 40356, 50616, 44327, 34067, 27778, 38038, - 44327, 46645, 31749, 38038, 40356, 50616, 31749, 34067, 27778, 50616, 48963, 42674, 36385, - 30096, 21185, 14896, 8607, 2318, 48963, 42674, 36385, 30096, 21185, 14896, 8607, 2318, - 48963, 42674, 36385, 30096, 21185, 14896, 8607, 2318, 48963, 42674, 36385, 30096, 21185, - 14896, 8607, 2318, 48963, 42674, 36385, 30096, 21185, 14896, 8607, 2318}; +enum OperationalState { HIGH_RESOLUTION = 0, STANDARD = 1 }; + + static constexpr int firing_time_offset_static_ns_[128] = { + 49758, 43224, 36690, 30156, 21980, 15446, 8912, 2378, 49758, 43224, 36690, 30156, 2378, + 15446, 8912, 21980, 43224, 30156, 49758, 15446, 36690, 2378, 21980, 8912, 34312, 45002, + 38468, 40846, 40846, 34312, 51536, 47380, 31934, 47380, 31934, 51536, 38468, 27778, 27778, + 45002, 38468, 40846, 51536, 45002, 31934, 47380, 34312, 27778, 38468, 40846, 51536, 45002, + 31934, 34312, 27778, 38468, 40846, 51536, 45002, 31934, 47380, 34312, 27778, 45002, 45002, + 51536, 38468, 40846, 47380, 40846, 31934, 45002, 27778, 38468, 40846, 31934, 27778, 38468, + 34312, 34312, 34312, 47380, 51536, 31934, 51536, 47380, 27778, 49758, 21980, 43224, 15446, + 43224, 36690, 8912, 30156, 2378, 49758, 21980, 36690, 15446, 8912, 30156, 2378, 49758, + 43224, 36690, 30156, 21980, 15446, 8912, 2378, 43224, 49758, 30156, 36690, 21980, 15446, + 2378, 8912, 49758, 43224, 36690, 30156, 21980, 15446, 8912, 2378, 30156}; + + static constexpr int firing_time_offset_as0_ns_[128] = { + -1, -1, -1, -1, 21980, 15446, 8912, 2378, -1, -1, -1, -1, 2378, + 15446, 8912, 21980, -1, 2378, 21980, 8912, 6534, 17224, 10690, 13068, 13068, 6534, + 23758, 19602, 4156, 19602, 4156, 23758, 13068, 13068, 23758, 10690, 4156, 19602, 19602, + 0, 10690, 6534, 23758, 17224, 23758, 19602, 6534, 17224, 4156, 0, 6534, 0, + 17224, 10690, 17224, 17224, 23758, 23758, 10690, 13068, 13068, 13068, 19602, 19602, 6534, + 13068, 4156, 4156, 17224, 17224, 17224, 0, 10690, 10690, 13068, 13068, 4156, 0, + 10690, 6534, 6534, 6534, 19602, 23758, 4156, 23758, 19602, 0, -1, 21980, -1, + 15446, -1, 8912, -1, 2378, -1, -1, 21980, -1, 15446, 8912, -1, 2378, + -1, -1, -1, -1, 21980, 15446, 8912, 2378, -1, -1, -1, -1, 21980, + 15446, 2378, 8912, -1, -1, -1, -1, 21980, 15446, 8912, 2378}; + + static constexpr int firing_time_offset_as1_ns_[128] = { + 21980, 15446, 8912, 2378, -1, -1, -1, -1, 21980, 15446, 8912, 2378, -1, + -1, -1, -1, 8912, -1, -1, -1, 6534, 17224, 10690, 13068, 13068, 6534, + 23758, 19602, 4156, 19602, 4156, 23758, 13068, 13068, 23758, 10690, 4156, 19602, 19602, + 0, 10690, 6534, 23758, 17224, 23758, 19602, 6534, 17224, 4156, 0, 6534, 0, + 17224, 10690, 17224, 17224, 23758, 23758, 10690, 13068, 13068, 13068, 19602, 19602, 6534, + 13068, 4156, 4156, 17224, 17224, 17224, 0, 10690, 10690, 13068, 13068, 4156, 0, + 10690, 6534, 6534, 6534, 19602, 23758, 4156, 23758, 19602, 0, 21980, -1, 15446, + -1, 8912, -1, 2378, -1, 21980, 15446, -1, 8912, -1, -1, 2378, -1, + 21980, 15446, 8912, 2378, -1, -1, -1, -1, 15446, 21980, 2378, 8912, -1, + -1, -1, -1, 21980, 15446, 8912, 2378, -1, -1, -1, -1}; public: static constexpr float MIN_RANGE = 0.1; @@ -94,28 +74,28 @@ enum OperationalState { HIGH_RESOLUTION = 0, SHUTDOWN = 1, STANDARD = 2 }; uint32_t block_id, uint32_t channel_id, const packet_t & packet) { auto n_returns = hesai_packet::get_n_returns(packet.tail.return_mode); - int block_offset_ns = -27778 * (2 - block_id - 1) / n_returns; + int block_offset_ns; + if (n_returns == 1) { + block_offset_ns = -27778 * 2 * (2 - block_id - 1); + } else { + block_offset_ns = 0; + } int channel_offset_ns; bool is_hires_mode = packet.tail.operational_state == OperationalState::HIGH_RESOLUTION; - if (!is_hires_mode) block_offset_ns *= 2; - - auto azimuth_state = packet.tail.geAzimuthState(block_id); - - if (is_hires_mode && azimuth_state == 0) - channel_offset_ns = hires_as0_time_offset_ns_[channel_id]; - else if (is_hires_mode && azimuth_state == 1) - channel_offset_ns = hires_as1_time_offset_ns_[channel_id]; - else if (is_hires_mode && azimuth_state == 2) - channel_offset_ns = hires_as2_time_offset_ns_[channel_id]; - else if (is_hires_mode && azimuth_state == 3) - channel_offset_ns = hires_as3_time_offset_ns_[channel_id]; - else if (!is_hires_mode) - channel_offset_ns = standard_time_offset_ns_[channel_id]; - else - throw std::runtime_error("Invalid combination of operational state and azimuth state"); - - return block_offset_ns + channel_offset_ns; + auto azimuth_state = packet.tail.geAzimuthState(block_id); + + if (!is_hires_mode) { + channel_offset_ns = firing_time_offset_static_ns_[channel_id]; + } else { + if (azimuth_state == 0) { + channel_offset_ns = firing_time_offset_as0_ns_[channel_id]; + } else /* azimuth_state == 1 */ { + channel_offset_ns = firing_time_offset_as1_ns_[channel_id]; + } + } + + return block_offset_ns + 43346 + channel_offset_ns; } ReturnType getReturnType( From 074cde048f138dcf527a62a289eabc2b724c33d5 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 14 Mar 2024 19:40:08 +0900 Subject: [PATCH 26/31] chore(scripts): delete defunct debug script --- scripts/plot_pcap_packet_times.py | 94 ------------------------------- 1 file changed, 94 deletions(-) delete mode 100644 scripts/plot_pcap_packet_times.py diff --git a/scripts/plot_pcap_packet_times.py b/scripts/plot_pcap_packet_times.py deleted file mode 100644 index cf07d9b5b..000000000 --- a/scripts/plot_pcap_packet_times.py +++ /dev/null @@ -1,94 +0,0 @@ -#!/usr/bin/python3 - -import socket -import dpkt -import time -import argparse -import os -import tqdm - -from dpkt.udp import UDP -from dpkt.tcp import TCP -from dpkt.arp import ARP -from dpkt.ip import IP - -from matplotlib import pyplot as plt - -Y_ARP = 0 -Y_TCP1 = 2 -Y_TCP2 = 3 -Y_TCP3 = 4 -Y_TCP4 = 5 -Y_UDP = 7 - -parser = argparse.ArgumentParser( - description="Replay a PCAP file containing UDP packets while rewriting the target IP address" -) -parser.add_argument("input", help="The PCAP file to read from. Supported formats: .pcap, .pcapng") - -args = parser.parse_args() - -file_type = os.path.splitext(args.input)[1] - -t0 = None - -packets = { - Y_ARP: [], - Y_TCP1: [], - Y_TCP2: [], - Y_TCP3: [], - Y_TCP4: [], - Y_UDP: [], -} - -tcp_streams = {} - -with open(args.input, "rb") as f: - if file_type == ".pcap": - pcap = dpkt.pcap.Reader(f) - elif file_type == ".pcapng": - pcap = dpkt.pcapng.Reader(f) - else: - print(f"Unknown file type: {file_type}. Expected .pcap or .pcapng.") - exit(1) - - for i, (timestamp_s, buf) in enumerate(pcap): - # This automatically discards everything that is not a UDP packet - try: - eth = dpkt.ethernet.Ethernet(buf) - - if t0 is None: - t0 = timestamp_s - - t = timestamp_s - t0 - - if type(eth.data) == ARP: - packets[Y_ARP].append(t) - continue - - ip: IP = eth.data - - if type(ip.data) == UDP: - packets[Y_UDP].append(t) - continue - - if type(ip.data) != TCP: - print(f"Skipped {type(ip)}") - continue - - tcp: TCP = ip.data - stream_tup = (ip.src, ip.dst, tcp.sport, tcp.dport) - if stream_tup not in tcp_streams: - tcp_streams[stream_tup] = len(tcp_streams) - - stream_id = tcp_streams[stream_tup] - packets[Y_TCP1 + stream_id].append(t) - except AttributeError: - continue - -fig, ax = plt.subplots() -ax: plt.Axes -for y in [Y_ARP, Y_TCP1, Y_TCP2, Y_TCP3, Y_TCP4, Y_UDP]: - ax.scatter(packets[y], [y] * len(packets[y])) - -plt.show() \ No newline at end of file From 27448e6edf4ccbe11465d396c509e780eae0cfa5 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 14 Mar 2024 20:15:51 +0900 Subject: [PATCH 27/31] fix(hesai_hw_interface): remove excessive debug logging --- nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp index 68aa71319..18e515262 100644 --- a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp @@ -7,8 +7,6 @@ namespace ros HesaiHwInterfaceRosWrapper::HesaiHwInterfaceRosWrapper(const rclcpp::NodeOptions & options) : rclcpp::Node("hesai_hw_interface_ros_wrapper", options), hw_interface_() { - this->get_logger().set_level(rclcpp::Logger::Level::Debug); - if (mtx_config_.try_lock()) { interface_status_ = GetParameters(sensor_configuration_); mtx_config_.unlock(); From a809559fb4eb84eb3d7b1b3381a296be58838009 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 14 Mar 2024 20:19:14 +0900 Subject: [PATCH 28/31] fix: rollback erroneous changes in velodyne launch --- nebula_ros/launch/velodyne_launch_all_hw.xml | 68 ++++++++++---------- 1 file changed, 34 insertions(+), 34 deletions(-) diff --git a/nebula_ros/launch/velodyne_launch_all_hw.xml b/nebula_ros/launch/velodyne_launch_all_hw.xml index 3d2059d9c..c39437039 100644 --- a/nebula_ros/launch/velodyne_launch_all_hw.xml +++ b/nebula_ros/launch/velodyne_launch_all_hw.xml @@ -28,40 +28,40 @@ - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + From 6004d41a9f8e7d498131cd0968810b7eedf060c7 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 14 Mar 2024 21:03:07 +0900 Subject: [PATCH 29/31] fix(hesai_hw_interface): remove unused params/variables --- .../hesai_hw_interface.hpp | 2 +- .../hesai_hw_interface.cpp | 56 +++++-------------- .../src/hesai/hesai_decoder_ros_wrapper.cpp | 4 +- .../hesai/hesai_hw_interface_ros_wrapper.cpp | 4 +- .../hesai/hesai_hw_monitor_ros_wrapper.cpp | 2 +- 5 files changed, 21 insertions(+), 47 deletions(-) diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp index 2f1ced03a..5e243d5eb 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 @@ -157,7 +157,7 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @brief Initializing tcp_driver for TCP communication /// @param setup_sensor Whether to also initialize tcp_driver for sensor configuration /// @return Resulting status - Status InitializeTcpDriver(bool setup_sensor = true); + Status InitializeTcpDriver(); /// @brief Closes the TcpDriver and related resources /// @return Status result Status FinalizeTcpDriver(); 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 f712db016..4efd0d15b 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 @@ -294,10 +294,10 @@ Status HesaiHwInterface::GetCalibrationConfiguration( return Status::ERROR_1; } -Status HesaiHwInterface::InitializeTcpDriver(bool setup_sensor) +Status HesaiHwInterface::InitializeTcpDriver() { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "HesaiHwInterface::InitializeTcpDriver, setup_sensor=" << setup_sensor << std::endl; + std::cout << "HesaiHwInterface::InitializeTcpDriver" << std::endl; std::cout << "st: tcp_driver_->init_socket" << std::endl; std::cout << "sensor_configuration_->sensor_ip=" << sensor_configuration_->sensor_ip << std::endl; std::cout << "sensor_configuration_->host_ip=" << sensor_configuration_->host_ip << std::endl; @@ -695,9 +695,7 @@ Status HesaiHwInterface::SetSpinRate(uint16_t rpm) request_payload.emplace_back((rpm >> 8) & 0xff); request_payload.emplace_back(rpm & 0xff); - auto response_ptr = SendReceive(PTC_COMMAND_SET_SPIN_RATE, request_payload); - auto & response = *response_ptr; - + SendReceive(PTC_COMMAND_SET_SPIN_RATE, request_payload); return Status::OK; } @@ -708,9 +706,7 @@ Status HesaiHwInterface::SetSyncAngle(int sync_angle, int angle) request_payload.emplace_back((angle >> 8) & 0xff); request_payload.emplace_back(angle & 0xff); - auto response_ptr = SendReceive(PTC_COMMAND_SET_SYNC_ANGLE, request_payload); - auto & response = *response_ptr; - + SendReceive(PTC_COMMAND_SET_SYNC_ANGLE, request_payload); return Status::OK; } @@ -719,9 +715,7 @@ Status HesaiHwInterface::SetTriggerMethod(int trigger_method) std::vector request_payload; request_payload.emplace_back(trigger_method & 0xff); - auto response_ptr = SendReceive(PTC_COMMAND_SET_TRIGGER_METHOD, request_payload); - auto & response = *response_ptr; - + SendReceive(PTC_COMMAND_SET_TRIGGER_METHOD, request_payload); return Status::OK; } @@ -730,9 +724,7 @@ Status HesaiHwInterface::SetStandbyMode(int standby_mode) std::vector request_payload; request_payload.emplace_back(standby_mode & 0xff); - auto response_ptr = SendReceive(PTC_COMMAND_SET_STANDBY_MODE, request_payload); - auto & response = *response_ptr; - + SendReceive(PTC_COMMAND_SET_STANDBY_MODE, request_payload); return Status::OK; } @@ -741,9 +733,7 @@ Status HesaiHwInterface::SetReturnMode(int return_mode) std::vector request_payload; request_payload.emplace_back(return_mode & 0xff); - auto response_ptr = SendReceive(PTC_COMMAND_SET_RETURN_MODE, request_payload); - auto & response = *response_ptr; - + SendReceive(PTC_COMMAND_SET_RETURN_MODE, request_payload); return Status::OK; } @@ -760,9 +750,7 @@ Status HesaiHwInterface::SetDestinationIp( request_payload.emplace_back((gps_port >> 8) & 0xff); request_payload.emplace_back(gps_port & 0xff); - auto response_ptr = SendReceive(PTC_COMMAND_SET_DESTINATION_IP, request_payload); - auto & response = *response_ptr; - + SendReceive(PTC_COMMAND_SET_DESTINATION_IP, request_payload); return Status::OK; } @@ -787,9 +775,7 @@ Status HesaiHwInterface::SetControlPort( request_payload.emplace_back((vlan_id >> 8) & 0xff); request_payload.emplace_back(vlan_id & 0xff); - auto response_ptr = SendReceive(PTC_COMMAND_SET_CONTROL_PORT, request_payload); - auto & response = *response_ptr; - + SendReceive(PTC_COMMAND_SET_CONTROL_PORT, request_payload); return Status::OK; } @@ -802,9 +788,7 @@ 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_ptr = SendReceive(PTC_COMMAND_SET_LIDAR_RANGE, request_payload); - auto & response = *response_ptr; - + SendReceive(PTC_COMMAND_SET_LIDAR_RANGE, request_payload); return Status::OK; } @@ -821,9 +805,7 @@ Status HesaiHwInterface::SetLidarRange(int start, int end) request_payload.emplace_back((end >> 8) & 0xff); request_payload.emplace_back(end & 0xff); - auto response_ptr = SendReceive(PTC_COMMAND_SET_LIDAR_RANGE, request_payload); - auto & response = *response_ptr; - + SendReceive(PTC_COMMAND_SET_LIDAR_RANGE, request_payload); return Status::OK; } @@ -859,9 +841,7 @@ Status HesaiHwInterface::SetClockSource(int clock_source) std::vector request_payload; request_payload.emplace_back(clock_source & 0xff); - auto response_ptr = SendReceive(PTC_COMMAND_SET_CLOCK_SOURCE, request_payload); - auto & response = *response_ptr; - + SendReceive(PTC_COMMAND_SET_CLOCK_SOURCE, request_payload); return Status::OK; } @@ -894,9 +874,7 @@ Status HesaiHwInterface::SetPtpConfig( request_payload.emplace_back(switch_type & 0xff); } - auto response_ptr = SendReceive(PTC_COMMAND_SET_PTP_CONFIG, request_payload); - auto & response = *response_ptr; - + SendReceive(PTC_COMMAND_SET_PTP_CONFIG, request_payload); return Status::OK; } @@ -927,9 +905,7 @@ HesaiPtpConfig HesaiHwInterface::GetPtpConfig() Status HesaiHwInterface::SendReset() { - auto response_ptr = SendReceive(PTC_COMMAND_RESET); - auto & response = *response_ptr; - + SendReceive(PTC_COMMAND_RESET); return Status::OK; } @@ -938,9 +914,7 @@ Status HesaiHwInterface::SetRotDir(int mode) std::vector request_payload; request_payload.emplace_back(mode & 0xff); - auto response_ptr = SendReceive(PTC_COMMAND_SET_ROTATE_DIRECTION, request_payload); - auto & response = *response_ptr; - + SendReceive(PTC_COMMAND_SET_ROTATE_DIRECTION, request_payload); return Status::OK; } diff --git a/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp index bfad771b8..6f27d547e 100644 --- a/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp @@ -305,7 +305,7 @@ Status HesaiDriverRosWrapper::GetParameters( << sensor_configuration.sensor_ip << "'"); std::future future = std::async(std::launch::async, [this, &calibration_configuration, &calibration_file_path_from_sensor, &run_local]() { - if (hw_interface_.InitializeTcpDriver(false) == Status::OK) { + if (hw_interface_.InitializeTcpDriver() == Status::OK) { auto str = hw_interface_.GetLidarCalibrationString(); auto rt = calibration_configuration.SaveFileFromString( calibration_file_path_from_sensor, str); @@ -394,7 +394,7 @@ Status HesaiDriverRosWrapper::GetParameters( correction_file_path_from_sensor += correction_file_path.substr(ext_pos, correction_file_path.size() - ext_pos); } std::future future = std::async(std::launch::async, [this, &correction_configuration, &correction_file_path_from_sensor, &run_local, &launch_hw]() { - if (launch_hw && hw_interface_.InitializeTcpDriver(false) == Status::OK) { + if (launch_hw && hw_interface_.InitializeTcpDriver() == Status::OK) { RCLCPP_INFO_STREAM( this->get_logger(), "Trying to acquire calibration data from sensor"); auto received_bytes = hw_interface_.GetLidarCalibrationBytes(); diff --git a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp index 18e515262..381a214f0 100644 --- a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp @@ -22,7 +22,7 @@ HesaiHwInterfaceRosWrapper::HesaiHwInterfaceRosWrapper(const rclcpp::NodeOptions hw_interface_.SetSensorConfiguration( std::static_pointer_cast(sensor_cfg_ptr)); #if not defined(TEST_PCAP) - Status rt = hw_interface_.InitializeTcpDriver(this->setup_sensor); + Status rt = hw_interface_.InitializeTcpDriver(); if(this->retry_hw_) { int cnt = 0; @@ -32,7 +32,7 @@ HesaiHwInterfaceRosWrapper::HesaiHwInterfaceRosWrapper(const rclcpp::NodeOptions cnt++; std::this_thread::sleep_for(std::chrono::milliseconds(8000));// >5000 RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Retry: " << cnt); - rt = hw_interface_.InitializeTcpDriver(this->setup_sensor); + rt = hw_interface_.InitializeTcpDriver(); } } diff --git a/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp index 626d6888b..239f6b8b0 100644 --- a/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp @@ -61,7 +61,7 @@ HesaiHwMonitorRosWrapper::HesaiHwMonitorRosWrapper(const rclcpp::NodeOptions & o hw_interface_.SetLogger(std::make_shared(this->get_logger())); hw_interface_.SetSensorConfiguration( std::static_pointer_cast(sensor_cfg_ptr)); - while(hw_interface_.InitializeTcpDriver(false) == Status::ERROR_1) + while(hw_interface_.InitializeTcpDriver() == Status::ERROR_1) { std::this_thread::sleep_for(std::chrono::milliseconds(8000));// >5000 } From adf43fef5af5924675b60e317a6de3b59e7a9f06 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 14 Mar 2024 21:06:52 +0900 Subject: [PATCH 30/31] style: fix weird spacings --- .../nebula_decoders_hesai/decoders/pandar_128e4x.hpp | 4 ++-- .../hesai_hw_interface.hpp | 12 ++++++------ 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp index 7108cad34..51ed06943 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp @@ -83,7 +83,7 @@ enum OperationalState { HIGH_RESOLUTION = 0, STANDARD = 1 }; int channel_offset_ns; bool is_hires_mode = packet.tail.operational_state == OperationalState::HIGH_RESOLUTION; - auto azimuth_state = packet.tail.geAzimuthState(block_id); + auto azimuth_state = packet.tail.geAzimuthState(block_id); if (!is_hires_mode) { channel_offset_ns = firing_time_offset_static_ns_[channel_id]; @@ -94,7 +94,7 @@ enum OperationalState { HIGH_RESOLUTION = 0, STANDARD = 1 }; channel_offset_ns = firing_time_offset_as1_ns_[channel_id]; } } - + return block_offset_ns + 43346 + channel_offset_ns; } 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 5e243d5eb..949c2a508 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp @@ -77,12 +77,12 @@ const uint16_t PANDAR128_E4X_EXTENDED_PACKET_SIZE = 1117; const uint16_t MTU_SIZE = 1500; -const int PTP_LOG_ANNOUNCE_INTERVAL = - 1; // Time interval between Announce messages, in units of log seconds (default: 1) -const int PTP_SYNC_INTERVAL = - 1; // Time interval between Sync messages, in units of log seconds (default: 1) -const int PTP_LOG_MIN_DELAY_INTERVAL = 0; // Minimum permitted mean time between Delay_Req - // messages, in units of log seconds (default: 0) +// Time interval between Announce messages, in units of log seconds (default: 1) +const int PTP_LOG_ANNOUNCE_INTERVAL = 1; +// Time interval between Sync messages, in units of log seconds (default: 1) +const int PTP_SYNC_INTERVAL = 1; +// Minimum permitted mean time between Delay_Req messages, in units of log seconds (default: 0) +const int PTP_LOG_MIN_DELAY_INTERVAL = 0; const int HESAI_LIDAR_GPS_CLOCK_SOURCE = 0; const int HESAI_LIDAR_PTP_CLOCK_SOURCE = 1; From d56b77e2adffe2c41f8990cfa4926608cb163fa6 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 14 Mar 2024 21:26:32 +0900 Subject: [PATCH 31/31] fix: spelling, whitespace --- .../nebula_decoders_hesai/decoders/pandar_128e4x.hpp | 2 +- .../src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp index 51ed06943..41cee89b3 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp @@ -21,7 +21,7 @@ typedef Packet128E3X Packet128E4X; } // namespace hesai_packet // FIXME(mojomex): -// The OT128 datasheet has entierly different numbers (and more azimuth states). +// The OT128 datasheet has entirely different numbers (and more azimuth states). // With the current sensor version, the numbers from the new datasheet are incorrect // (clouds do not sync to ToS but ToS+.052s) class Pandar128E4X : public HesaiSensor 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 4efd0d15b..b35162439 100644 --- a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp @@ -1128,8 +1128,8 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( #endif auto current_return_mode = nebula::drivers::ReturnModeFromIntHesai( hesai_config.return_mode, sensor_configuration->sensor_model); - auto wait_time = - 100ms; // Avoids spamming the sensor, which leads to failure when configuring it. + // Avoids spamming the sensor, which leads to failure when configuring it. + auto wait_time = 100ms; if (sensor_configuration->return_mode != current_return_mode) { std::stringstream ss; ss << current_return_mode; @@ -1345,7 +1345,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig() CheckAndSetConfig( std::static_pointer_cast(sensor_configuration_), result); }); - t.join(); // here + t.join(); std::thread t2([this] { auto result = GetLidarRange();