From f71026f45c224f8927350533c3bf10231b1fa4a3 Mon Sep 17 00:00:00 2001 From: amc-nu Date: Mon, 13 Nov 2023 14:14:48 +0900 Subject: [PATCH 01/14] 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/14] 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/14] 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/14] 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/14] 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/14] 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/14] 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/14] 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/14] 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/14] 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/14] 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 e3175a8908012ac8e75d4a13883edb85c88a3098 Mon Sep 17 00:00:00 2001 From: amc-nu Date: Fri, 8 Mar 2024 09:40:22 +0900 Subject: [PATCH 12/14] velodyne launch. revert commented out hw Signed-off-by: amc-nu --- 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 fe8d2af31ffde899ba6e957aeb507c30372b9d80 Mon Sep 17 00:00:00 2001 From: amc-nu Date: Fri, 8 Mar 2024 09:44:47 +0900 Subject: [PATCH 13/14] hesai_hw_interface. add explanation to sleep time between cmds Signed-off-by: amc-nu --- .../nebula_hesai_hw_interfaces/hesai_hw_interface.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 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 9b353132d..a5589f4e7 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 @@ -2906,6 +2906,7 @@ 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. if (sensor_configuration->return_mode != current_return_mode) { std::stringstream ss; ss << current_return_mode; @@ -2923,7 +2924,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( SetReturnMode(return_mode_int); }); t.join(); - std::this_thread::sleep_for(100ms); + std::this_thread::sleep_for(wait_time); } auto current_rotation_speed = hesai_config.spin_rate; @@ -2940,7 +2941,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( [this, sensor_configuration] { SetSpinRate(sensor_configuration->rotation_speed); }); t.join(); } - std::this_thread::sleep_for(100ms); + std::this_thread::sleep_for(wait_time); } bool set_flg = false; @@ -2980,7 +2981,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( sensor_configuration->gnss_port); }); t.join(); - std::this_thread::sleep_for(100ms); + std::this_thread::sleep_for(wait_time); } if (sensor_configuration->sensor_model != SensorModel::HESAI_PANDARAT128 @@ -3000,7 +3001,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( SetSyncAngle(sync_flg, scan_phase); }); t.join(); - std::this_thread::sleep_for(100ms); + std::this_thread::sleep_for(wait_time); } std::thread t([this, sensor_configuration] { @@ -3026,7 +3027,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( ); }); t.join(); - std::this_thread::sleep_for(100ms); + std::this_thread::sleep_for(wait_time); } else { //AT128 only supports PTP setup via HTTP PrintInfo("Trying to set SyncAngle via HTTP"); From 1d59276315b1274bf0b6cbdfe568098cbdc1dcc4 Mon Sep 17 00:00:00 2001 From: amc-nu Date: Fri, 8 Mar 2024 09:48:20 +0900 Subject: [PATCH 14/14] hesai_common. reduce cout verbosity Signed-off-by: amc-nu --- .../nebula_common/hesai/hesai_common.hpp | 27 ------------------- 1 file changed, 27 deletions(-) diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index b13d829c7..ad1dcdf57 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_common.hpp +++ b/nebula_common/include/nebula_common/hesai/hesai_common.hpp @@ -86,7 +86,6 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase 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; } @@ -127,7 +126,6 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase return Status::CANNOT_SAVE_FILE; } ofs << calibration_string; -// std::cout << calibration_string << std::endl; ofs.close(); return Status::OK; } @@ -166,21 +164,15 @@ struct HesaiCorrection delimiter = (buf[index] & 0xff) << 8 | ((buf[index + 1] & 0xff)); versionMajor = buf[index + 2] & 0xff; versionMinor = buf[index + 3] & 0xff; - std::cout << "versionMajor=" << static_cast(versionMajor) << std::endl; - std::cout << "versionMinor=" << static_cast(versionMinor) << std::endl; channelNumber = buf[index + 4] & 0xff; - std::cout << "channelNumber=" << static_cast(channelNumber) << std::endl; mirrorNumber = buf[index + 5] & 0xff; - std::cout << "mirrorNumber=" << static_cast(mirrorNumber) << std::endl; frameNumber = buf[index + 6] & 0xff; - std::cout << "frameNumber=" << static_cast(frameNumber) << std::endl; index += 7; for (uint8_t i = 0; i < 8; i++) { frameConfig[i] = buf[index] & 0xff; index++; } resolution = buf[index] & 0xff; - std::cout << "resolution=" << static_cast(resolution) << std::endl; index++; switch (versionMinor) { case 5: @@ -213,14 +205,9 @@ struct HesaiCorrection index++; } - // 230328 add for (uint8_t i = 0; i < mirrorNumber; i++) { startFrame[i] *= resolution; endFrame[i] *= resolution; - std::cout << "startFrame[" << static_cast(i) - << "]=" << static_cast(startFrame[i]) << std::endl; - std::cout << "endFrame[" << static_cast(i) << "]=" << static_cast(endFrame[i]) - << std::endl; } for (uint8_t i = 0; i < channelNumber; i++) { azimuth[i] *= resolution; @@ -258,20 +245,6 @@ struct HesaiCorrection index++; } - for (uint8_t i = 0; i < mirrorNumber; i++) { - std::cout << "startFrame[" << static_cast(i) - << "]=" << static_cast(startFrame[i]) << std::endl; - std::cout << "endFrame[" << static_cast(i) << "]=" << static_cast(endFrame[i]) - << std::endl; - /* - startFrame[i] *= 2.56; - endFrame[i] *= 2.56; - std::cout << "startFrame[" << static_cast(i) << "]=" << static_cast(startFrame[i]) - << std::endl; std::cout << "endFrame[" << static_cast(i) << "]=" << - static_cast(endFrame[i]) << std::endl; - */ - } - break; default: