feat(hesai): multicast support (#187)

* chore(continental): add missing imports, fix symbols * chore(hesai): add missing imports * feat(hesai): add multicast support * chore(hesai): add multicast_ip parameter to schemas and param files * chore: fix missing dependencies that cause CI fail * chore(nebula_tests): fix diagnostic_updater dependency for all vendors * chore: correct default params for OT128 * chore(ars548): revert erroneously committed case style changes
tier4 · Sep 7, 2024 · 15c1e51 · 15c1e51
1 parent 723a853
commit 15c1e51
Show file tree

Hide file tree

Showing 20 changed files with 72 additions and 7 deletions.
diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp
@@ -34,6 +34,7 @@ namespace drivers
 /// @brief struct for Hesai sensor configuration
 struct HesaiSensorConfiguration : public LidarConfigurationBase
 {
+  std::string multicast_ip;
   uint16_t gnss_port{};
   double scan_phase{};
   double dual_return_distance_threshold{};
@@ -54,6 +55,8 @@ inline std::ostream & operator<<(std::ostream & os, HesaiSensorConfiguration con
 {
   os << "Hesai Sensor Configuration:" << '\n';
   os << (LidarConfigurationBase)(arg) << '\n';
+  os << "Multicast: "
+     << (arg.multicast_ip.empty() ? "disabled" : "enabled, group " + arg.multicast_ip) << '\n';
   os << "GNSS Port: " << arg.gnss_port << '\n';
   os << "Scan Phase: " << arg.scan_phase << '\n';
   os << "Rotation Speed: " << arg.rotation_speed << '\n';

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
@@ -149,8 +149,15 @@ Status HesaiHwInterface::SensorInterfaceStart()
 {
   try {
     std::cout << "Starting UDP server on: " << *sensor_configuration_ << std::endl;
-    cloud_udp_driver_->init_receiver(
-      sensor_configuration_->host_ip, sensor_configuration_->data_port);
+    if (sensor_configuration_->multicast_ip.empty()) {
+      cloud_udp_driver_->init_receiver(
+        sensor_configuration_->host_ip, sensor_configuration_->data_port);
+    } else {
+      cloud_udp_driver_->init_receiver(
+        sensor_configuration_->multicast_ip, sensor_configuration_->data_port,
+        sensor_configuration_->host_ip, sensor_configuration_->data_port);
+      cloud_udp_driver_->receiver()->setMulticast(true);
+    }
 #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE
     PrintError("init ok");
 #endif
@@ -179,8 +186,8 @@ Status HesaiHwInterface::SensorInterfaceStart()
 #endif
   } catch (const std::exception & ex) {
     Status status = Status::UDP_CONNECTION_ERROR;
-    std::cerr << status << sensor_configuration_->sensor_ip << ","
-              << sensor_configuration_->data_port << std::endl;
+    std::cerr << status << " for " << sensor_configuration_->sensor_ip << ":"
+              << sensor_configuration_->data_port << " - " << ex.what() << std::endl;
     return status;
   }
   return Status::OK;
@@ -853,10 +860,13 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig(
      << static_cast<int>(hesai_config.dest_ipaddr[2]) << "."
      << static_cast<int>(hesai_config.dest_ipaddr[3]);
   auto current_host_addr = ss.str();
-  if (sensor_configuration->host_ip != current_host_addr) {
+  auto desired_host_addr = sensor_configuration->multicast_ip.empty()
+                             ? sensor_configuration->host_ip
+                             : sensor_configuration->multicast_ip;
+  if (desired_host_addr != current_host_addr) {
     set_flg = true;
     PrintInfo("current lidar dest_ipaddr: " + current_host_addr);
-    PrintInfo("current configuration host_ip: " + sensor_configuration->host_ip);
+    PrintInfo("current configuration host_ip: " + desired_host_addr);
   }
 
   auto current_host_dport = hesai_config.dest_LiDAR_udp_port;
@@ -881,7 +891,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig(
 
   if (set_flg) {
     std::vector<std::string> list_string;
-    boost::split(list_string, sensor_configuration->host_ip, boost::is_any_of("."));
+    boost::split(list_string, desired_host_addr, boost::is_any_of("."));
     std::thread t([this, sensor_configuration, list_string] {
       SetDestinationIp(
         std::stoi(list_string[0]), std::stoi(list_string[1]), std::stoi(list_string[2]),

diff --git a/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml b/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml
@@ -2,6 +2,7 @@
   ros__parameters:
     host_ip: 255.255.255.255
     sensor_ip: 192.168.1.201
+    multicast_ip: ""
     data_port: 2368
     gnss_port: 10110
     packet_mtu_size: 1500

diff --git a/nebula_ros/config/lidar/hesai/Pandar40P.param.yaml b/nebula_ros/config/lidar/hesai/Pandar40P.param.yaml
@@ -2,6 +2,7 @@
   ros__parameters:
     host_ip: 255.255.255.255
     sensor_ip: 192.168.1.201
+    multicast_ip: ""
     data_port: 2368
     gnss_port: 10110
     packet_mtu_size: 1500

diff --git a/nebula_ros/config/lidar/hesai/Pandar64.param.yaml b/nebula_ros/config/lidar/hesai/Pandar64.param.yaml
@@ -2,6 +2,7 @@
   ros__parameters:
     host_ip: 255.255.255.255
     sensor_ip: 192.168.1.201
+    multicast_ip: ""
     data_port: 2368
     gnss_port: 10110
     packet_mtu_size: 1500

diff --git a/nebula_ros/config/lidar/hesai/PandarAT128.param.yaml b/nebula_ros/config/lidar/hesai/PandarAT128.param.yaml
@@ -2,6 +2,7 @@
   ros__parameters:
     host_ip: 255.255.255.255
     sensor_ip: 192.168.1.201
+    multicast_ip: ""
     data_port: 2368
     gnss_port: 10110
     packet_mtu_size: 1500

diff --git a/nebula_ros/config/lidar/hesai/PandarQT128.param.yaml b/nebula_ros/config/lidar/hesai/PandarQT128.param.yaml
@@ -2,6 +2,7 @@
   ros__parameters:
     host_ip: 255.255.255.255
     sensor_ip: 192.168.1.201
+    multicast_ip: ""
     data_port: 2368
     gnss_port: 10110
     packet_mtu_size: 1500

diff --git a/nebula_ros/config/lidar/hesai/PandarQT64.param.yaml b/nebula_ros/config/lidar/hesai/PandarQT64.param.yaml
@@ -2,6 +2,7 @@
   ros__parameters:
     host_ip: 255.255.255.255
     sensor_ip: 192.168.1.201
+    multicast_ip: ""
     data_port: 2368
     gnss_port: 10110
     packet_mtu_size: 1500

diff --git a/nebula_ros/config/lidar/hesai/PandarXT32.param.yaml b/nebula_ros/config/lidar/hesai/PandarXT32.param.yaml
@@ -2,6 +2,7 @@
   ros__parameters:
     host_ip: 255.255.255.255
     sensor_ip: 192.168.1.201
+    multicast_ip: ""
     data_port: 2368
     gnss_port: 10110
     packet_mtu_size: 1500

diff --git a/nebula_ros/config/lidar/hesai/PandarXT32M.param.yaml b/nebula_ros/config/lidar/hesai/PandarXT32M.param.yaml
@@ -2,6 +2,7 @@
   ros__parameters:
     host_ip: 255.255.255.255
     sensor_ip: 192.168.1.201
+    multicast_ip: ""
     data_port: 2368
     gnss_port: 10110
     packet_mtu_size: 1500

diff --git a/nebula_ros/schema/Pandar128E4X.schema.json b/nebula_ros/schema/Pandar128E4X.schema.json
@@ -12,6 +12,9 @@
         "sensor_ip": {
           "$ref": "sub/communication.json#/definitions/sensor_ip"
         },
+        "multicast_ip": {
+          "$ref": "sub/lidar_hesai.json#/definitions/multicast_ip"
+        },
         "data_port": {
           "$ref": "sub/communication.json#/definitions/data_port"
         },
@@ -97,6 +100,7 @@
       "required": [
         "host_ip",
         "sensor_ip",
+        "multicast_ip",
         "data_port",
         "gnss_port",
         "packet_mtu_size",

diff --git a/nebula_ros/schema/Pandar40P.schema.json b/nebula_ros/schema/Pandar40P.schema.json
@@ -12,6 +12,9 @@
         "sensor_ip": {
           "$ref": "sub/communication.json#/definitions/sensor_ip"
         },
+        "multicast_ip": {
+          "$ref": "sub/lidar_hesai.json#/definitions/multicast_ip"
+        },
         "data_port": {
           "$ref": "sub/communication.json#/definitions/data_port"
         },
@@ -88,6 +91,7 @@
       "required": [
         "host_ip",
         "sensor_ip",
+        "multicast_ip",
         "data_port",
         "gnss_port",
         "packet_mtu_size",

diff --git a/nebula_ros/schema/Pandar64.schema.json b/nebula_ros/schema/Pandar64.schema.json
@@ -12,6 +12,9 @@
         "sensor_ip": {
           "$ref": "sub/communication.json#/definitions/sensor_ip"
         },
+        "multicast_ip": {
+          "$ref": "sub/lidar_hesai.json#/definitions/multicast_ip"
+        },
         "data_port": {
           "$ref": "sub/communication.json#/definitions/data_port"
         },
@@ -85,6 +88,7 @@
       "required": [
         "host_ip",
         "sensor_ip",
+        "multicast_ip",
         "data_port",
         "gnss_port",
         "packet_mtu_size",

diff --git a/nebula_ros/schema/PandarAT128.schema.json b/nebula_ros/schema/PandarAT128.schema.json
@@ -12,6 +12,9 @@
         "sensor_ip": {
           "$ref": "sub/communication.json#/definitions/sensor_ip"
         },
+        "multicast_ip": {
+          "$ref": "sub/lidar_hesai.json#/definitions/multicast_ip"
+        },
         "data_port": {
           "$ref": "sub/communication.json#/definitions/data_port"
         },
@@ -105,6 +108,7 @@
       "required": [
         "host_ip",
         "sensor_ip",
+        "multicast_ip",
         "data_port",
         "gnss_port",
         "packet_mtu_size",

diff --git a/nebula_ros/schema/PandarQT128.schema.json b/nebula_ros/schema/PandarQT128.schema.json
@@ -12,6 +12,9 @@
         "sensor_ip": {
           "$ref": "sub/communication.json#/definitions/sensor_ip"
         },
+        "multicast_ip": {
+          "$ref": "sub/lidar_hesai.json#/definitions/multicast_ip"
+        },
         "data_port": {
           "$ref": "sub/communication.json#/definitions/data_port"
         },
@@ -91,6 +94,7 @@
       "required": [
         "host_ip",
         "sensor_ip",
+        "multicast_ip",
         "data_port",
         "gnss_port",
         "packet_mtu_size",

diff --git a/nebula_ros/schema/PandarQT64.schema.json b/nebula_ros/schema/PandarQT64.schema.json
@@ -12,6 +12,9 @@
         "sensor_ip": {
           "$ref": "sub/communication.json#/definitions/sensor_ip"
         },
+        "multicast_ip": {
+          "$ref": "sub/lidar_hesai.json#/definitions/multicast_ip"
+        },
         "data_port": {
           "$ref": "sub/communication.json#/definitions/data_port"
         },
@@ -88,6 +91,7 @@
       "required": [
         "host_ip",
         "sensor_ip",
+        "multicast_ip",
         "data_port",
         "gnss_port",
         "packet_mtu_size",

diff --git a/nebula_ros/schema/PandarXT32.schema.json b/nebula_ros/schema/PandarXT32.schema.json
@@ -12,6 +12,9 @@
         "sensor_ip": {
           "$ref": "sub/communication.json#/definitions/sensor_ip"
         },
+        "multicast_ip": {
+          "$ref": "sub/lidar_hesai.json#/definitions/multicast_ip"
+        },
         "data_port": {
           "$ref": "sub/communication.json#/definitions/data_port"
         },
@@ -91,6 +94,7 @@
       "required": [
         "host_ip",
         "sensor_ip",
+        "multicast_ip",
         "data_port",
         "gnss_port",
         "packet_mtu_size",

diff --git a/nebula_ros/schema/PandarXT32M.schema.json b/nebula_ros/schema/PandarXT32M.schema.json
@@ -12,6 +12,9 @@
         "sensor_ip": {
           "$ref": "sub/communication.json#/definitions/sensor_ip"
         },
+        "multicast_ip": {
+          "$ref": "sub/lidar_hesai.json#/definitions/multicast_ip"
+        },
         "data_port": {
           "$ref": "sub/communication.json#/definitions/data_port"
         },
@@ -91,6 +94,7 @@
       "required": [
         "host_ip",
         "sensor_ip",
+        "multicast_ip",
         "data_port",
         "gnss_port",
         "packet_mtu_size",

diff --git a/nebula_ros/schema/sub/lidar_hesai.json b/nebula_ros/schema/sub/lidar_hesai.json
@@ -31,6 +31,13 @@
       "minimum": 300,
       "maximum": 1200,
       "multipleOf": 60
+    },
+    "multicast_ip": {
+      "type": "string",
+      "default": "\"\"",
+      "pattern": "(^((22[4-9]|23[0-9])\\.)((25[0-5]|2[0-4][0-9]|[01]?[0-9][0-9]?)\\.){2}(25[0-5]|2[0-4][0-9]|[01]?[0-9][0-9]?)$)|",
+      "readOnly": true,
+      "description": "Multicast IPv4 address (leave empty to disable multicast)."
     }
   }
 }
diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp
@@ -2,6 +2,10 @@
 
 #include "nebula_ros/hesai/hesai_ros_wrapper.hpp"
 
+#include "nebula_ros/common/parameter_descriptors.hpp"
+
+#include <string>
+
 #pragma clang diagnostic ignored "-Wbitwise-instead-of-logical"
 
 namespace nebula
@@ -77,6 +81,7 @@ nebula::Status HesaiRosWrapper::DeclareAndGetSensorConfigParams()
 
   config.host_ip = declare_parameter<std::string>("host_ip", param_read_only());
   config.sensor_ip = declare_parameter<std::string>("sensor_ip", param_read_only());
+  config.multicast_ip = declare_parameter<std::string>("multicast_ip", param_read_only());
   config.data_port = declare_parameter<uint16_t>("data_port", param_read_only());
   config.gnss_port = declare_parameter<uint16_t>("gnss_port", param_read_only());
   config.frame_id = declare_parameter<std::string>("frame_id", param_read_write());