From 45c01cf48cbdba3696f1e40a68726af50b67ba19 Mon Sep 17 00:00:00 2001 From: Abraham Monrroy Cano Date: Fri, 8 Mar 2024 15:27:42 +0900 Subject: [PATCH] feat: hesai ptp setup parameters (#110) * add initial support for ot128/128e4x Signed-off-by: amc-nu * Initial support for QT128 Signed-off-by: amc-nu * hesai. automotive ptp support Signed-off-by: amc-nu * nebula_decoder. hesai tokenize the calibration file to avoid errors at load Signed-off-by: amc-nu * hesai_w. fix spellcheck Signed-off-by: amc-nu * nebula_ros. add xml based component launcher Signed-off-by: amc-nu * launch. launch typo Signed-off-by: amc-nu * launch. match target container Signed-off-by: amc-nu * hesai_hw. replace publisher with move operator Signed-off-by: amc-nu * launch. add ptp support to all launch systems Signed-off-by: amc-nu * launch. component_launch reset namespace Signed-off-by: amc-nu * velodyne launch. revert commented out hw Signed-off-by: amc-nu * hesai_hw_interface. add explanation to sleep time between cmds Signed-off-by: amc-nu * hesai_common. reduce cout verbosity Signed-off-by: amc-nu --------- Signed-off-by: amc-nu --- .../nebula_common/hesai/hesai_common.hpp | 96 +++++------- .../include/nebula_common/nebula_common.hpp | 89 ++++++++++- .../hesai_hw_interface.hpp | 5 +- .../hesai_hw_interface.cpp | 60 +++++-- nebula_ros/launch/hesai_launch_all_hw.xml | 101 +++++++----- .../launch/hesai_launch_component.launch.xml | 88 +++++++++++ nebula_ros/launch/nebula_launch.py | 23 ++- .../src/hesai/hesai_decoder_ros_wrapper.cpp | 148 ++++++++++-------- .../hesai/hesai_hw_interface_ros_wrapper.cpp | 45 +++++- 9 files changed, 466 insertions(+), 189 deletions(-) create mode 100644 nebula_ros/launch/hesai_launch_component.launch.xml diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index a19b025b0..ad1dcdf57 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; } @@ -48,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 @@ -72,28 +66,31 @@ 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; } - 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; + + 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; + } catch (const std::invalid_argument& ia) { + continue; + } + } -// std::cout << "LoadFromString fin" << std::endl; return Status::OK; } @@ -129,9 +126,7 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase return Status::CANNOT_SAVE_FILE; } ofs << calibration_string; -// std::cout << calibration_string << std::endl; ofs.close(); - return Status::OK; } }; @@ -169,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: @@ -216,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; @@ -261,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: @@ -392,6 +362,8 @@ inline ReturnMode ReturnModeFromStringHesai( switch (sensor_model) { 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; @@ -424,6 +396,8 @@ 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: + 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; @@ -455,6 +429,8 @@ 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: + 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_common/include/nebula_common/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp index c75359aa8..70466c141 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 @@ -341,6 +341,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 { @@ -556,6 +569,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..c9c54896a 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) @@ -116,7 +113,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 f5815fc40..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 @@ -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; @@ -2900,11 +2900,13 @@ 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 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; @@ -2922,7 +2924,9 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( SetReturnMode(return_mode_int); }); t.join(); + std::this_thread::sleep_for(wait_time); } + 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)); @@ -2932,10 +2936,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(wait_time); } bool set_flg = false; @@ -2975,9 +2981,11 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( 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); @@ -2993,30 +3001,46 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( SetSyncAngle(sync_flg, scan_phase); }); t.join(); + std::this_thread::sleep_for(wait_time); } - 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_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(); + std::this_thread::sleep_for(wait_time); } else { //AT128 only supports PTP setup via HTTP 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_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); @@ -3195,8 +3219,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; // All other vendors and unknown sensors @@ -3237,6 +3261,9 @@ bool HesaiHwInterface::UseHttpSetSpinRate(int model) case 38: return false; break; + case 42: + return false; + break; case 48: return false; break; @@ -3276,6 +3303,9 @@ bool HesaiHwInterface::UseHttpGetLidarMonitor(int model) case 38: return false; break; + case 42: + return false; + break; case 48: return false; break; diff --git a/nebula_ros/launch/hesai_launch_all_hw.xml b/nebula_ros/launch/hesai_launch_all_hw.xml index a04333a59..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,20 +16,26 @@ - + - + + + + + + + + name="hesai_cloud" output="screen" ros_args="--log-level $(var debug_level)"> @@ -33,47 +43,52 @@ + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + 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..fbd4b2cf7 --- /dev/null +++ b/nebula_ros/launch/hesai_launch_component.launch.xml @@ -0,0 +1,88 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/nebula_ros/launch/nebula_launch.py b/nebula_ros/launch/nebula_launch.py index f84c0004b..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,12 @@ 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"), + "ptp_transport_type": LaunchConfiguration("ptp_transport_type"), }, ], ), @@ -145,10 +157,15 @@ 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"), 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 573de5696..0152fa772 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()); @@ -260,6 +263,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; } @@ -276,65 +289,72 @@ 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); + 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); + } 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); @@ -347,6 +367,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 << "'"); @@ -369,16 +390,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"); @@ -405,22 +427,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..df78c6a34 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; } @@ -367,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(