feat(hesai): add TSN settings for Hesai sensors (OT128) (#125)

* feat: add tsn settings for hesai (AT128 and OT128) Signed-off-by: David Wong <[email protected]> * fix: special handling for OT128 needing a different definition for PTP profile Signed-off-by: David Wong <[email protected]> * fix: move defaults in ptp config functions to header files Signed-off-by: David Wong <[email protected]> * fix: correctly assign header size in PTP config Signed-off-by: David Wong <[email protected]> * fix: spelling miss Signed-off-by: David Wong <[email protected]> --------- Signed-off-by: David Wong <[email protected]>
tier4 · Mar 14, 2024 · 490069e · 490069e
1 parent 45c01cf
commit 490069e
Show file tree

Hide file tree

Showing 8 changed files with 115 additions and 28 deletions.
diff --git 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
@@ -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<PointXYZIR>::Ptr convertPointXYZIRADTToPointXYZIR(
   const pcl::PointCloud<PointXYZIRADT>::ConstPtr & input_pointcloud);
 

diff --git a/...interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/...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<boost::asio::io_context> ctx, int profile, int domain, int network,
-    int logAnnounceInterval, int logSyncInterval, int logMinDelayReqInterval, bool with_run = true);
+    std::shared_ptr<boost::asio::io_context> 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
@@ -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<unsigned char> 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<int>(PtpProfile::IEEE_802_1AS_AUTO)) {
+      return Status::SENSOR_CONFIG_ERROR;
+    }
+    profile = 3;
+  }
+
+  std::vector<unsigned char> 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,29 +2385,29 @@ Status HesaiHwInterface::SetPtpConfig(
 }
 Status HesaiHwInterface::SetPtpConfig(
   std::shared_ptr<boost::asio::io_context> 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()) {
       tcp_driver_s_->GetIOContext()->restart();
     }
   }
   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<int>(hesai_config.sync_angle / 100);
     auto scan_phase = static_cast<int>(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<int>(sensor_configuration->ptp_profile),
                    sensor_configuration->ptp_domain,
                    static_cast<int>(sensor_configuration->ptp_transport_type),
+                   static_cast<int>(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
@@ -26,6 +26,7 @@
     <arg name="ptp_profile" default="1588v2" description="1588v2|802.1as|automotive"/>
     <arg name="ptp_domain" default="0" description="PTP Domain [0-127]."/>
     <arg name="ptp_transport_type" default="UDP" description="1588v2 supports 'UDP' or 'L2', other profiles only L2 (HW)"/>
+    <arg name="ptp_switch_type" default="TSN" description="For automotive profile,'TSN' or 'NON_TSN'"/>
     <arg name="delay_hw_ms" default="1000" description="hw driver startup delay in milliseconds."/>
     <arg name="delay_monitor_ms" default="2000" description="hw monitor startup delay in milliseconds."/>
     <arg name="retry_hw" default="True" description="hw driver startup retry (false when using pcap)."/>
@@ -68,6 +69,7 @@
             <param name="ptp_profile" value="$(var ptp_profile)"/>
             <param name="ptp_domain" value="$(var ptp_domain)"/>
             <param name="ptp_transport_type" value="$(var ptp_transport_type)"/>
+            <param name="ptp_switch_type" value="$(var ptp_switch_type)"/>
         </node>
 
         <node pkg="nebula_ros" exec="hesai_hw_monitor_ros_wrapper_node"

diff --git a/nebula_ros/launch/hesai_launch_component.launch.xml b/nebula_ros/launch/hesai_launch_component.launch.xml
@@ -26,6 +26,7 @@
     <arg name="ptp_profile" default="1588v2" description="1588v2|802.1as|automotive"/>
     <arg name="ptp_domain" default="0" description="PTP Domain [0-127]."/>
     <arg name="ptp_transport_type" default="UDP" description="1588v2 supports 'UDP' or 'L2', other profiles only L2 (HW)"/>
+    <arg name="ptp_switch_type" default="TSN" description="For automotive profile,'TSN' or 'NON_TSN'"/>
     <arg name="delay_hw_ms" default="1000" description="hw driver startup delay in milliseconds."/>
     <arg name="delay_monitor_ms" default="2000" description="hw monitor startup delay in milliseconds."/>
     <arg name="retry_hw" default="True" description="hw driver startup retry (false when using pcap)."/>
@@ -80,6 +81,7 @@
                 <param name="ptp_profile" value="$(var ptp_profile)"/>
                 <param name="ptp_domain" value="$(var ptp_domain)"/>
                 <param name="ptp_transport_type" value="$(var ptp_transport_type)"/>
+                <param name="ptp_switch_type" value="$(var ptp_switch_type)"/>
                 <extra_arg name="use_intra_process_comms" value="true" />
             </composable_node>
         </load_composable_node>

diff --git 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
@@ -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<std::string>("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;
   }