tier4 · mojomex · Sep 7, 2024 · Aug 30, 2024 · Aug 30, 2024 · Aug 30, 2024
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
@@ -144,8 +144,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
@@ -165,8 +172,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;
@@ -839,10 +846,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;
@@ -867,7 +877,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/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp
@@ -66,7 +66,7 @@ class ContinentalARS548HwInterfaceWrapper
 
   /// @brief Callback to send the steering angle information to the radar device
   /// @param msg The steering angle message
-  void SteeringAngleCallback(const std_msgs::msg::Float32::SharedPtr msg);
+  void steeringAngleCallback(const std_msgs::msg::Float32::SharedPtr msg);
 
   /// @brief Service callback to set the new sensor ip
   /// @param request service request

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/continental/continental_ars548_hw_interface_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp
@@ -67,7 +67,8 @@ void ContinentalARS548HwInterfaceWrapper::SensorInterfaceStart()
     steering_angle_sub_ = parent_node_->create_subscription<std_msgs::msg::Float32>(
       "steering_angle_input", rclcpp::SensorDataQoS(),
       std::bind(
-        &ContinentalARS548HwInterfaceWrapper::SteeringAngleCallback, this, std::placeholders::_1));
+        &::nebula::ros::ContinentalARS548HwInterfaceWrapper::steeringAngleCallback, this,
+        std::placeholders::_1));
 
     set_network_configuration_service_server_ =
       parent_node_->create_service<continental_srvs::srv::ContinentalArs548SetNetworkConfiguration>(
@@ -151,7 +152,7 @@ void ContinentalARS548HwInterfaceWrapper::AccelerationCallback(
   hw_interface_->SetAccelerationLongitudinalCog(msg->accel.accel.linear.x);
 }
 
-void ContinentalARS548HwInterfaceWrapper::SteeringAngleCallback(
+void ContinentalARS548HwInterfaceWrapper::steeringAngleCallback(
   const std_msgs::msg::Float32::SharedPtr msg)
 {
   constexpr float rad_to_deg = 180.f / M_PI;

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());