From 476224d5844c24721030da176d9def615fa9c43a Mon Sep 17 00:00:00 2001 From: Abraham Monrroy Cano Date: Wed, 25 Oct 2023 13:53:55 +0900 Subject: [PATCH] fix(hesai_hw_interface): hesai interface (#72) * Fix get lidar calibration (#52) * Fix(GetLidarCalibration) * Skip GetInventory if unable to connect * fix monitor * clean * remove comments Signed-off-by: amc-nu * at128. remove warnings Signed-off-by: amc-nu * decoder. check calibration Signed-off-by: amc-nu * Saving calibration data from sensor (except AT128) * Saving correction data from sensor (AT128) * fix(AT128 path) * info for debugging * nebula_hesai_ros. at128 fallback to offline Signed-off-by: amc-nu * syncGetLidarCalibrationFromSensor -> GetLidarCalibrationFromSensor (AT128) * hesai_decoder_ros. message on tcp fail Signed-off-by: amc-nu * comment out da902901c4cd900bfbf906caf76ae143fe4a88a6 * Check "start of packet" in correction data * hesai_decoder. save dat file Signed-off-by: amc-nu * TcpDriver startup delay for setup_sensor * delay & retry * at1282ex. return mode checks Signed-off-by: amc-nu * check lock * check "isOpen" --------- Signed-off-by: amc-nu Co-authored-by: Kyutoku * cspell. fix typos Signed-off-by: amc-nu * cspell. fix typos Signed-off-by: amc-nu --------- Signed-off-by: amc-nu Co-authored-by: Kyutoku --- .../nebula_common/hesai/hesai_common.hpp | 73 +++- .../hesai_cmd_response.hpp | 3 + .../hesai_hw_interface.hpp | 41 ++ .../hesai_hw_interface.cpp | 361 ++++++++++++++++-- .../hesai/hesai_hw_interface_ros_wrapper.hpp | 2 + .../hesai/hesai_hw_monitor_ros_wrapper.hpp | 1 + nebula_ros/launch/hesai_launch_all_hw.xml | 6 + .../src/hesai/hesai_decoder_ros_wrapper.cpp | 214 +++++++++-- .../hesai/hesai_hw_interface_ros_wrapper.cpp | 81 +++- .../hesai/hesai_hw_monitor_ros_wrapper.cpp | 24 +- 10 files changed, 711 insertions(+), 95 deletions(-) diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index e8be0ce35..a19b025b0 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_common.hpp +++ b/nebula_common/include/nebula_common/hesai/hesai_common.hpp @@ -65,7 +65,7 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase return Status::OK; } - /// @brief Loading calibration data (not used) + /// @brief Loading calibration data /// @param calibration_content /// @return Resulting status inline nebula::Status LoadFromString(const std::string & calibration_content) @@ -80,11 +80,20 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase int laser_id; float elevation; float azimuth; - while (!ss.eof()) { - ss >> laser_id >> sep >> elevation >> sep >> azimuth; + + std::string line; + while (std::getline(ss, line)) { + if (line.empty()) { + 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; } @@ -108,6 +117,23 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase return Status::OK; } + + /// @brief Saving calibration data from string + /// @param calibration_file path + /// @param calibration_string calibration string + /// @return Resulting status + inline nebula::Status SaveFileFromString(const std::string & calibration_file, const std::string & calibration_string) + { + std::ofstream ofs(calibration_file); + if (!ofs) { + return Status::CANNOT_SAVE_FILE; + } + ofs << calibration_string; +// std::cout << calibration_string << std::endl; + ofs.close(); + + return Status::OK; + } }; /// @brief struct for Hesai correction configuration (for AT) @@ -135,7 +161,11 @@ struct HesaiCorrection /// @return Resulting status inline nebula::Status LoadFromBinary(const std::vector & buf) { - size_t index = 0; + size_t index; + for (index = 0; index < buf.size()-1; index++) { + if(buf[index]==0xee && buf[index+1]==0xff) + break; + } delimiter = (buf[index] & 0xff) << 8 | ((buf[index + 1] & 0xff)); versionMajor = buf[index + 2] & 0xff; versionMinor = buf[index + 3] & 0xff; @@ -275,6 +305,38 @@ struct HesaiCorrection return Status::OK; } + /// @brief Save correction data from binary buffer + /// @param correction_file path + /// @param buf correction binary + /// @return Resulting status + inline nebula::Status SaveFileFromBinary(const std::string & correction_file, const std::vector & buf) + { + std::cerr << "Saving in:" << correction_file << "\n"; + std::ofstream ofs(correction_file, std::ios::trunc | std::ios::binary); + if (!ofs) { + std::cerr << "Could not create file:" << correction_file << "\n"; + return Status::CANNOT_SAVE_FILE; + } + std::cerr << "Writing start...." << buf.size() << "\n"; + bool sop_received = false; + for (const auto &byte : buf) { + if (!sop_received) { + if (byte == 0xEE) { + std::cerr << "SOP received....\n"; + sop_received = true; + } + } + if(sop_received) { + ofs << byte; + } + } + std::cerr << "Closing file\n"; + ofs.close(); + if(sop_received) + return Status::OK; + return Status::INVALID_CALIBRATION_FILE; + } + static const int STEP3 = 200 * 256; /// @brief Get azimuth adjustment for channel and precision azimuth @@ -395,7 +457,8 @@ inline int IntFromReturnModeHesai(const ReturnMode return_mode, const SensorMode case SensorModel::HESAI_PANDARAT128: if (return_mode == ReturnMode::LAST) return 0; if (return_mode == ReturnMode::STRONGEST) return 1; - if (return_mode == ReturnMode::DUAL_LAST_STRONGEST) return 2; + if (return_mode == ReturnMode::DUAL_LAST_STRONGEST + || return_mode == ReturnMode::DUAL) return 2; if (return_mode == ReturnMode::FIRST) return 3; if (return_mode == ReturnMode::DUAL_LAST_FIRST) return 4; if (return_mode == ReturnMode::DUAL_FIRST_STRONGEST) return 5; 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 990af986a..f8d9a4819 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 @@ -248,6 +248,9 @@ struct HesaiInventory case 17: return "Pandar40M"; break; + case 20: + return "PandarMind(PM64)"; + break; case 25: return "PandarXT32"; 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 5ebeb91bb..ad0ce4c16 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 @@ -166,6 +166,8 @@ class HesaiHwInterface : NebulaHwInterfaceBase public: /// @brief Constructor HesaiHwInterface(); + /// @brief Destructor + ~HesaiHwInterface(); /// @brief Initializing tcp_driver for TCP communication /// @param setup_sensor Whether to also initialize tcp_driver for sensor configuration /// @return Resulting status @@ -363,6 +365,35 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @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); + /// @brief Getting data with PTC_COMMAND_GET_INVENTORY_INFO /// @param target_tcp_driver TcpDriver used /// @param callback Callback function for received HesaiInventory @@ -935,11 +966,21 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @return Resulting status HesaiStatus CheckAndSetConfig(); + /// @brief Convert to model in Hesai protocol from nebula::drivers::SensorModel + /// @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 /// target model) /// @param model Model number void SetTargetModel(int model); + /// @brief Set target model number (for proper use of HTTP and TCP according to the support of the + /// target model) + /// @param model Model + void SetTargetModel(nebula::drivers::SensorModel model); + /// @brief Whether to use HTTP for setting SpinRate /// @param model Model number /// @return Use HTTP 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 430bcb706..29c3f69a0 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,5 +1,12 @@ #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" +//#define WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE + +#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE +#include +#include +#endif + namespace nebula { namespace drivers @@ -14,6 +21,53 @@ HesaiHwInterface::HesaiHwInterface() scan_cloud_ptr_{std::make_unique()} { } +HesaiHwInterface::~HesaiHwInterface() +{ +#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE + std::cout << ".......................st: HesaiHwInterface::~HesaiHwInterface()" << std::endl; +#endif + 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()) + { +#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE + std::cout << ".......................st: tcp_driver_->close();" << std::endl; +#endif + tcp_driver_->close(); +#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE + std::cout << ".......................ed: tcp_driver_->close();" << std::endl; +#endif + } +#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 + std::cout << ".......................ed: HesaiHwInterface::~HesaiHwInterface()" << std::endl; +#endif +} Status HesaiHwInterface::SetSensorConfiguration( std::shared_ptr sensor_configuration) @@ -84,11 +138,23 @@ Status HesaiHwInterface::CloudInterfaceStart() std::cout << "Starting UDP server on: " << *sensor_configuration_ << std::endl; cloud_udp_driver_->init_receiver( sensor_configuration_->host_ip, sensor_configuration_->data_port); +#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE + PrintError("init ok"); +#endif cloud_udp_driver_->receiver()->open(); +#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE + PrintError("open ok"); +#endif cloud_udp_driver_->receiver()->bind(); +#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE + PrintError("bind ok"); +#endif cloud_udp_driver_->receiver()->asyncReceive( std::bind(&HesaiHwInterface::ReceiveCloudPacketCallback, this, std::placeholders::_1)); +#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE + PrintError("async receive set"); +#endif } catch (const std::exception & ex) { Status status = Status::UDP_CONNECTION_ERROR; std::cerr << status << sensor_configuration_->sensor_ip << "," @@ -178,19 +244,37 @@ Status HesaiHwInterface::GetCalibrationConfiguration( Status HesaiHwInterface::InitializeTcpDriver(bool setup_sensor) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "HesaiHwInterface::InitializeTcpDriver" << std::endl; + std::cout << "HesaiHwInterface::InitializeTcpDriver, setup_sensor=" << setup_sensor << 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; + std::cout << "PandarTcpCommandPort=" << PandarTcpCommandPort << std::endl; #endif tcp_driver_->init_socket( sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, PandarTcpCommandPort); +#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE + std::cout << "ed: tcp_driver_->init_socket" << std::endl; +#endif if (!tcp_driver_->open()) { +#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE + std::cout << "!tcp_driver_->open()" << std::endl; +#endif +// tcp_driver_->close(); + tcp_driver_->closeSync(); return Status::ERROR_1; } - if (setup_sensor) { + 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; } } @@ -235,18 +319,18 @@ Status HesaiHwInterface::syncGetLidarCalibration( // if (!CheckLock(tm_, tm_fail_cnt, tm_fail_cnt_max, "GetLidarCalibration")) { // return GetLidarCalibration(target_tcp_driver, with_run); // } - PrintDebug("GetLidarCalibration: start"); + PrintDebug("syncGetLidarCalibration: start"); target_tcp_driver->syncSendReceiveHeaderPayload( buf_vec, - [this](const std::vector & received_bytes) { + [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; -#endif PrintDebug(received_bytes); +#endif }, [this, target_tcp_driver, bytes_callback](const std::vector & received_bytes) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE @@ -343,14 +427,14 @@ Status HesaiHwInterface::GetLidarCalibration( target_tcp_driver->asyncSendReceiveHeaderPayload( buf_vec, - [this](const std::vector & received_bytes) { + [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; -#endif PrintDebug(received_bytes); +#endif }, [this, target_tcp_driver, bytes_callback](const std::vector & received_bytes) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE @@ -964,6 +1048,135 @@ Status HesaiHwInterface::GetPtpDiagGrandmaster(bool with_run) 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); + + 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){ + 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]() { CheckUnlock(tm_, "syncGetInventory"); }); + + 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::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( + 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) @@ -1428,7 +1641,7 @@ Status HesaiHwInterface::SetSpinRate( Status HesaiHwInterface::SetSpinRate(uint16_t rpm, bool with_run) { if (with_run) { - if (tcp_driver_s_->GetIOContext()->stopped()) { + if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { tcp_driver_s_->GetIOContext()->restart(); } } @@ -1484,7 +1697,7 @@ Status HesaiHwInterface::SetSyncAngle( Status HesaiHwInterface::SetSyncAngle(int sync_angle, int angle, bool with_run) { if (with_run) { - if (tcp_driver_s_->GetIOContext()->stopped()) { + if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { tcp_driver_s_->GetIOContext()->restart(); } } @@ -1538,7 +1751,7 @@ Status HesaiHwInterface::SetTriggerMethod( Status HesaiHwInterface::SetTriggerMethod(int trigger_method, bool with_run) { if (with_run) { - if (tcp_driver_s_->GetIOContext()->stopped()) { + if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { tcp_driver_s_->GetIOContext()->restart(); } } @@ -1591,7 +1804,7 @@ Status HesaiHwInterface::SetStandbyMode( Status HesaiHwInterface::SetStandbyMode(int standby_mode, bool with_run) { if (with_run) { - if (tcp_driver_s_->GetIOContext()->stopped()) { + if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { tcp_driver_s_->GetIOContext()->restart(); } } @@ -1614,12 +1827,11 @@ Status HesaiHwInterface::SetReturnMode( buf_vec.emplace_back((len >> 0) & 0xff); 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")) { return SetReturnMode(target_tcp_driver, return_mode, with_run); } - PrintDebug("SetReturnMode: start"); - + PrintDebug("SetReturnMode: asyncSend"); target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tms_, "SetReturnMode"); }); if (with_run) { boost::system::error_code ec = target_tcp_driver->run(); @@ -1630,7 +1842,7 @@ Status HesaiHwInterface::SetReturnMode( std::cout << "ctx->run(): SetReturnMode" << std::endl; #endif } - + PrintDebug("SetReturnMode: done"); return Status::WAITING_FOR_SENSOR_RESPONSE; } Status HesaiHwInterface::SetReturnMode( @@ -1646,7 +1858,7 @@ Status HesaiHwInterface::SetReturnMode(int return_mode, bool with_run) { //* if (with_run) { - if (tcp_driver_s_->GetIOContext()->stopped()) { + if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { tcp_driver_s_->GetIOContext()->restart(); } } @@ -1712,7 +1924,7 @@ 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_->GetIOContext()->stopped()) { + if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { tcp_driver_s_->GetIOContext()->restart(); } } @@ -1791,7 +2003,7 @@ Status HesaiHwInterface::SetControlPort( bool with_run) { if (with_run) { - if (tcp_driver_s_->GetIOContext()->stopped()) { + if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { tcp_driver_s_->GetIOContext()->restart(); } } @@ -1854,7 +2066,7 @@ Status HesaiHwInterface::SetLidarRange( Status HesaiHwInterface::SetLidarRange(int method, std::vector data, bool with_run) { if (with_run) { - if (tcp_driver_s_->GetIOContext()->stopped()) { + if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { tcp_driver_s_->GetIOContext()->restart(); } } @@ -1919,7 +2131,7 @@ Status HesaiHwInterface::SetLidarRange( Status HesaiHwInterface::SetLidarRange(int start, int end, bool with_run) { if (with_run) { - if (tcp_driver_s_->GetIOContext()->stopped()) { + if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { tcp_driver_s_->GetIOContext()->restart(); } } @@ -2097,7 +2309,7 @@ Status HesaiHwInterface::SetClockSource(int clock_source, bool with_run) { //* if (with_run) { - if (tcp_driver_s_->GetIOContext()->stopped()) { + if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { tcp_driver_s_->GetIOContext()->restart(); } } @@ -2174,7 +2386,7 @@ Status HesaiHwInterface::SetPtpConfig( int logMinDelayReqInterval, bool with_run) { if (with_run) { - if (tcp_driver_s_->GetIOContext()->stopped()) { + if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { tcp_driver_s_->GetIOContext()->restart(); } } @@ -2276,7 +2488,7 @@ Status HesaiHwInterface::GetPtpConfig(std::shared_ptr c Status HesaiHwInterface::GetPtpConfig(bool with_run) { if (with_run) { - if (tcp_driver_s_->GetIOContext()->stopped()) { + if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { tcp_driver_s_->GetIOContext()->restart(); } } @@ -2327,7 +2539,7 @@ Status HesaiHwInterface::SendReset(bool with_run) { //* if (with_run) { - if (tcp_driver_s_->GetIOContext()->stopped()) { + if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { tcp_driver_s_->GetIOContext()->restart(); } } @@ -2381,7 +2593,7 @@ Status HesaiHwInterface::SetRotDir( Status HesaiHwInterface::SetRotDir(int mode, bool with_run) { if (with_run) { - if (tcp_driver_s_->GetIOContext()->stopped()) { + if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { tcp_driver_s_->GetIOContext()->restart(); } } @@ -2699,13 +2911,18 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( if (sensor_configuration->return_mode != current_return_mode) { std::stringstream ss; ss << current_return_mode; - PrintInfo("current lidar return_mode: " + ss.str()); + PrintInfo("Current LiDAR return_mode: " + ss.str()); std::stringstream ss2; ss2 << sensor_configuration->return_mode; - PrintInfo("current configuration return_mode: " + ss2.str()); + PrintInfo("Current Configuration return_mode: " + ss2.str()); std::thread t([this, sensor_configuration] { - SetReturnMode(nebula::drivers::IntFromReturnModeHesai( - sensor_configuration->return_mode, sensor_configuration->sensor_model)); + 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(); } @@ -2947,7 +3164,57 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig() return Status::WAITING_FOR_SENSOR_RESPONSE; } +/* +0: Pandar40P +2: Pandar64 +3: Pandar 128 +15: PandarQT +17: Pandar40M +20: PandarMind64 (Pandar64) +25: Pandar XT32 +26: Pandar XT16 +32: QT128C2X +38: ? +40: AT128? +48: ? +*/ +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 + return 15; + case SensorModel::HESAI_PANDAR40M: + return 17; + case SensorModel::HESAI_PANDARXT32: + return 25; + case SensorModel::HESAI_PANDARQT128: + return 32; + case SensorModel::HESAI_PANDARXT32M: + return 38; + case SensorModel::HESAI_PANDAR128_E3X://check required + return 40; + case SensorModel::HESAI_PANDAR128_E4X://check required + return 40; + case SensorModel::HESAI_PANDARAT128: + return 48; + case SensorModel::VELODYNE_VLS128: + case SensorModel::VELODYNE_HDL64: + case SensorModel::VELODYNE_VLP32: + case SensorModel::VELODYNE_VLP32MR: + case SensorModel::VELODYNE_HDL32: + case SensorModel::VELODYNE_VLP16: + case SensorModel::UNKNOWN: + return -1; + } + return -1; +} 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) { @@ -3032,15 +3299,43 @@ 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(); - tcp_driver_->close(); - tcp_driver_->open(); - tcp_driver_s_->close(); - tcp_driver_s_->open(); + if (tcp_driver_ && tcp_driver_->isOpen()) { + 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/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp index dd65699bd..91caca085 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp @@ -80,6 +80,8 @@ class HesaiHwInterfaceRosWrapper final : public rclcpp::Node, NebulaHwInterfaceW Status GetParameters(drivers::HesaiSensorConfiguration & sensor_configuration); private: + uint16_t delay_hw_ms_; + bool retry_hw_; std::mutex mtx_config_; OnSetParametersCallbackHandle::SharedPtr set_param_res_; /// @brief rclcpp parameter callback diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp index b6484e73a..857f638cd 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp @@ -124,6 +124,7 @@ class HesaiHwMonitorRosWrapper final : public rclcpp::Node, NebulaHwMonitorWrapp uint8_t current_monitor_status; uint16_t diag_span_; + uint16_t delay_monitor_ms_; std::mutex mtx_diag; std::mutex mtx_status; std::mutex mtx_lidar_monitor; diff --git a/nebula_ros/launch/hesai_launch_all_hw.xml b/nebula_ros/launch/hesai_launch_all_hw.xml index 5a959513c..a04333a59 100644 --- a/nebula_ros/launch/hesai_launch_all_hw.xml +++ b/nebula_ros/launch/hesai_launch_all_hw.xml @@ -20,6 +20,9 @@ + + + @@ -49,6 +52,8 @@ + + + diff --git a/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp index b9037b129..d8e206995 100644 --- a/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp @@ -41,7 +41,7 @@ HesaiDriverRosWrapper::HesaiDriverRosWrapper(const rclcpp::NodeOptions & options std::static_pointer_cast(calibration_cfg_ptr_)); } - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Wrapper=" << wrapper_status_); pandar_scan_sub_ = create_subscription( "pandar_packets", rclcpp::SensorDataQoS(), std::bind(&HesaiDriverRosWrapper::ReceiveScanMsgCallback, this, std::placeholders::_1)); @@ -57,7 +57,6 @@ void HesaiDriverRosWrapper::ReceiveScanMsgCallback( const pandar_msgs::msg::PandarScan::SharedPtr scan_msg) { auto t_start = std::chrono::high_resolution_clock::now(); - std::tuple pointcloud_ts = driver_ptr_->ConvertScanToPointcloud(scan_msg); nebula::drivers::NebulaPointCloudPtr pointcloud = std::get<0>(pointcloud_ts); @@ -258,45 +257,192 @@ 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; if (sensor_configuration.sensor_model != drivers::SensorModel::HESAI_PANDARAT128) { - if (calibration_configuration.calibration_file.empty()) { - RCLCPP_ERROR_STREAM( - this->get_logger(), - "Empty Calibration_file File: '" << calibration_configuration.calibration_file << "'"); - return Status::INVALID_CALIBRATION_FILE; - } else { - auto cal_status = - calibration_configuration.LoadFromFile(calibration_configuration.calibration_file); - - if (cal_status != Status::OK) { - RCLCPP_ERROR_STREAM( - this->get_logger(), - "Given Calibration File: '" << calibration_configuration.calibration_file << "'"); - return cal_status; - } else { - RCLCPP_INFO_STREAM( - this->get_logger(), - "Load calibration data from: '" << calibration_configuration.calibration_file << "'"); + std::string calibration_file_path_from_sensor; + if (!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{ + 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 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 << "'"); } - } else { - if (correction_file_path.empty()) { - RCLCPP_ERROR_STREAM( - this->get_logger(), "Empty Correction File: '" << correction_file_path << "'"); - return Status::INVALID_CALIBRATION_FILE; - } else { - auto cal_status = correction_configuration.LoadFromFile(correction_file_path); + if(run_local) { + bool run_org = false; + if (calibration_file_path_from_sensor.empty()) { + run_org = true; + } else { + auto cal_status = + calibration_configuration.LoadFromFile(calibration_file_path_from_sensor); + + if (cal_status != Status::OK) { + run_org = true; + }else{ + RCLCPP_INFO_STREAM( + this->get_logger(), "Load calibration data from: '" + << calibration_file_path_from_sensor << "'"); + } + } + if(run_org) { + if (calibration_configuration.calibration_file.empty()) { + RCLCPP_ERROR_STREAM( + this->get_logger(), "Empty Calibration_file File: '" << calibration_configuration.calibration_file << "'"); + return Status::INVALID_CALIBRATION_FILE; + } else { + auto cal_status = + calibration_configuration.LoadFromFile(calibration_configuration.calibration_file); - if (cal_status != Status::OK) { - RCLCPP_ERROR_STREAM( - this->get_logger(), "Given Correction File: '" << correction_file_path << "'"); - return cal_status; + if (cal_status != Status::OK) { + RCLCPP_ERROR_STREAM( + this->get_logger(), + "Given Calibration File: '" << calibration_configuration.calibration_file << "'"); + return cal_status; + }else{ + RCLCPP_INFO_STREAM( + this->get_logger(), "Load calibration data from: '" + << calibration_configuration.calibration_file << "'"); + } + } + } + } + } else { // sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128 + run_local = true; + std::string correction_file_path_from_sensor; + if (!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) { + 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; + } + }); + }else{ + RCLCPP_ERROR_STREAM(get_logger(), "InitializeTcpDriver failed. 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) { + bool run_org = false; + if (correction_file_path_from_sensor.empty()) { + run_org = true; } else { - RCLCPP_INFO_STREAM( - this->get_logger(), "Load correction data from: '" << correction_file_path << "'"); + auto cal_status = + correction_configuration.LoadFromFile(correction_file_path_from_sensor); + + if (cal_status != Status::OK) { + run_org = true; + }else{ + RCLCPP_INFO_STREAM( + this->get_logger(), "Load correction data from: '" + << correction_file_path_from_sensor << "'"); + } + } + if(run_org) { + if (correction_file_path.empty()) { + RCLCPP_ERROR_STREAM( + this->get_logger(), "Empty Correction File: '" << correction_file_path << "'"); + return Status::INVALID_CALIBRATION_FILE; + } else { + auto cal_status = correction_configuration.LoadFromFile(correction_file_path); + + if (cal_status != Status::OK) { + RCLCPP_ERROR_STREAM( + this->get_logger(), "Given Correction File: '" << correction_file_path << "'"); + return cal_status; + }else{ + RCLCPP_INFO_STREAM( + this->get_logger(), "Load correction data from: '" + << correction_file_path << "'"); + } + } } } - } + } // end AT128 + // Do not use outside of this location + hw_interface_.~HesaiHwInterface(); RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); return Status::OK; } 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 6a3beb38c..f3d706c09 100644 --- a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp @@ -15,30 +15,56 @@ HesaiHwInterfaceRosWrapper::HesaiHwInterfaceRosWrapper(const rclcpp::NodeOptions RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); return; } + std::this_thread::sleep_for(std::chrono::milliseconds(this->delay_hw_ms_)); hw_interface_.SetLogger(std::make_shared(this->get_logger())); std::shared_ptr sensor_cfg_ptr = std::make_shared(sensor_configuration_); - if (this->setup_sensor) { - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - } + hw_interface_.SetSensorConfiguration( + std::static_pointer_cast(sensor_cfg_ptr)); #if not defined(TEST_PCAP) - hw_interface_.InitializeTcpDriver(this->setup_sensor); + Status rt = hw_interface_.InitializeTcpDriver(this->setup_sensor); + if(this->retry_hw_) + { + int cnt = 0; + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << " Retry: " << cnt); + while(rt == Status::ERROR_1) + { + 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); + } + } - std::vector thread_pool{}; - thread_pool.emplace_back([this] { - hw_interface_.GetInventory( // ios, - [this](HesaiInventory & result) { - std::cout << result << std::endl; - hw_interface_.SetTargetModel(result.model); - }); - }); - for (std::thread & th : thread_pool) { - th.join(); + if(rt != Status::ERROR_1){ + 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); + }); + }); + for (std::thread & th : thread_pool) { + th.join(); + } + + } + catch (...) + { + std::cout << "catch (...) in parent" << std::endl; + RCLCPP_ERROR_STREAM(get_logger(), "Failed to get model from sensor..."); + } + if (this->setup_sensor) { + hw_interface_.CheckAndSetConfig(); + updateParameters(); + } } - if (this->setup_sensor) { - hw_interface_.CheckAndSetConfig(); - updateParameters(); + else + { + RCLCPP_ERROR_STREAM(get_logger(), "Failed to get model from sensor... Set from config: " << sensor_cfg_ptr->sensor_model); + hw_interface_.SetTargetModel(sensor_cfg_ptr->sensor_model); } #endif @@ -302,6 +328,25 @@ Status HesaiHwInterfaceRosWrapper::GetParameters( this->setup_sensor = this->get_parameter("setup_sensor").as_bool(); } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = "milliseconds"; + this->declare_parameter("delay_hw_ms", 0, descriptor); + this->delay_hw_ms_ = this->get_parameter("delay_hw_ms").as_int(); + } + { + 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("retry_hw", true, descriptor); + this->retry_hw_ = this->get_parameter("retry_hw").as_bool(); + } + if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { return Status::INVALID_SENSOR_MODEL; } 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 0c77058f8..82be03022 100644 --- a/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp @@ -18,12 +18,9 @@ HesaiHwMonitorRosWrapper::HesaiHwMonitorRosWrapper(const rclcpp::NodeOptions & o RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); return; } - hw_interface_.SetLogger(std::make_shared(this->get_logger())); + std::this_thread::sleep_for(std::chrono::milliseconds(this->delay_monitor_ms_)); std::shared_ptr sensor_cfg_ptr = std::make_shared(sensor_configuration_); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - hw_interface_.InitializeTcpDriver(); message_sep = ": "; not_supported_message = "Not supported"; @@ -61,6 +58,13 @@ HesaiHwMonitorRosWrapper::HesaiHwMonitorRosWrapper(const rclcpp::NodeOptions & o break; } + 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) + { + std::this_thread::sleep_for(std::chrono::milliseconds(8000));// >5000 + } std::vector thread_pool{}; thread_pool.emplace_back([this] { hw_interface_.GetInventory( // ios, @@ -262,7 +266,17 @@ Status HesaiHwMonitorRosWrapper::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = "milliseconds"; this->declare_parameter("diag_span", 1000, descriptor); - diag_span_ = this->get_parameter("diag_span").as_int(); + this->diag_span_ = this->get_parameter("diag_span").as_int(); + } + + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = "milliseconds"; + this->declare_parameter("delay_monitor_ms", 0, descriptor); + this->delay_monitor_ms_ = this->get_parameter("delay_monitor_ms").as_int(); } RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration);