From 490069efac8610c0dfb3cb8ab53a549fb87a32fa Mon Sep 17 00:00:00 2001 From: David Wong <33114676+drwnz@users.noreply.github.com> Date: Thu, 14 Mar 2024 19:48:32 +0900 Subject: [PATCH] feat(hesai): add TSN settings for Hesai sensors (OT128) (#125) * feat: add tsn settings for hesai (AT128 and OT128) Signed-off-by: David Wong * fix: special handling for OT128 needing a different definition for PTP profile Signed-off-by: David Wong * fix: move defaults in ptp config functions to header files Signed-off-by: David Wong * fix: correctly assign header size in PTP config Signed-off-by: David Wong * fix: spelling miss Signed-off-by: David Wong --------- Signed-off-by: David Wong --- .../nebula_common/hesai/hesai_common.hpp | 4 +- .../include/nebula_common/nebula_common.hpp | 41 ++++++++++++ .../hesai_hw_interface.hpp | 13 ++-- .../hesai_hw_interface.cpp | 63 ++++++++++++------- nebula_ros/launch/hesai_launch_all_hw.xml | 2 + .../launch/hesai_launch_component.launch.xml | 2 + nebula_ros/launch/nebula_launch.py | 3 + .../hesai/hesai_hw_interface_ros_wrapper.cpp | 15 +++++ 8 files changed, 115 insertions(+), 28 deletions(-) diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index ad1dcdf57..154034111 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_common.hpp +++ b/nebula_common/include/nebula_common/hesai/hesai_common.hpp @@ -25,6 +25,7 @@ struct HesaiSensorConfiguration : SensorConfigurationBase PtpProfile ptp_profile; uint8_t ptp_domain; PtpTransportType ptp_transport_type; + PtpSwitchType ptp_switch_type; }; /// @brief Convert HesaiSensorConfiguration to string (Overloading the << operator) /// @param os @@ -37,7 +38,8 @@ inline std::ostream & operator<<(std::ostream & os, HesaiSensorConfiguration con << ", FOV(Start):" << arg.cloud_min_angle << ", FOV(End):" << arg.cloud_max_angle << ", DualReturnDistanceThreshold:" << arg.dual_return_distance_threshold << ", PtpProfile:" << arg.ptp_profile << ", PtpDomain:" << std::to_string(arg.ptp_domain) - << ", PtpTransportType:" << arg.ptp_transport_type; + << ", PtpTransportType:" << arg.ptp_transport_type + << ", PtpSwitchType:" << arg.ptp_switch_type; return os; } diff --git a/nebula_common/include/nebula_common/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp index 70466c141..563f166ab 100644 --- a/nebula_common/include/nebula_common/nebula_common.hpp +++ b/nebula_common/include/nebula_common/nebula_common.hpp @@ -354,6 +354,12 @@ enum class PtpTransportType { UNKNOWN_TRANSPORT }; +enum class PtpSwitchType { + NON_TSN = 0, + TSN, + UNKNOWN_SWITCH +}; + /// @brief not used? struct PointField { @@ -643,6 +649,41 @@ inline std::ostream & operator<<(std::ostream & os, nebula::drivers::PtpTranspor return os; } +/// @brief Converts String to PTP SwitchType +/// @param switch_type Switch as String +/// @return Corresponding PtpSwitchType +inline PtpSwitchType PtpSwitchTypeFromString(const std::string & switch_type) +{ + // Hesai + auto tmp_str = switch_type; + std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), + [](unsigned char c){ return std::tolower(c); }); + if (tmp_str == "tsn") return PtpSwitchType::TSN; + if (tmp_str == "non_tsn") return PtpSwitchType::NON_TSN; + + return PtpSwitchType::UNKNOWN_SWITCH; +} + +/// @brief Convert PtpSwitchType enum to string (Overloading the << operator) +/// @param os +/// @param arg +/// @return stream +inline std::ostream & operator<<(std::ostream & os, nebula::drivers::PtpSwitchType const & arg) +{ + switch (arg) { + case PtpSwitchType::TSN: + os << "TSN"; + break; + case PtpSwitchType::NON_TSN: + os << "NON_TSN"; + break; + case PtpSwitchType::UNKNOWN_SWITCH: + os << "UNKNOWN"; + break; + } + return os; +} + [[maybe_unused]] pcl::PointCloud::Ptr convertPointXYZIRADTToPointXYZIR( const pcl::PointCloud::ConstPtr & input_pointcloud); diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp index c9c54896a..01081e320 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp @@ -776,6 +776,7 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @param profile IEEE timing and synchronization standard /// @param domain Domain attribute of the local clock /// @param network Network transport type of 1588v2 + /// @param switch_type Switch type of 802.1AS Automotive /// @param logAnnounceInterval Time interval between Announce messages, in units of log seconds /// (default: 1) /// @param logSyncInterval Time interval between Sync messages, in units of log seconds (default: @@ -786,13 +787,14 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @return Resulting status Status SetPtpConfig( std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int profile, int domain, - int network, int logAnnounceInterval, int logSyncInterval, int logMinDelayReqInterval, + int network, int switch_type, int logAnnounceInterval = 1, int logSyncInterval = 1, int logMinDelayReqInterval = 0, bool with_run = true); /// @brief Setting values with PTC_COMMAND_SET_PTP_CONFIG /// @param ctx IO Context used /// @param profile IEEE timing and synchronization standard /// @param domain Domain attribute of the local clock /// @param network Network transport type of 1588v2 + /// @param switch_type Switch type of 802.1AS Automotive /// @param logAnnounceInterval Time interval between Announce messages, in units of log seconds /// (default: 1) /// @param logSyncInterval Time interval between Sync messages, in units of log seconds (default: @@ -802,12 +804,13 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status Status SetPtpConfig( - std::shared_ptr ctx, int profile, int domain, int network, - int logAnnounceInterval, int logSyncInterval, int logMinDelayReqInterval, bool with_run = true); + std::shared_ptr ctx, int profile, int domain, int network, int switch_type, + int logAnnounceInterval = 1, int logSyncInterval = 1, int logMinDelayReqInterval = 0, bool with_run = true); /// @brief Setting values with PTC_COMMAND_SET_PTP_CONFIG /// @param profile IEEE timing and synchronization standard /// @param domain Domain attribute of the local clock /// @param network Network transport type of 1588v2 + /// @param switch_type Switch type of 802.1AS Automotive /// @param logAnnounceInterval Time interval between Announce messages, in units of log seconds /// (default: 1) /// @param logSyncInterval Time interval between Sync messages, in units of log seconds (default: @@ -817,8 +820,8 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status Status SetPtpConfig( - int profile, int domain, int network, int logAnnounceInterval, int logSyncInterval, - int logMinDelayReqInterval, bool with_run = true); + int profile, int domain, int network, int switch_type, int logAnnounceInterval = 1, int logSyncInterval = 1, + int logMinDelayReqInterval = 0, bool with_run = true); /// @brief Getting data with PTC_COMMAND_GET_PTP_CONFIG /// @param target_tcp_driver TcpDriver used /// @param with_run Automatically executes run() of TcpDriver diff --git a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp index a5589f4e7..6be12fc22 100644 --- a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp @@ -2316,17 +2316,33 @@ Status HesaiHwInterface::SetClockSource(int clock_source, bool with_run) Status HesaiHwInterface::SetPtpConfig( std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int profile, int domain, - int network, int logAnnounceInterval = 1, int logSyncInterval = 1, int logMinDelayReqInterval = 0, - bool with_run) + int network, int switch_type, int logAnnounceInterval, int logSyncInterval, + int logMinDelayReqInterval, bool with_run) { - std::vector buf_vec; - int len = 6; - if (profile == 0) { - } else if (profile >= 1) { - len = 3; - } else { + if (profile < 0 || profile > 3) { return Status::ERROR_1; } + // Handle the OT128 differently - it has TSN settings and defines the PTP profile + // for automotive as 0x03 instead of 0x02 for other sensors. + if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR128_E4X) { + if (profile != static_cast(PtpProfile::IEEE_802_1AS_AUTO)) { + return Status::SENSOR_CONFIG_ERROR; + } + profile = 3; + } + + std::vector buf_vec; + int len = 3; + switch (profile) { + case 0: + len = 6; + break; + case 3: + len = 4; + break; + default: + len = 3; + } buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW); buf_vec.emplace_back(PTC_COMMAND_SET_PTP_CONFIG); // Cmd PTC_COMMAND_SET_PTP_CONFIG @@ -2335,7 +2351,6 @@ Status HesaiHwInterface::SetPtpConfig( buf_vec.emplace_back((len >> 16) & 0xff); buf_vec.emplace_back((len >> 8) & 0xff); buf_vec.emplace_back((len >> 0) & 0xff); - buf_vec.emplace_back((profile >> 0) & 0xff); buf_vec.emplace_back((domain >> 0) & 0xff); buf_vec.emplace_back((network >> 0) & 0xff); @@ -2344,11 +2359,14 @@ Status HesaiHwInterface::SetPtpConfig( buf_vec.emplace_back((logSyncInterval >> 0) & 0xff); buf_vec.emplace_back((logMinDelayReqInterval >> 0) & 0xff); } + else if (profile == 3) { + buf_vec.emplace_back((switch_type >> 0) & 0xff); + } if (!CheckLock(tms_, tms_fail_cnt, tms_fail_cnt_max, "SetPtpConfig")) { return SetPtpConfig( - target_tcp_driver, profile, domain, network, logAnnounceInterval, logSyncInterval, - logMinDelayReqInterval, with_run); + target_tcp_driver, profile, domain, network, switch_type, logAnnounceInterval, + logSyncInterval, logMinDelayReqInterval, with_run); } PrintDebug("SetPtpConfig: start"); @@ -2367,20 +2385,20 @@ Status HesaiHwInterface::SetPtpConfig( } Status HesaiHwInterface::SetPtpConfig( std::shared_ptr ctx, int profile, int domain, int network, - int logAnnounceInterval = 1, int logSyncInterval = 1, int logMinDelayReqInterval = 0, - bool with_run) + int switch_type, int logAnnounceInterval, int logSyncInterval, + int logMinDelayReqInterval, bool with_run) { auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx); tcp_driver_local->init_socket( sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip, PandarTcpCommandPort); return SetPtpConfig( - tcp_driver_local, profile, domain, network, logAnnounceInterval, logSyncInterval, - logMinDelayReqInterval, with_run); + tcp_driver_local, profile, domain, network, switch_type, logAnnounceInterval, + logSyncInterval, logMinDelayReqInterval, with_run); } Status HesaiHwInterface::SetPtpConfig( - int profile, int domain, int network, int logAnnounceInterval, int logSyncInterval, - int logMinDelayReqInterval, bool with_run) + int profile, int domain, int network, int switch_type, int logAnnounceInterval, + int logSyncInterval, int logMinDelayReqInterval, bool with_run) { if (with_run) { if (tcp_driver_s_ && tcp_driver_s_->GetIOContext()->stopped()) { @@ -2388,8 +2406,8 @@ Status HesaiHwInterface::SetPtpConfig( } } return SetPtpConfig( - tcp_driver_s_, profile, domain, network, logAnnounceInterval, logSyncInterval, - logMinDelayReqInterval, with_run); + tcp_driver_s_, profile, domain, network, switch_type, logAnnounceInterval, + logSyncInterval, logMinDelayReqInterval, with_run); } Status HesaiHwInterface::GetPtpConfig( @@ -2984,8 +3002,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( std::this_thread::sleep_for(wait_time); } - if (sensor_configuration->sensor_model != SensorModel::HESAI_PANDARAT128 - && sensor_configuration->sensor_model != SensorModel::HESAI_PANDARQT128) { + if (sensor_configuration->sensor_model != SensorModel::HESAI_PANDARAT128) { set_flg = true; auto sync_angle = static_cast(hesai_config.sync_angle / 100); auto scan_phase = static_cast(sensor_configuration->scan_phase); @@ -3016,11 +3033,13 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( std::ostringstream tmp_ostringstream; tmp_ostringstream << "Trying to set PTP Config: " << sensor_configuration->ptp_profile << ", Domain: " << std::to_string(sensor_configuration->ptp_domain) - << ", Transport: " << sensor_configuration->ptp_transport_type << " via TCP"; + << ", Transport: " << sensor_configuration->ptp_transport_type + << ", Switch Type: " << sensor_configuration->ptp_switch_type << " via TCP"; PrintInfo(tmp_ostringstream.str()); SetPtpConfig(static_cast(sensor_configuration->ptp_profile), sensor_configuration->ptp_domain, static_cast(sensor_configuration->ptp_transport_type), + static_cast(sensor_configuration->ptp_switch_type), PTP_LOG_ANNOUNCE_INTERVAL, PTP_SYNC_INTERVAL, PTP_LOG_MIN_DELAY_INTERVAL diff --git a/nebula_ros/launch/hesai_launch_all_hw.xml b/nebula_ros/launch/hesai_launch_all_hw.xml index 204d08c87..b26193414 100644 --- a/nebula_ros/launch/hesai_launch_all_hw.xml +++ b/nebula_ros/launch/hesai_launch_all_hw.xml @@ -26,6 +26,7 @@ + @@ -68,6 +69,7 @@ + + @@ -80,6 +81,7 @@ + diff --git a/nebula_ros/launch/nebula_launch.py b/nebula_ros/launch/nebula_launch.py index f22a9d6c3..e29dbb4f0 100644 --- a/nebula_ros/launch/nebula_launch.py +++ b/nebula_ros/launch/nebula_launch.py @@ -72,6 +72,7 @@ def launch_setup(context, *args, **kwargs): "ptp_profile": LaunchConfiguration("ptp_profile"), "ptp_domain": LaunchConfiguration("ptp_domain"), "ptp_transport_type": LaunchConfiguration("ptp_transport_type"), + "ptp_switch_type": LaunchConfiguration("ptp_switch_type"), }, ], ), @@ -111,6 +112,7 @@ def launch_setup(context, *args, **kwargs): "ptp_profile": LaunchConfiguration("ptp_profile"), "ptp_domain": LaunchConfiguration("ptp_domain"), "ptp_transport_type": LaunchConfiguration("ptp_transport_type"), + "ptp_switch_type": LaunchConfiguration("ptp_switch_type"), }, ], ), @@ -166,6 +168,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("ptp_profile", "1588v2"), add_launch_arg("ptp_domain", "0"), add_launch_arg("ptp_transport_type", "UDP"), + add_launch_arg("ptp_switch_type", "TSN"), ] + [OpaqueFunction(function=launch_setup)] ) diff --git a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp index df78c6a34..8a6060d21 100644 --- a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp @@ -369,6 +369,16 @@ Status HesaiHwInterfaceRosWrapper::GetParameters( sensor_configuration.ptp_transport_type = nebula::drivers::PtpTransportType::L2; } } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("ptp_switch_type", ""); + sensor_configuration.ptp_switch_type = + nebula::drivers::PtpSwitchTypeFromString(this->get_parameter("ptp_switch_type").as_string()); + } { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; @@ -390,6 +400,11 @@ Status HesaiHwInterfaceRosWrapper::GetParameters( "Invalid PTP Transport Provided. Please use 'udp' or 'l2', 'udp' is only available when using the '1588v2' PTP Profile"); return Status::SENSOR_CONFIG_ERROR; } + if(sensor_configuration.ptp_switch_type == nebula::drivers::PtpSwitchType::UNKNOWN_SWITCH) { + RCLCPP_ERROR_STREAM(get_logger(), + "Invalid PTP Switch Type Provided. Please use 'tsn' or 'non_tsn'"); + return Status::SENSOR_CONFIG_ERROR; + } if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { return Status::INVALID_SENSOR_MODEL; }