From 22498916bcf1420d226dcd846ede68f9ff20b398 Mon Sep 17 00:00:00 2001 From: Abraham Monrroy Cano Date: Sun, 30 Jul 2023 14:06:39 +0900 Subject: [PATCH] feat(sync_angle): hesai. Enable Clock Sync Features (#39) * hesai. Enable Sync Phase Signed-off-by: amc-nu * cspell fixes Signed-off-by: amc-nu * cspell fixes Signed-off-by: amc-nu * hesai. enable ptp clock source Signed-off-by: amc-nu * at128. enably clock features sync angle/ptp Signed-off-by: amc-nu --------- Signed-off-by: amc-nu Co-authored-by: David Wong <33114676+drwnz@users.noreply.github.com> --- .cspell.json | 9 +- .../include/nebula_common/nebula_status.hpp | 3 + .../hesai_hw_interface.hpp | 44 +++- .../hesai_hw_interface.cpp | 207 ++++++++++++++++-- .../hesai/hesai_hw_interface_ros_wrapper.hpp | 1 + .../hesai/hesai_hw_monitor_ros_wrapper.hpp | 6 +- .../velodyne_hw_interface_ros_wrapper.hpp | 4 +- .../velodyne_hw_monitor_ros_wrapper.hpp | 62 +++--- nebula_ros/launch/hesai_launch_all_hw.xml | 1 + .../hesai/hesai_hw_interface_ros_wrapper.cpp | 6 +- .../hesai/hesai_hw_monitor_ros_wrapper.cpp | 7 +- 11 files changed, 292 insertions(+), 58 deletions(-) diff --git a/.cspell.json b/.cspell.json index 4c299ad96..b485820c1 100644 --- a/.cspell.json +++ b/.cspell.json @@ -14,6 +14,13 @@ "AT", "XT", "QT", - "XTM" + "XTM", + "Pdelay", + "gprmc", + "Vccint", + "vccint", + "adctp", + "Adctp", + ] } diff --git a/nebula_common/include/nebula_common/nebula_status.hpp b/nebula_common/include/nebula_common/nebula_status.hpp index 6f14339ea..f36d8b7d2 100644 --- a/nebula_common/include/nebula_common/nebula_status.hpp +++ b/nebula_common/include/nebula_common/nebula_status.hpp @@ -70,6 +70,9 @@ struct Status case Status::HTTP_CONNECTION_ERROR: os << "Http Connection Error"; break; + case Status::WAITING_FOR_SENSOR_RESPONSE: + os << "Waiting for Sensor Response"; + break; case Status::ERROR_1: default: os << "Generic Error"; 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 c37fb8295..ebed3bc84 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 @@ -52,6 +52,7 @@ const uint8_t PTC_COMMAND_SET_SYNC_ANGLE = 0x18; const uint8_t PTC_COMMAND_SET_TRIGGER_METHOD = 0x1b; const uint8_t PTC_COMMAND_SET_STANDBY_MODE = 0x1c; const uint8_t PTC_COMMAND_SET_RETURN_MODE = 0x1e; +const uint8_t PTC_COMMAND_SET_CLOCK_SOURCE = 0x1f; const uint8_t PTC_COMMAND_SET_DESTINATION_IP = 0x20; const uint8_t PTC_COMMAND_SET_CONTROL_PORT = 0x21; const uint8_t PTC_COMMAND_SET_LIDAR_RANGE = 0x22; @@ -76,6 +77,16 @@ 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) + +const int HESAI_LIDAR_GPS_CLOCK_SOURCE = 0; +const int HESAI_LIDAR_PTP_CLOCK_SOURCE = 1; + /// @brief Hardware interface of hesai driver class HesaiHwInterface : NebulaHwInterfaceBase { @@ -104,7 +115,7 @@ class HesaiHwInterface : NebulaHwInterfaceBase int tm_fail_cnt_max = 0; std::timed_mutex tms_; int tms_fail_cnt = 0; - int tms_fail_cnt_max = 0; + int tms_fail_cnt_max = 3; bool wl = true; bool is_solid_state = false; int target_model_no; @@ -159,6 +170,9 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @param setup_sensor Whether to also initialize tcp_driver for sensor configuration /// @return Resulting status Status InitializeTcpDriver(bool setup_sensor = true); + /// @brief Closes the TcpDriver and related resources + /// @return Status result + Status FinalizeTcpDriver(); /// @brief Parsing json string to property_tree /// @param str JSON string /// @return Parsed property_tree @@ -724,6 +738,11 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status Status GetLidarRange(bool with_run = true); + + Status SetClockSource(std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int clock_source, bool with_run); + Status SetClockSource(std::shared_ptr ctx, int clock_source, bool with_run); + Status SetClockSource(int clock_source, bool with_run = true); + /// @brief Setting values with PTC_COMMAND_SET_PTP_CONFIG /// @param target_tcp_driver TcpDriver used /// @param profile IEEE timing and synchronization standard @@ -864,6 +883,29 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @param rpm spin_speed (300, 600, 1200) /// @return Resulting status HesaiStatus SetSpinSpeedAsyncHttp(uint16_t rpm); + + HesaiStatus SetPtpConfigSyncHttp( + std::shared_ptr ctx, + int profile, + int domain, + int network, + int logAnnounceInterval, + int logSyncInterval, + int logMinDelatReqInterval); + HesaiStatus SetPtpConfigSyncHttp(int profile, + int domain, + int network, + int logAnnounceInterval, + int logSyncInterval, + int logMinDelatReqInterval); + HesaiStatus SetSyncAngleSyncHttp( + std::shared_ptr ctx, + int enable, + int angle); + HesaiStatus SetSyncAngleSyncHttp(int enable, int angle); + + + /// @brief Getting lidar_monitor via HTTP API /// @param ctx IO Context /// @param str_callback Callback function for received string 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 9b3d964ae..aa78f5e01 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 @@ -84,15 +84,11 @@ 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); - PrintError("init ok"); cloud_udp_driver_->receiver()->open(); - PrintError("open ok"); cloud_udp_driver_->receiver()->bind(); - PrintError("bind ok"); cloud_udp_driver_->receiver()->asyncReceive( std::bind(&HesaiHwInterface::ReceiveCloudPacketCallback, this, std::placeholders::_1)); - PrintError("async receive set"); } catch (const std::exception & ex) { Status status = Status::UDP_CONNECTION_ERROR; std::cerr << status << sensor_configuration_->sensor_ip << "," @@ -201,6 +197,17 @@ Status HesaiHwInterface::InitializeTcpDriver(bool setup_sensor) return Status::OK; } +Status HesaiHwInterface::FinalizeTcpDriver() { + try { + tcp_driver_->close(); + } + catch(std::exception &e) { + PrintError("Error while finalizing the TcpDriver"); + return Status::UDP_CONNECTION_ERROR; + } + return Status::OK; +} + boost::property_tree::ptree HesaiHwInterface::ParseJson(const std::string & str) { boost::property_tree::ptree tree; @@ -2042,6 +2049,62 @@ Status HesaiHwInterface::GetLidarRange(bool with_run) [this](HesaiLidarRangeAll & result) { std::cout << result << std::endl; }, with_run); } +Status HesaiHwInterface::SetClockSource( + std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int clock_source, + bool with_run) +{ + std::vector buf_vec; + int len = 1; + buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); + buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); + buf_vec.emplace_back(PTC_COMMAND_SET_CLOCK_SOURCE); + buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE); + buf_vec.emplace_back((len >> 24) & 0xff); + buf_vec.emplace_back((len >> 16) & 0xff); + buf_vec.emplace_back((len >> 8) & 0xff); + buf_vec.emplace_back((len >> 0) & 0xff); + + buf_vec.emplace_back((clock_source >> 0) & 0xff); + + if (!CheckLock(tms_, tms_fail_cnt, tms_fail_cnt_max, "SetClockSource")) { + return SetClockSource(target_tcp_driver, clock_source, with_run); + } + PrintDebug("SetClockSource: start"); + + target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tms_, "SetClockSource"); }); + if (with_run) { + boost::system::error_code ec = target_tcp_driver->run(); + if (ec) { + PrintError("HesaiHwInterface::SetClockSource: " + ec.message()); + } +#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE + std::cout << "ctx->run(): SetReturnMode" << std::endl; +#endif + } + + return Status::WAITING_FOR_SENSOR_RESPONSE; +} +Status HesaiHwInterface::SetClockSource( + std::shared_ptr ctx, int clock_source, bool with_run) +{ + auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); + tcp_driver_local->init_socket( + sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, + PandarTcpCommandPort); + return SetClockSource(tcp_driver_local, clock_source, with_run); +} +Status HesaiHwInterface::SetClockSource(int clock_source, bool with_run) +{ + //* + if (with_run) { + if (tcp_driver_s_->GetIOContext()->stopped()) { + tcp_driver_s_->GetIOContext()->restart(); + } + } + //*/ + return SetClockSource(tcp_driver_s_, clock_source, with_run); +} + Status HesaiHwInterface::SetPtpConfig( std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int profile, int domain, int network, int logAnnounceInterval = 1, int logSyncInterval = 1, int logMinDelayReqInterval = 0, @@ -2521,6 +2584,82 @@ HesaiStatus HesaiHwInterface::SetSpinSpeedAsyncHttp(uint16_t rpm) return SetSpinSpeedAsyncHttp(std::make_shared(), rpm); } +HesaiStatus HesaiHwInterface::SetPtpConfigSyncHttp( + std::shared_ptr ctx, + int profile, + int domain, + int network, + int logAnnounceInterval, + int logSyncInterval, + int logMinDelatReqInterval) +{ + std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; + auto st = GetHttpClientDriverOnce(ctx, hcd); + if (st != Status::OK) { + return st; + } + + auto response = hcd->get( + (boost::format("/pandar.cgi?action=set&object=lidar&key=ptp_configuration&value={" \ + "\"Profile\": %d," \ + "\"Domain\": %d," \ + "\"Network\": %d," \ + "\"LogAnnounceInterval\": %d," \ + "\"LogSyncInterval\": %d," \ + "\"LogMinDelayReqInterval\": %d," \ + "\"tsn_switch\": %d" \ + "}") + % profile % domain % network % logAnnounceInterval % logSyncInterval %logMinDelatReqInterval % 0 + ).str()); + ctx->run(); + PrintInfo(response); + return Status::OK; +} + +HesaiStatus HesaiHwInterface::SetPtpConfigSyncHttp(int profile, + int domain, + int network, + int logAnnounceInterval, + int logSyncInterval, + int logMinDelatReqInterval) +{ + return SetPtpConfigSyncHttp(std::make_shared(), + profile, + domain, + network, + logAnnounceInterval, + logSyncInterval, + logMinDelatReqInterval); +} + +HesaiStatus HesaiHwInterface::SetSyncAngleSyncHttp( + std::shared_ptr ctx, + int enable, + int angle) +{ + std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; + auto st = GetHttpClientDriverOnce(ctx, hcd); + if (st != Status::OK) { + return st; + } + auto tmp_str = (boost::format("/pandar.cgi?action=set&object=lidar_sync&key=sync_angle&value={" \ + "\"sync\": %d," \ + "\"syncAngle\": %d" \ + "}") % enable % angle).str(); + PrintInfo(tmp_str); + auto response = hcd->get(tmp_str); + ctx->run(); + PrintInfo(response); + return Status::OK; +} + +HesaiStatus HesaiHwInterface::SetSyncAngleSyncHttp(int enable, int angle) +{ + return SetSyncAngleSyncHttp(std::make_shared(), + enable, + angle); +} + HesaiStatus HesaiHwInterface::GetLidarMonitorAsyncHttp( std::shared_ptr ctx, std::function str_callback) @@ -2528,7 +2667,7 @@ HesaiStatus HesaiHwInterface::GetLidarMonitorAsyncHttp( std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; auto st = GetHttpClientDriverOnce(ctx, hcd); if (st != Status::OK) { - PrintError("HesaiHwInterface::GetLidarMonitorAsyncHttp: cannnot GetHttpClientDriverOnce"); + PrintError("HesaiHwInterface::GetLidarMonitorAsyncHttp: cannot GetHttpClientDriverOnce"); return st; } @@ -2624,22 +2763,51 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( t.join(); } - set_flg = false; - auto sync_angle = static_cast(hesai_config.sync_angle / 100); - auto scan_phase = static_cast(sensor_configuration->scan_phase); - int sync_flg = 0 < scan_phase ? 1 : 0; - if (hesai_config.sync != sync_flg) { - set_flg = true; - } else if (0 < sync_flg && scan_phase != sync_angle) { + if (sensor_configuration->sensor_model != SensorModel::HESAI_PANDARAT128){ set_flg = true; - } - if (set_flg) { - PrintInfo("current lidar sync: " + std::to_string(hesai_config.sync)); - PrintInfo("current lidar sync_angle: " + std::to_string(hesai_config.sync_angle)); - PrintInfo("current configuration scan_phase: " + std::to_string(scan_phase)); - std::thread t([this, sync_flg, scan_phase] { SetSyncAngle(sync_flg, scan_phase); }); + auto sync_angle = static_cast(hesai_config.sync_angle / 100); + auto scan_phase = static_cast(sensor_configuration->scan_phase); + int sync_flg = 1; + if (scan_phase != sync_angle) { + set_flg = true; + } + if (sync_flg && set_flg) { + PrintInfo("current lidar sync: " + std::to_string(hesai_config.sync)); + PrintInfo("current lidar sync_angle: " + std::to_string(sync_angle)); + PrintInfo("current configuration scan_phase: " + std::to_string(scan_phase)); + std::thread t([this, sync_flg, scan_phase] { + SetSyncAngle(sync_flg, scan_phase); + }); + t.join(); + } + + 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, + PTP_LOG_ANNOUNCE_INTERVAL, + PTP_SYNC_INTERVAL, + PTP_LOG_MIN_DELAY_INTERVAL + ); + }); t.join(); } + 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, + PTP_LOG_ANNOUNCE_INTERVAL, + PTP_SYNC_INTERVAL, + PTP_LOG_MIN_DELAY_INTERVAL); + + } #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << "End CheckAndSetConfig(HesaiConfig)!!" << std::endl; @@ -2874,6 +3042,9 @@ bool HesaiHwInterface::CheckLock( tcp_driver_s_->close(); tcp_driver_s_->open(); } + else { + return true; + } return false; } fail_cnt = 0; 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 89edb66f1..fe1e43c79 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 @@ -64,6 +64,7 @@ class HesaiHwInterfaceRosWrapper final : public rclcpp::Node, NebulaHwInterfaceW public: explicit HesaiHwInterfaceRosWrapper(const rclcpp::NodeOptions & options); + ~HesaiHwInterfaceRosWrapper() noexcept override; /// @brief Start point cloud streaming (Call CloudInterfaceStart of HwInterface) /// @return Resulting status Status StreamStart() override; 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 ee945b4d5..b0d7f2a45 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 @@ -20,10 +20,10 @@ namespace nebula { namespace ros { -/// @brief Get parametor from rclcpp::Parameter +/// @brief Get parameter from rclcpp::Parameter /// @tparam T /// @param p Parameter from rclcpp parameter callback -/// @param name Target parametor name +/// @param name Target parameter name /// @param value Corresponding value /// @return Whether the target name existed template @@ -55,7 +55,7 @@ class HesaiHwMonitorRosWrapper final : public rclcpp::Node, NebulaHwMonitorWrapp public: explicit HesaiHwMonitorRosWrapper(const rclcpp::NodeOptions & options); - + ~HesaiHwMonitorRosWrapper() noexcept override; /// @brief Not used /// @return Current status Status MonitorStart() override; diff --git a/nebula_ros/include/nebula_ros/velodyne/velodyne_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/velodyne/velodyne_hw_interface_ros_wrapper.hpp index b9aa49fed..a5001b62f 100644 --- a/nebula_ros/include/nebula_ros/velodyne/velodyne_hw_interface_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/velodyne/velodyne_hw_interface_ros_wrapper.hpp @@ -22,10 +22,10 @@ namespace nebula { namespace ros { -/// @brief Get parametor from rclcpp::Parameter +/// @brief Get parameter from rclcpp::Parameter /// @tparam T /// @param p Parameter from rclcpp parameter callback -/// @param name Target parametor name +/// @param name Target parameter name /// @param value Corresponding value /// @return Whether the target name existed template diff --git a/nebula_ros/include/nebula_ros/velodyne/velodyne_hw_monitor_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/velodyne/velodyne_hw_monitor_ros_wrapper.hpp index dd98197e5..3f589bbbf 100644 --- a/nebula_ros/include/nebula_ros/velodyne/velodyne_hw_monitor_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/velodyne/velodyne_hw_monitor_ros_wrapper.hpp @@ -17,10 +17,10 @@ namespace nebula { namespace ros { -/// @brief Get parametor from rclcpp::Parameter +/// @brief Get parameter from rclcpp::Parameter /// @tparam T /// @param p Parameter from rclcpp parameter callback -/// @param name Target parametor name +/// @param name Target parameter name /// @param value Corresponding value /// @return Whether the target name existed template @@ -89,92 +89,92 @@ class VelodyneHwMonitorRosWrapper final : public rclcpp::Node, NebulaHwMonitorWr void OnVelodyneDiagnosticsTimer(); /// @brief Getting top:hv from the current property_tree - /// @return tuple + /// @return tuple std::tuple VelodyneGetTopHv(); /// @brief Getting top:ad_temp from the current property_tree (only VLP32) - /// @return tuple + /// @return tuple std::tuple VelodyneGetTopAdTemp(); // only32 /// @brief Getting top:lm20_temp from the current property_tree - /// @return tuple + /// @return tuple std::tuple VelodyneGetTopLm20Temp(); /// @brief Getting top:pwr_5v from the current property_tree - /// @return tuple + /// @return tuple std::tuple VelodyneGetTopPwr5v(); /// @brief Getting top:pwr_2_5v from the current property_tree - /// @return tuple + /// @return tuple std::tuple VelodyneGetTopPwr25v(); /// @brief Getting top:pwr_3_3v from the current property_tree - /// @return tuple + /// @return tuple std::tuple VelodyneGetTopPwr33v(); /// @brief Getting top:pwr_5v_raw from the current property_tree (only VLP16) - /// @return tuple + /// @return tuple std::tuple VelodyneGetTopPwr5vRaw(); // only16 /// @brief Getting top:pwr_raw from the current property_tree (only VLP32) - /// @return tuple + /// @return tuple std::tuple VelodyneGetTopPwrRaw(); // only32 /// @brief Getting top:pwr_vccint from the current property_tree - /// @return tuple + /// @return tuple std::tuple VelodyneGetTopPwrVccint(); /// @brief Getting bot:i_out from the current property_tree - /// @return tuple + /// @return tuple std::tuple VelodyneGetBotIOut(); /// @brief Getting bot:pwr_1_2v from the current property_tree - /// @return tuple + /// @return tuple std::tuple VelodyneGetBotPwr12v(); /// @brief Getting bot:lm20_temp from the current property_tree - /// @return tuple + /// @return tuple std::tuple VelodyneGetBotLm20Temp(); /// @brief Getting bot:pwr_5v from the current property_tree - /// @return tuple + /// @return tuple std::tuple VelodyneGetBotPwr5v(); /// @brief Getting bot:pwr_2_5v from the current property_tree - /// @return tuple + /// @return tuple std::tuple VelodyneGetBotPwr25v(); /// @brief Getting bot:pwr_3_3v from the current property_tree - /// @return tuple + /// @return tuple std::tuple VelodyneGetBotPwr33v(); /// @brief Getting bot:pwr_v_in from the current property_tree - /// @return tuple + /// @return tuple std::tuple VelodyneGetBotPwrVIn(); /// @brief Getting bot:pwr_1_25v from the current property_tree - /// @return tuple + /// @return tuple std::tuple VelodyneGetBotPwr125v(); /// @brief Getting vhv from the current property_tree - /// @return tuple + /// @return tuple std::tuple VelodyneGetVhv(); /// @brief Getting adc_nf from the current property_tree - /// @return tuple + /// @return tuple std::tuple VelodyneGetAdcNf(); /// @brief Getting adc_stats from the current property_tree - /// @return tuple + /// @return tuple std::tuple VelodyneGetAdcStats(); /// @brief Getting ixe from the current property_tree - /// @return tuple + /// @return tuple std::tuple VelodyneGetIxe(); /// @brief Getting adctp_stat from the current property_tree - /// @return tuple + /// @return tuple std::tuple VelodyneGetAdctpStat(); /// @brief Getting gps:pps_state from the current property_tree - /// @return tuple + /// @return tuple std::tuple VelodyneGetGpsPpsState(); /// @brief Getting gps:position from the current property_tree - /// @return tuple + /// @return tuple std::tuple VelodyneGetGpsPosition(); /// @brief Getting motor:state from the current property_tree - /// @return tuple + /// @return tuple std::tuple VelodyneGetMotorState(); /// @brief Getting motor:rpm from the current property_tree - /// @return tuple + /// @return tuple std::tuple VelodyneGetMotorRpm(); /// @brief Getting motor:lock from the current property_tree - /// @return tuple + /// @return tuple std::tuple VelodyneGetMotorLock(); /// @brief Getting motor:phase from the current property_tree - /// @return tuple + /// @return tuple std::tuple VelodyneGetMotorPhase(); /// @brief Getting laser:state from the current property_tree - /// @return tuple + /// @return tuple std::tuple VelodyneGetLaserState(); /// @brief Check top:hv from the current property_tree for diagnostic_updater diff --git a/nebula_ros/launch/hesai_launch_all_hw.xml b/nebula_ros/launch/hesai_launch_all_hw.xml index 024d4cbc5..5a959513c 100644 --- a/nebula_ros/launch/hesai_launch_all_hw.xml +++ b/nebula_ros/launch/hesai_launch_all_hw.xml @@ -27,6 +27,7 @@ + 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 9c9cf3e08..6a3beb38c 100644 --- a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp @@ -36,7 +36,6 @@ HesaiHwInterfaceRosWrapper::HesaiHwInterfaceRosWrapper(const rclcpp::NodeOptions for (std::thread & th : thread_pool) { th.join(); } - RCLCPP_DEBUG_STREAM(this->get_logger(), "hw_interface_.CheckAndSetConfig();"); if (this->setup_sensor) { hw_interface_.CheckAndSetConfig(); updateParameters(); @@ -122,6 +121,11 @@ HesaiHwInterfaceRosWrapper::HesaiHwInterfaceRosWrapper(const rclcpp::NodeOptions StreamStart(); } +HesaiHwInterfaceRosWrapper::~HesaiHwInterfaceRosWrapper() { + RCLCPP_INFO_STREAM(get_logger(), "Closing TcpDriver"); + hw_interface_.FinalizeTcpDriver(); +} + Status HesaiHwInterfaceRosWrapper::StreamStart() { if (Status::OK == interface_status_) { 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 23a75678f..e2fc38d60 100644 --- a/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp @@ -607,6 +607,11 @@ void HesaiHwMonitorRosWrapper::HesaiCheckVoltage( } } -RCLCPP_COMPONENTS_REGISTER_NODE(HesaiHwMonitorRosWrapper) + HesaiHwMonitorRosWrapper::~HesaiHwMonitorRosWrapper() { + RCLCPP_INFO_STREAM(get_logger(), "Closing TcpDriver"); + hw_interface_.FinalizeTcpDriver(); + } + + RCLCPP_COMPONENTS_REGISTER_NODE(HesaiHwMonitorRosWrapper) } // namespace ros } // namespace nebula