diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index 555f4f1e8..50a5d55ab 100644 --- 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 index 1657b72f7..dd5055850 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 @@ -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(hesai_config.dest_ipaddr[2]) << "." << static_cast(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 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 index 7f0375f42..9da22a169 100644 --- 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 index c24d61d36..0ee7b83ec 100644 --- 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 index 1a1b975f6..1bb195550 100644 --- 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 index 7db50fb52..462a4fd78 100644 --- 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 index c26977a2b..f9ee8029a 100644 --- 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 index c707f7e12..fcb7f89ac 100644 --- 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 index db56ed435..51666ea7f 100644 --- 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 index 66cee1218..01fc35990 100644 --- 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 index 913d58be0..9de8b1134 100644 --- 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 index 5f822f4fc..f83cdc393 100644 --- 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 index 680ffc501..0b15a0f9b 100644 --- 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 index 5b4c07f13..08c85e108 100644 --- 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 index f139b890c..efac39914 100644 --- 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 index 81f30a875..bb16cfcbb 100644 --- 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 index 2ef52aa39..e4ff2eae9 100644 --- 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 index ac5051f5e..43198d162 100644 --- 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 index 2a74eb481..caed4db7a 100644 --- 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 index e0e0b91c1..2a212b8d1 100644 --- 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 + #pragma clang diagnostic ignored "-Wbitwise-instead-of-logical" namespace nebula @@ -77,6 +81,7 @@ nebula::Status HesaiRosWrapper::DeclareAndGetSensorConfigParams() config.host_ip = declare_parameter("host_ip", param_read_only()); config.sensor_ip = declare_parameter("sensor_ip", param_read_only()); + config.multicast_ip = declare_parameter("multicast_ip", param_read_only()); config.data_port = declare_parameter("data_port", param_read_only()); config.gnss_port = declare_parameter("gnss_port", param_read_only()); config.frame_id = declare_parameter("frame_id", param_read_write());