From 214373ae4ee131aa6c281d3e7f377c2ed1522ebc Mon Sep 17 00:00:00 2001 From: Mateusz Szczygielski <112629916+msz-rai@users.noreply.github.com> Date: Fri, 25 Oct 2024 06:46:01 +0200 Subject: [PATCH] feat: allow to launch hardware in listen mode (#210) * Add flag to disable communication with hesai sensor * Add flag to disable communication with velodyne sensor * Skip setting parameters to sensor onConfigChange silently To be consistent with setup_sensor parameter * ci(pre-commit): autofix * Review fixes: Rename communicate_with_sensor -> udp_only * Review fixes: Log info when UDP-only mode is enabled * Review fixes: Fix schemas * Review fixes: Rephrase docs * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- docs/parameters.md | 54 ++++++++++--------- .../lidar/hesai/Pandar128E4X.param.yaml | 1 + .../config/lidar/hesai/Pandar40P.param.yaml | 1 + .../config/lidar/hesai/Pandar64.param.yaml | 1 + .../config/lidar/hesai/PandarAT128.param.yaml | 1 + .../config/lidar/hesai/PandarQT128.param.yaml | 1 + .../config/lidar/hesai/PandarQT64.param.yaml | 1 + .../config/lidar/hesai/PandarXT32.param.yaml | 1 + .../config/lidar/hesai/PandarXT32M.param.yaml | 1 + .../config/lidar/velodyne/VLP16.param.yaml | 1 + .../config/lidar/velodyne/VLP32.param.yaml | 1 + .../config/lidar/velodyne/VLS128.param.yaml | 1 + .../nebula_ros/hesai/hw_interface_wrapper.hpp | 4 +- .../velodyne/hw_interface_wrapper.hpp | 4 +- nebula_ros/schema/Pandar128E4X.schema.json | 4 ++ nebula_ros/schema/Pandar40P.schema.json | 4 ++ nebula_ros/schema/Pandar64.schema.json | 4 ++ nebula_ros/schema/PandarAT128.schema.json | 4 ++ nebula_ros/schema/PandarQT128.schema.json | 4 ++ nebula_ros/schema/PandarQT64.schema.json | 4 ++ nebula_ros/schema/PandarXT32.schema.json | 4 ++ nebula_ros/schema/PandarXT32M.schema.json | 4 ++ nebula_ros/schema/VLP16.schema.json | 4 ++ nebula_ros/schema/VLP32.schema.json | 4 ++ nebula_ros/schema/VLS128.schema.json | 4 ++ nebula_ros/schema/sub/hardware.json | 6 +++ nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 21 ++++++-- nebula_ros/src/hesai/hw_interface_wrapper.cpp | 12 +++-- .../src/velodyne/hw_interface_wrapper.cpp | 13 ++++- .../src/velodyne/velodyne_ros_wrapper.cpp | 14 ++++- 30 files changed, 144 insertions(+), 39 deletions(-) diff --git a/docs/parameters.md b/docs/parameters.md index 8cb3c62a0..d0603012d 100644 --- a/docs/parameters.md +++ b/docs/parameters.md @@ -60,21 +60,22 @@ Parameters shared by all supported models: ### Hardware interface parameters -| Parameter | Type | Default | Accepted values | Description | -| ------------------------------ | ------ | --------------- | ----------------- | ------------------------------ | -| frame_id | string | hesai | | ROS frame ID | -| sensor_ip | string | 192.168.1.201 | | Sensor IP | -| host_ip | string | 255.255.255.255 | | Host IP | -| data_port | uint16 | 2368 | | Sensor port | -| gnss_port | uint16 | 2369 | | GNSS port | -| frequency_ms | uint16 | 100 | milliseconds, > 0 | Time per scan | -| packet_mtu_size | uint16 | 1500 | | Packet MTU size | -| rotation_speed | uint16 | 600 | | Rotation speed | -| cloud_min_angle | uint16 | 0 | degrees [0, 360] | FoV start angle | -| cloud_max_angle | uint16 | 359 | degrees [0, 360] | FoV end angle | -| dual_return_distance_threshold | double | 0.1 | | Dual return distance threshold | -| diag_span | uint16 | 1000 | milliseconds, > 0 | Diagnostic span | -| setup_sensor | bool | True | True, False | Configure sensor settings | +| Parameter | Type | Default | Accepted values | Description | +| ------------------------------ | ------ | --------------- | ----------------- | ---------------------------------------------------------------------------------------- | +| frame_id | string | hesai | | ROS frame ID | +| sensor_ip | string | 192.168.1.201 | | Sensor IP | +| host_ip | string | 255.255.255.255 | | Host IP | +| data_port | uint16 | 2368 | | Sensor port | +| gnss_port | uint16 | 2369 | | GNSS port | +| frequency_ms | uint16 | 100 | milliseconds, > 0 | Time per scan | +| packet_mtu_size | uint16 | 1500 | | Packet MTU size | +| rotation_speed | uint16 | 600 | | Rotation speed | +| cloud_min_angle | uint16 | 0 | degrees [0, 360] | FoV start angle | +| cloud_max_angle | uint16 | 359 | degrees [0, 360] | FoV end angle | +| dual_return_distance_threshold | double | 0.1 | | Dual return distance threshold | +| diag_span | uint16 | 1000 | milliseconds, > 0 | Diagnostic span | +| setup_sensor | bool | True | True, False | Configure sensor settings | +| udp_only | bool | False | True, False | Use UDP protocol only (settings synchronization and diagnostics publishing are disabled) | ### Driver parameters @@ -97,17 +98,18 @@ Parameters shared by all supported models: ### Hardware interface parameters -| Parameter | Type | Default | Accepted values | Description | -| --------------- | ------ | --------------- | ----------------- | --------------- | -| frame_id | string | velodyne | | ROS frame ID | -| sensor_ip | string | 192.168.1.201 | | Sensor IP | -| host_ip | string | 255.255.255.255 | | Host IP | -| data_port | uint16 | 2368 | | Sensor port | -| gnss_port | uint16 | 2369 | | GNSS port | -| frequency_ms | uint16 | 100 | milliseconds, > 0 | Time per scan | -| packet_mtu_size | uint16 | 1500 | | Packet MTU size | -| cloud_min_angle | uint16 | 0 | degrees [0, 360] | FoV start angle | -| cloud_max_angle | uint16 | 359 | degrees [0, 360] | FoV end angle | +| Parameter | Type | Default | Accepted values | Description | +| --------------- | ------ | --------------- | ----------------- | ---------------------------------------------------------------------------------------- | +| frame_id | string | velodyne | | ROS frame ID | +| sensor_ip | string | 192.168.1.201 | | Sensor IP | +| host_ip | string | 255.255.255.255 | | Host IP | +| data_port | uint16 | 2368 | | Sensor port | +| gnss_port | uint16 | 2369 | | GNSS port | +| frequency_ms | uint16 | 100 | milliseconds, > 0 | Time per scan | +| packet_mtu_size | uint16 | 1500 | | Packet MTU size | +| cloud_min_angle | uint16 | 0 | degrees [0, 360] | FoV start angle | +| cloud_max_angle | uint16 | 359 | degrees [0, 360] | FoV end angle | +| udp_only | bool | False | True, False | Use UDP protocol only (settings synchronization and diagnostics publishing are disabled) | ### Driver parameters diff --git a/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml b/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml index 85361864c..da32fc0eb 100644 --- a/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml +++ b/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml @@ -8,6 +8,7 @@ packet_mtu_size: 1500 launch_hw: true setup_sensor: true + udp_only: false frame_id: hesai diag_span: 1000 min_range: 0.3 diff --git a/nebula_ros/config/lidar/hesai/Pandar40P.param.yaml b/nebula_ros/config/lidar/hesai/Pandar40P.param.yaml index 1f6b79ff6..332b2a1c0 100644 --- a/nebula_ros/config/lidar/hesai/Pandar40P.param.yaml +++ b/nebula_ros/config/lidar/hesai/Pandar40P.param.yaml @@ -8,6 +8,7 @@ packet_mtu_size: 1500 launch_hw: true setup_sensor: true + udp_only: false frame_id: hesai diag_span: 1000 min_range: 0.3 diff --git a/nebula_ros/config/lidar/hesai/Pandar64.param.yaml b/nebula_ros/config/lidar/hesai/Pandar64.param.yaml index 31c695643..5e78f796a 100644 --- a/nebula_ros/config/lidar/hesai/Pandar64.param.yaml +++ b/nebula_ros/config/lidar/hesai/Pandar64.param.yaml @@ -8,6 +8,7 @@ packet_mtu_size: 1500 launch_hw: true setup_sensor: true + udp_only: false frame_id: hesai diag_span: 1000 min_range: 0.3 diff --git a/nebula_ros/config/lidar/hesai/PandarAT128.param.yaml b/nebula_ros/config/lidar/hesai/PandarAT128.param.yaml index 5a2a8858b..dff7789dd 100644 --- a/nebula_ros/config/lidar/hesai/PandarAT128.param.yaml +++ b/nebula_ros/config/lidar/hesai/PandarAT128.param.yaml @@ -8,6 +8,7 @@ packet_mtu_size: 1500 launch_hw: true setup_sensor: true + udp_only: false frame_id: hesai diag_span: 1000 correction_file: $(find-pkg-share nebula_decoders)/calibration/hesai/$(var sensor_model).dat diff --git a/nebula_ros/config/lidar/hesai/PandarQT128.param.yaml b/nebula_ros/config/lidar/hesai/PandarQT128.param.yaml index 31e046be2..17779ee02 100644 --- a/nebula_ros/config/lidar/hesai/PandarQT128.param.yaml +++ b/nebula_ros/config/lidar/hesai/PandarQT128.param.yaml @@ -8,6 +8,7 @@ packet_mtu_size: 1500 launch_hw: true setup_sensor: true + udp_only: false frame_id: hesai diag_span: 1000 min_range: 0.3 diff --git a/nebula_ros/config/lidar/hesai/PandarQT64.param.yaml b/nebula_ros/config/lidar/hesai/PandarQT64.param.yaml index 4d3d119a8..f11d30076 100644 --- a/nebula_ros/config/lidar/hesai/PandarQT64.param.yaml +++ b/nebula_ros/config/lidar/hesai/PandarQT64.param.yaml @@ -8,6 +8,7 @@ packet_mtu_size: 1500 launch_hw: true setup_sensor: true + udp_only: false frame_id: hesai diag_span: 1000 min_range: 0.3 diff --git a/nebula_ros/config/lidar/hesai/PandarXT32.param.yaml b/nebula_ros/config/lidar/hesai/PandarXT32.param.yaml index 09eaef375..7288ec371 100644 --- a/nebula_ros/config/lidar/hesai/PandarXT32.param.yaml +++ b/nebula_ros/config/lidar/hesai/PandarXT32.param.yaml @@ -8,6 +8,7 @@ packet_mtu_size: 1500 launch_hw: true setup_sensor: true + udp_only: false frame_id: hesai diag_span: 1000 min_range: 0.3 diff --git a/nebula_ros/config/lidar/hesai/PandarXT32M.param.yaml b/nebula_ros/config/lidar/hesai/PandarXT32M.param.yaml index 6ad0e6566..cc1fffbc9 100644 --- a/nebula_ros/config/lidar/hesai/PandarXT32M.param.yaml +++ b/nebula_ros/config/lidar/hesai/PandarXT32M.param.yaml @@ -8,6 +8,7 @@ packet_mtu_size: 1500 launch_hw: true setup_sensor: true + udp_only: false frame_id: hesai diag_span: 1000 min_range: 0.3 diff --git a/nebula_ros/config/lidar/velodyne/VLP16.param.yaml b/nebula_ros/config/lidar/velodyne/VLP16.param.yaml index b46e980f0..d44c4d9ce 100644 --- a/nebula_ros/config/lidar/velodyne/VLP16.param.yaml +++ b/nebula_ros/config/lidar/velodyne/VLP16.param.yaml @@ -7,6 +7,7 @@ packet_mtu_size: 1500 launch_hw: true setup_sensor: true + udp_only: false frame_id: velodyne advanced_diagnostics: false diag_span: 1000 diff --git a/nebula_ros/config/lidar/velodyne/VLP32.param.yaml b/nebula_ros/config/lidar/velodyne/VLP32.param.yaml index 96ce39a32..0a18d780c 100644 --- a/nebula_ros/config/lidar/velodyne/VLP32.param.yaml +++ b/nebula_ros/config/lidar/velodyne/VLP32.param.yaml @@ -7,6 +7,7 @@ packet_mtu_size: 1500 launch_hw: true setup_sensor: true + udp_only: false frame_id: velodyne advanced_diagnostics: false diag_span: 1000 diff --git a/nebula_ros/config/lidar/velodyne/VLS128.param.yaml b/nebula_ros/config/lidar/velodyne/VLS128.param.yaml index c5c0c6313..10cc8406b 100644 --- a/nebula_ros/config/lidar/velodyne/VLS128.param.yaml +++ b/nebula_ros/config/lidar/velodyne/VLS128.param.yaml @@ -7,6 +7,7 @@ packet_mtu_size: 1500 launch_hw: true setup_sensor: true + udp_only: false frame_id: velodyne advanced_diagnostics: false diag_span: 1000 diff --git a/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp index cb5ec2942..4b2572b1b 100644 --- a/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp @@ -27,7 +27,8 @@ class HesaiHwInterfaceWrapper public: HesaiHwInterfaceWrapper( rclcpp::Node * const parent_node, - std::shared_ptr & config); + std::shared_ptr & config, + bool use_udp_only = false); void on_config_change( const std::shared_ptr & new_config); @@ -41,5 +42,6 @@ class HesaiHwInterfaceWrapper rclcpp::Logger logger_; nebula::Status status_; bool setup_sensor_; + bool use_udp_only_; }; } // namespace nebula::ros diff --git a/nebula_ros/include/nebula_ros/velodyne/hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/velodyne/hw_interface_wrapper.hpp index 74b02dbcc..8ae102e9c 100644 --- a/nebula_ros/include/nebula_ros/velodyne/hw_interface_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/velodyne/hw_interface_wrapper.hpp @@ -29,7 +29,8 @@ class VelodyneHwInterfaceWrapper public: VelodyneHwInterfaceWrapper( rclcpp::Node * const parent_node, - std::shared_ptr & config); + std::shared_ptr & config, + bool use_udp_only = false); void on_config_change( const std::shared_ptr & new_config); @@ -43,5 +44,6 @@ class VelodyneHwInterfaceWrapper rclcpp::Logger logger_; nebula::Status status_; bool setup_sensor_; + bool use_udp_only_; }; } // namespace nebula::ros diff --git a/nebula_ros/schema/Pandar128E4X.schema.json b/nebula_ros/schema/Pandar128E4X.schema.json index 656ebcbb8..93e5806cc 100644 --- a/nebula_ros/schema/Pandar128E4X.schema.json +++ b/nebula_ros/schema/Pandar128E4X.schema.json @@ -30,6 +30,9 @@ "setup_sensor": { "$ref": "sub/hardware.json#/definitions/setup_sensor" }, + "udp_only": { + "$ref": "sub/hardware.json#/definitions/udp_only" + }, "frame_id": { "$ref": "sub/topic.json#/definitions/frame_id" }, @@ -109,6 +112,7 @@ "packet_mtu_size", "launch_hw", "setup_sensor", + "udp_only", "frame_id", "diag_span", "cloud_min_angle", diff --git a/nebula_ros/schema/Pandar40P.schema.json b/nebula_ros/schema/Pandar40P.schema.json index d867ea4c4..2e13bd43e 100644 --- a/nebula_ros/schema/Pandar40P.schema.json +++ b/nebula_ros/schema/Pandar40P.schema.json @@ -30,6 +30,9 @@ "setup_sensor": { "$ref": "sub/hardware.json#/definitions/setup_sensor" }, + "udp_only": { + "$ref": "sub/hardware.json#/definitions/udp_only" + }, "frame_id": { "$ref": "sub/topic.json#/definitions/frame_id" }, @@ -100,6 +103,7 @@ "packet_mtu_size", "launch_hw", "setup_sensor", + "udp_only", "frame_id", "diag_span", "cloud_min_angle", diff --git a/nebula_ros/schema/Pandar64.schema.json b/nebula_ros/schema/Pandar64.schema.json index 3559e8764..666f1253d 100644 --- a/nebula_ros/schema/Pandar64.schema.json +++ b/nebula_ros/schema/Pandar64.schema.json @@ -30,6 +30,9 @@ "setup_sensor": { "$ref": "sub/hardware.json#/definitions/setup_sensor" }, + "udp_only": { + "$ref": "sub/hardware.json#/definitions/udp_only" + }, "frame_id": { "$ref": "sub/topic.json#/definitions/frame_id" }, @@ -97,6 +100,7 @@ "packet_mtu_size", "launch_hw", "setup_sensor", + "udp_only", "frame_id", "diag_span", "cloud_min_angle", diff --git a/nebula_ros/schema/PandarAT128.schema.json b/nebula_ros/schema/PandarAT128.schema.json index 81ea068f8..61786c368 100644 --- a/nebula_ros/schema/PandarAT128.schema.json +++ b/nebula_ros/schema/PandarAT128.schema.json @@ -30,6 +30,9 @@ "setup_sensor": { "$ref": "sub/hardware.json#/definitions/setup_sensor" }, + "udp_only": { + "$ref": "sub/hardware.json#/definitions/udp_only" + }, "frame_id": { "$ref": "sub/topic.json#/definitions/frame_id" }, @@ -120,6 +123,7 @@ "packet_mtu_size", "launch_hw", "setup_sensor", + "udp_only", "frame_id", "diag_span", "correction_file", diff --git a/nebula_ros/schema/PandarQT128.schema.json b/nebula_ros/schema/PandarQT128.schema.json index 52cbf3ed7..01f186d52 100644 --- a/nebula_ros/schema/PandarQT128.schema.json +++ b/nebula_ros/schema/PandarQT128.schema.json @@ -30,6 +30,9 @@ "setup_sensor": { "$ref": "sub/hardware.json#/definitions/setup_sensor" }, + "udp_only": { + "$ref": "sub/hardware.json#/definitions/udp_only" + }, "frame_id": { "$ref": "sub/topic.json#/definitions/frame_id" }, @@ -103,6 +106,7 @@ "packet_mtu_size", "launch_hw", "setup_sensor", + "udp_only", "frame_id", "diag_span", "cloud_min_angle", diff --git a/nebula_ros/schema/PandarQT64.schema.json b/nebula_ros/schema/PandarQT64.schema.json index d1680b0fb..af82060f9 100644 --- a/nebula_ros/schema/PandarQT64.schema.json +++ b/nebula_ros/schema/PandarQT64.schema.json @@ -30,6 +30,9 @@ "setup_sensor": { "$ref": "sub/hardware.json#/definitions/setup_sensor" }, + "udp_only": { + "$ref": "sub/hardware.json#/definitions/udp_only" + }, "frame_id": { "$ref": "sub/topic.json#/definitions/frame_id" }, @@ -100,6 +103,7 @@ "packet_mtu_size", "launch_hw", "setup_sensor", + "udp_only", "frame_id", "diag_span", "cloud_min_angle", diff --git a/nebula_ros/schema/PandarXT32.schema.json b/nebula_ros/schema/PandarXT32.schema.json index aac236d7a..1f0c61133 100644 --- a/nebula_ros/schema/PandarXT32.schema.json +++ b/nebula_ros/schema/PandarXT32.schema.json @@ -30,6 +30,9 @@ "setup_sensor": { "$ref": "sub/hardware.json#/definitions/setup_sensor" }, + "udp_only": { + "$ref": "sub/hardware.json#/definitions/udp_only" + }, "frame_id": { "$ref": "sub/topic.json#/definitions/frame_id" }, @@ -103,6 +106,7 @@ "packet_mtu_size", "launch_hw", "setup_sensor", + "udp_only", "frame_id", "diag_span", "cloud_min_angle", diff --git a/nebula_ros/schema/PandarXT32M.schema.json b/nebula_ros/schema/PandarXT32M.schema.json index ea29162fc..7ab960449 100644 --- a/nebula_ros/schema/PandarXT32M.schema.json +++ b/nebula_ros/schema/PandarXT32M.schema.json @@ -30,6 +30,9 @@ "setup_sensor": { "$ref": "sub/hardware.json#/definitions/setup_sensor" }, + "udp_only": { + "$ref": "sub/hardware.json#/definitions/udp_only" + }, "frame_id": { "$ref": "sub/topic.json#/definitions/frame_id" }, @@ -103,6 +106,7 @@ "packet_mtu_size", "launch_hw", "setup_sensor", + "udp_only", "frame_id", "diag_span", "cloud_min_angle", diff --git a/nebula_ros/schema/VLP16.schema.json b/nebula_ros/schema/VLP16.schema.json index db72f5778..4e656db12 100644 --- a/nebula_ros/schema/VLP16.schema.json +++ b/nebula_ros/schema/VLP16.schema.json @@ -27,6 +27,9 @@ "setup_sensor": { "$ref": "sub/hardware.json#/definitions/setup_sensor" }, + "udp_only": { + "$ref": "sub/hardware.json#/definitions/udp_only" + }, "frame_id": { "$ref": "sub/topic.json#/definitions/frame_id" }, @@ -72,6 +75,7 @@ "packet_mtu_size", "launch_hw", "setup_sensor", + "udp_only", "frame_id", "advanced_diagnostics", "diag_span", diff --git a/nebula_ros/schema/VLP32.schema.json b/nebula_ros/schema/VLP32.schema.json index 55cbc546d..6eeccbc2a 100644 --- a/nebula_ros/schema/VLP32.schema.json +++ b/nebula_ros/schema/VLP32.schema.json @@ -27,6 +27,9 @@ "setup_sensor": { "$ref": "sub/hardware.json#/definitions/setup_sensor" }, + "udp_only": { + "$ref": "sub/hardware.json#/definitions/udp_only" + }, "frame_id": { "$ref": "sub/topic.json#/definitions/frame_id" }, @@ -72,6 +75,7 @@ "packet_mtu_size", "launch_hw", "setup_sensor", + "udp_only", "frame_id", "advanced_diagnostics", "diag_span", diff --git a/nebula_ros/schema/VLS128.schema.json b/nebula_ros/schema/VLS128.schema.json index 86ddcb79f..802a3b251 100644 --- a/nebula_ros/schema/VLS128.schema.json +++ b/nebula_ros/schema/VLS128.schema.json @@ -27,6 +27,9 @@ "setup_sensor": { "$ref": "sub/hardware.json#/definitions/setup_sensor" }, + "udp_only": { + "$ref": "sub/hardware.json#/definitions/udp_only" + }, "frame_id": { "$ref": "sub/topic.json#/definitions/frame_id" }, @@ -72,6 +75,7 @@ "packet_mtu_size", "launch_hw", "setup_sensor", + "udp_only", "frame_id", "advanced_diagnostics", "diag_span", diff --git a/nebula_ros/schema/sub/hardware.json b/nebula_ros/schema/sub/hardware.json index ea1b17586..e6f0dcee3 100644 --- a/nebula_ros/schema/sub/hardware.json +++ b/nebula_ros/schema/sub/hardware.json @@ -28,6 +28,12 @@ "type": "boolean", "default": "true", "description": "Whether TCP connections are retried on failure or the driver should instead exit." + }, + "udp_only": { + "type": "boolean", + "default": "false", + "readOnly": true, + "description": "Use UDP protocol only (settings synchronization and diagnostics publishing are disabled)." } } } diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index 756e7dd44..77267c4a6 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -40,20 +40,33 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) RCLCPP_INFO_STREAM(get_logger(), "Sensor Configuration: " << *sensor_cfg_ptr_); launch_hw_ = declare_parameter("launch_hw", param_read_only()); + bool use_udp_only = declare_parameter("udp_only", param_read_only()); + + if (use_udp_only) { + RCLCPP_INFO_STREAM( + get_logger(), + "UDP-only mode is enabled. Settings checks, synchronization, and diagnostics publishing are " + "disabled."); + } if (launch_hw_) { - hw_interface_wrapper_.emplace(this, sensor_cfg_ptr_); - hw_monitor_wrapper_.emplace(this, hw_interface_wrapper_->hw_interface(), sensor_cfg_ptr_); + hw_interface_wrapper_.emplace(this, sensor_cfg_ptr_, use_udp_only); + if (!use_udp_only) { // hardware monitor requires TCP connection + hw_monitor_wrapper_.emplace(this, hw_interface_wrapper_->hw_interface(), sensor_cfg_ptr_); + } } - auto calibration_result = get_calibration_data(sensor_cfg_ptr_->calibration_path); + bool force_load_caibration_from_file = + use_udp_only; // Downloading from device requires TCP connection + auto calibration_result = + get_calibration_data(sensor_cfg_ptr_->calibration_path, force_load_caibration_from_file); if (!calibration_result.has_value()) { throw std::runtime_error( (std::stringstream() << "No valid calibration found: " << calibration_result.error()).str()); } if ( - hw_interface_wrapper_ && + hw_interface_wrapper_ && !use_udp_only && sensor_cfg_ptr_->sensor_model != drivers::SensorModel::HESAI_PANDARAT128) { auto status = hw_interface_wrapper_->hw_interface()->checkAndSetLidarRange(*calibration_result.value()); diff --git a/nebula_ros/src/hesai/hw_interface_wrapper.cpp b/nebula_ros/src/hesai/hw_interface_wrapper.cpp index 4ad318da2..bc746c523 100644 --- a/nebula_ros/src/hesai/hw_interface_wrapper.cpp +++ b/nebula_ros/src/hesai/hw_interface_wrapper.cpp @@ -9,10 +9,11 @@ namespace nebula::ros HesaiHwInterfaceWrapper::HesaiHwInterfaceWrapper( rclcpp::Node * const parent_node, - std::shared_ptr & config) + std::shared_ptr & config, bool use_udp_only) : hw_interface_(new nebula::drivers::HesaiHwInterface()), logger_(parent_node->get_logger().get_child("HwInterface")), - status_(Status::NOT_INITIALIZED) + status_(Status::NOT_INITIALIZED), + use_udp_only_(use_udp_only) { setup_sensor_ = parent_node->declare_parameter("setup_sensor", param_read_only()); bool retry_connect = parent_node->declare_parameter("retry_hw", param_read_only()); @@ -28,6 +29,11 @@ HesaiHwInterfaceWrapper::HesaiHwInterfaceWrapper( hw_interface_->SetLogger(std::make_shared(parent_node->get_logger())); hw_interface_->SetTargetModel(config->sensor_model); + if (use_udp_only) { + // Do not initialize TCP + return; + } + int retry_count = 0; while (true) { @@ -65,7 +71,7 @@ void HesaiHwInterfaceWrapper::on_config_change( { hw_interface_->SetSensorConfiguration( std::static_pointer_cast(new_config)); - if (setup_sensor_) { + if (!use_udp_only_ && setup_sensor_) { hw_interface_->CheckAndSetConfig(); } } diff --git a/nebula_ros/src/velodyne/hw_interface_wrapper.cpp b/nebula_ros/src/velodyne/hw_interface_wrapper.cpp index 792012bce..338f2ab3e 100644 --- a/nebula_ros/src/velodyne/hw_interface_wrapper.cpp +++ b/nebula_ros/src/velodyne/hw_interface_wrapper.cpp @@ -7,10 +7,11 @@ namespace nebula::ros VelodyneHwInterfaceWrapper::VelodyneHwInterfaceWrapper( rclcpp::Node * const parent_node, - std::shared_ptr & config) + std::shared_ptr & config, bool use_udp_only) : hw_interface_(new nebula::drivers::VelodyneHwInterface()), logger_(parent_node->get_logger().get_child("HwInterfaceWrapper")), - status_(Status::NOT_INITIALIZED) + status_(Status::NOT_INITIALIZED), + use_udp_only_(use_udp_only) { setup_sensor_ = parent_node->declare_parameter("setup_sensor", param_read_only()); @@ -23,6 +24,11 @@ VelodyneHwInterfaceWrapper::VelodyneHwInterfaceWrapper( (std::stringstream{} << "Could not initialize HW interface: " << status_).str()); } + if (use_udp_only_) { + // Do not initialize http client + return; + } + status_ = hw_interface_->init_http_client(); if (status_ != Status::OK) { @@ -47,6 +53,9 @@ void VelodyneHwInterfaceWrapper::on_config_change( const std::shared_ptr & new_config) { hw_interface_->initialize_sensor_configuration(new_config); + if (use_udp_only_) { + return; + } hw_interface_->init_http_client(); if (setup_sensor_) { hw_interface_->set_sensor_configuration(new_config); diff --git a/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp index d23c13e72..e06535766 100644 --- a/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp +++ b/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp @@ -27,10 +27,20 @@ VelodyneRosWrapper::VelodyneRosWrapper(const rclcpp::NodeOptions & options) RCLCPP_INFO_STREAM(get_logger(), "Sensor Configuration: " << *sensor_cfg_ptr_); launch_hw_ = declare_parameter("launch_hw", param_read_only()); + bool use_udp_only = declare_parameter("udp_only", param_read_only()); + + if (use_udp_only) { + RCLCPP_INFO_STREAM( + get_logger(), + "UDP-only mode is enabled. Settings checks, synchronization, and diagnostics publishing are " + "disabled."); + } if (launch_hw_) { - hw_interface_wrapper_.emplace(this, sensor_cfg_ptr_); - hw_monitor_wrapper_.emplace(this, hw_interface_wrapper_->hw_interface(), sensor_cfg_ptr_); + hw_interface_wrapper_.emplace(this, sensor_cfg_ptr_, use_udp_only); + if (!use_udp_only) { // hardware monitor requires HTTP connection + hw_monitor_wrapper_.emplace(this, hw_interface_wrapper_->hw_interface(), sensor_cfg_ptr_); + } } decoder_wrapper_.emplace(