From ac5ef18e299467643c23627987aca5a5899ba563 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Fri, 25 Oct 2024 15:44:52 +0900 Subject: [PATCH 1/5] chore(nebula_ros): combine connection-related parameters into `connection_mode` parameter Signed-off-by: Max SCHMELLER --- .../config/lidar/hesai/Pandar128E4X.param.yaml | 4 +--- nebula_ros/config/lidar/hesai/Pandar40P.param.yaml | 4 +--- nebula_ros/config/lidar/hesai/Pandar64.param.yaml | 4 +--- .../config/lidar/hesai/PandarAT128.param.yaml | 4 +--- .../config/lidar/hesai/PandarQT128.param.yaml | 4 +--- .../config/lidar/hesai/PandarQT64.param.yaml | 4 +--- .../config/lidar/hesai/PandarXT32.param.yaml | 4 +--- .../config/lidar/hesai/PandarXT32M.param.yaml | 4 +--- .../config/lidar/robosense/Bpearl.param.yaml | 2 +- .../config/lidar/robosense/Helios.param.yaml | 2 +- nebula_ros/config/lidar/velodyne/VLP16.param.yaml | 4 +--- nebula_ros/config/lidar/velodyne/VLP32.param.yaml | 4 +--- nebula_ros/config/lidar/velodyne/VLS128.param.yaml | 4 +--- .../config/radar/continental/ARS548.param.yaml | 2 +- .../config/radar/continental/SRR520.param.yaml | 2 +- nebula_ros/schema/Pandar128E4X.schema.json | 14 +++----------- nebula_ros/schema/Pandar40P.schema.json | 14 +++----------- nebula_ros/schema/Pandar64.schema.json | 14 +++----------- nebula_ros/schema/PandarAT128.schema.json | 14 +++----------- nebula_ros/schema/PandarQT128.schema.json | 14 +++----------- nebula_ros/schema/PandarQT64.schema.json | 14 +++----------- nebula_ros/schema/PandarXT32.schema.json | 14 +++----------- nebula_ros/schema/PandarXT32M.schema.json | 14 +++----------- nebula_ros/schema/VLP16.schema.json | 14 +++----------- nebula_ros/schema/VLP32.schema.json | 14 +++----------- nebula_ros/schema/VLS128.schema.json | 14 +++----------- nebula_ros/schema/sub/communication.json | 11 +++++++++++ nebula_ros/schema/sub/hardware.json | 12 ------------ 28 files changed, 59 insertions(+), 170 deletions(-) diff --git a/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml b/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml index da32fc0eb..8593500f5 100644 --- a/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml +++ b/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml @@ -6,9 +6,7 @@ data_port: 2368 gnss_port: 10110 packet_mtu_size: 1500 - launch_hw: true - setup_sensor: true - udp_only: false + connection_mode: full 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 332b2a1c0..68a7eb38a 100644 --- a/nebula_ros/config/lidar/hesai/Pandar40P.param.yaml +++ b/nebula_ros/config/lidar/hesai/Pandar40P.param.yaml @@ -6,9 +6,7 @@ data_port: 2368 gnss_port: 10110 packet_mtu_size: 1500 - launch_hw: true - setup_sensor: true - udp_only: false + connection_mode: full 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 5e78f796a..08a9f65ed 100644 --- a/nebula_ros/config/lidar/hesai/Pandar64.param.yaml +++ b/nebula_ros/config/lidar/hesai/Pandar64.param.yaml @@ -6,9 +6,7 @@ data_port: 2368 gnss_port: 10110 packet_mtu_size: 1500 - launch_hw: true - setup_sensor: true - udp_only: false + connection_mode: full 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 dff7789dd..9641ab704 100644 --- a/nebula_ros/config/lidar/hesai/PandarAT128.param.yaml +++ b/nebula_ros/config/lidar/hesai/PandarAT128.param.yaml @@ -6,9 +6,7 @@ data_port: 2368 gnss_port: 10110 packet_mtu_size: 1500 - launch_hw: true - setup_sensor: true - udp_only: false + connection_mode: full 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 17779ee02..8ad6687e5 100644 --- a/nebula_ros/config/lidar/hesai/PandarQT128.param.yaml +++ b/nebula_ros/config/lidar/hesai/PandarQT128.param.yaml @@ -6,9 +6,7 @@ data_port: 2368 gnss_port: 10110 packet_mtu_size: 1500 - launch_hw: true - setup_sensor: true - udp_only: false + connection_mode: full 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 f11d30076..a4a5fc2c6 100644 --- a/nebula_ros/config/lidar/hesai/PandarQT64.param.yaml +++ b/nebula_ros/config/lidar/hesai/PandarQT64.param.yaml @@ -6,9 +6,7 @@ data_port: 2368 gnss_port: 10110 packet_mtu_size: 1500 - launch_hw: true - setup_sensor: true - udp_only: false + connection_mode: full 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 7288ec371..b4fa1b509 100644 --- a/nebula_ros/config/lidar/hesai/PandarXT32.param.yaml +++ b/nebula_ros/config/lidar/hesai/PandarXT32.param.yaml @@ -6,9 +6,7 @@ data_port: 2368 gnss_port: 10110 packet_mtu_size: 1500 - launch_hw: true - setup_sensor: true - udp_only: false + connection_mode: full 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 cc1fffbc9..876d3e5fe 100644 --- a/nebula_ros/config/lidar/hesai/PandarXT32M.param.yaml +++ b/nebula_ros/config/lidar/hesai/PandarXT32M.param.yaml @@ -6,9 +6,7 @@ data_port: 2368 gnss_port: 10110 packet_mtu_size: 1500 - launch_hw: true - setup_sensor: true - udp_only: false + connection_mode: full frame_id: hesai diag_span: 1000 min_range: 0.3 diff --git a/nebula_ros/config/lidar/robosense/Bpearl.param.yaml b/nebula_ros/config/lidar/robosense/Bpearl.param.yaml index 74a12080d..bc9ed2eb1 100644 --- a/nebula_ros/config/lidar/robosense/Bpearl.param.yaml +++ b/nebula_ros/config/lidar/robosense/Bpearl.param.yaml @@ -5,7 +5,7 @@ data_port: 2368 gnss_port: 2369 packet_mtu_size: 1500 - launch_hw: true + connection_mode: full setup_sensor: true frame_id: robosense diag_span: 1000 diff --git a/nebula_ros/config/lidar/robosense/Helios.param.yaml b/nebula_ros/config/lidar/robosense/Helios.param.yaml index 5694362fc..1d75fbb75 100644 --- a/nebula_ros/config/lidar/robosense/Helios.param.yaml +++ b/nebula_ros/config/lidar/robosense/Helios.param.yaml @@ -5,7 +5,7 @@ data_port: 2368 gnss_port: 2369 packet_mtu_size: 1500 - launch_hw: true + connection_mode: full setup_sensor: true frame_id: robosense diag_span: 1000 diff --git a/nebula_ros/config/lidar/velodyne/VLP16.param.yaml b/nebula_ros/config/lidar/velodyne/VLP16.param.yaml index d44c4d9ce..2b2603793 100644 --- a/nebula_ros/config/lidar/velodyne/VLP16.param.yaml +++ b/nebula_ros/config/lidar/velodyne/VLP16.param.yaml @@ -5,9 +5,7 @@ data_port: 2368 gnss_port: 2369 packet_mtu_size: 1500 - launch_hw: true - setup_sensor: true - udp_only: false + connection_mode: full 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 0a18d780c..e7d478991 100644 --- a/nebula_ros/config/lidar/velodyne/VLP32.param.yaml +++ b/nebula_ros/config/lidar/velodyne/VLP32.param.yaml @@ -5,9 +5,7 @@ data_port: 2368 gnss_port: 2369 packet_mtu_size: 1500 - launch_hw: true - setup_sensor: true - udp_only: false + connection_mode: full 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 10cc8406b..79730dca5 100644 --- a/nebula_ros/config/lidar/velodyne/VLS128.param.yaml +++ b/nebula_ros/config/lidar/velodyne/VLS128.param.yaml @@ -5,9 +5,7 @@ data_port: 2368 gnss_port: 2369 packet_mtu_size: 1500 - launch_hw: true - setup_sensor: true - udp_only: false + connection_mode: full frame_id: velodyne advanced_diagnostics: false diag_span: 1000 diff --git a/nebula_ros/config/radar/continental/ARS548.param.yaml b/nebula_ros/config/radar/continental/ARS548.param.yaml index 7f9a76ed7..d4311f210 100644 --- a/nebula_ros/config/radar/continental/ARS548.param.yaml +++ b/nebula_ros/config/radar/continental/ARS548.param.yaml @@ -6,7 +6,7 @@ frame_id: continental base_frame: base_link object_frame: base_link - launch_hw: true + connection_mode: full multicast_ip: 224.0.2.2 sensor_model: ARS548 configuration_host_port: 42401 diff --git a/nebula_ros/config/radar/continental/SRR520.param.yaml b/nebula_ros/config/radar/continental/SRR520.param.yaml index f3e50f934..02387cc50 100644 --- a/nebula_ros/config/radar/continental/SRR520.param.yaml +++ b/nebula_ros/config/radar/continental/SRR520.param.yaml @@ -4,7 +4,7 @@ frame_id: continental base_frame: base_link interface: can4 - launch_hw: true + connection_mode: full receiver_timeout_sec: 0.03 sender_timeout_sec: 0.01 filters: 0:0 # candump-like filters diff --git a/nebula_ros/schema/Pandar128E4X.schema.json b/nebula_ros/schema/Pandar128E4X.schema.json index 93e5806cc..3bda2c137 100644 --- a/nebula_ros/schema/Pandar128E4X.schema.json +++ b/nebula_ros/schema/Pandar128E4X.schema.json @@ -24,14 +24,8 @@ "packet_mtu_size": { "$ref": "sub/communication.json#/definitions/packet_mtu_size" }, - "launch_hw": { - "$ref": "sub/hardware.json#/definitions/launch_hw" - }, - "setup_sensor": { - "$ref": "sub/hardware.json#/definitions/setup_sensor" - }, - "udp_only": { - "$ref": "sub/hardware.json#/definitions/udp_only" + "connection_mode": { + "$ref": "sub/hardware.json#/definitions/connection_mode" }, "frame_id": { "$ref": "sub/topic.json#/definitions/frame_id" @@ -110,9 +104,7 @@ "data_port", "gnss_port", "packet_mtu_size", - "launch_hw", - "setup_sensor", - "udp_only", + "connection_mode", "frame_id", "diag_span", "cloud_min_angle", diff --git a/nebula_ros/schema/Pandar40P.schema.json b/nebula_ros/schema/Pandar40P.schema.json index 2e13bd43e..340bbac01 100644 --- a/nebula_ros/schema/Pandar40P.schema.json +++ b/nebula_ros/schema/Pandar40P.schema.json @@ -24,14 +24,8 @@ "packet_mtu_size": { "$ref": "sub/communication.json#/definitions/packet_mtu_size" }, - "launch_hw": { - "$ref": "sub/hardware.json#/definitions/launch_hw" - }, - "setup_sensor": { - "$ref": "sub/hardware.json#/definitions/setup_sensor" - }, - "udp_only": { - "$ref": "sub/hardware.json#/definitions/udp_only" + "connection_mode": { + "$ref": "sub/hardware.json#/definitions/connection_mode" }, "frame_id": { "$ref": "sub/topic.json#/definitions/frame_id" @@ -101,9 +95,7 @@ "data_port", "gnss_port", "packet_mtu_size", - "launch_hw", - "setup_sensor", - "udp_only", + "connection_mode", "frame_id", "diag_span", "cloud_min_angle", diff --git a/nebula_ros/schema/Pandar64.schema.json b/nebula_ros/schema/Pandar64.schema.json index 666f1253d..ad30196f9 100644 --- a/nebula_ros/schema/Pandar64.schema.json +++ b/nebula_ros/schema/Pandar64.schema.json @@ -24,14 +24,8 @@ "packet_mtu_size": { "$ref": "sub/communication.json#/definitions/packet_mtu_size" }, - "launch_hw": { - "$ref": "sub/hardware.json#/definitions/launch_hw" - }, - "setup_sensor": { - "$ref": "sub/hardware.json#/definitions/setup_sensor" - }, - "udp_only": { - "$ref": "sub/hardware.json#/definitions/udp_only" + "connection_mode": { + "$ref": "sub/hardware.json#/definitions/connection_mode" }, "frame_id": { "$ref": "sub/topic.json#/definitions/frame_id" @@ -98,9 +92,7 @@ "data_port", "gnss_port", "packet_mtu_size", - "launch_hw", - "setup_sensor", - "udp_only", + "connection_mode", "frame_id", "diag_span", "cloud_min_angle", diff --git a/nebula_ros/schema/PandarAT128.schema.json b/nebula_ros/schema/PandarAT128.schema.json index 61786c368..ef3e96182 100644 --- a/nebula_ros/schema/PandarAT128.schema.json +++ b/nebula_ros/schema/PandarAT128.schema.json @@ -24,14 +24,8 @@ "packet_mtu_size": { "$ref": "sub/communication.json#/definitions/packet_mtu_size" }, - "launch_hw": { - "$ref": "sub/hardware.json#/definitions/launch_hw" - }, - "setup_sensor": { - "$ref": "sub/hardware.json#/definitions/setup_sensor" - }, - "udp_only": { - "$ref": "sub/hardware.json#/definitions/udp_only" + "connection_mode": { + "$ref": "sub/hardware.json#/definitions/connection_mode" }, "frame_id": { "$ref": "sub/topic.json#/definitions/frame_id" @@ -121,9 +115,7 @@ "data_port", "gnss_port", "packet_mtu_size", - "launch_hw", - "setup_sensor", - "udp_only", + "connection_mode", "frame_id", "diag_span", "correction_file", diff --git a/nebula_ros/schema/PandarQT128.schema.json b/nebula_ros/schema/PandarQT128.schema.json index 01f186d52..9d1de1ea1 100644 --- a/nebula_ros/schema/PandarQT128.schema.json +++ b/nebula_ros/schema/PandarQT128.schema.json @@ -24,14 +24,8 @@ "packet_mtu_size": { "$ref": "sub/communication.json#/definitions/packet_mtu_size" }, - "launch_hw": { - "$ref": "sub/hardware.json#/definitions/launch_hw" - }, - "setup_sensor": { - "$ref": "sub/hardware.json#/definitions/setup_sensor" - }, - "udp_only": { - "$ref": "sub/hardware.json#/definitions/udp_only" + "connection_mode": { + "$ref": "sub/hardware.json#/definitions/connection_mode" }, "frame_id": { "$ref": "sub/topic.json#/definitions/frame_id" @@ -104,9 +98,7 @@ "data_port", "gnss_port", "packet_mtu_size", - "launch_hw", - "setup_sensor", - "udp_only", + "connection_mode", "frame_id", "diag_span", "cloud_min_angle", diff --git a/nebula_ros/schema/PandarQT64.schema.json b/nebula_ros/schema/PandarQT64.schema.json index af82060f9..edac35864 100644 --- a/nebula_ros/schema/PandarQT64.schema.json +++ b/nebula_ros/schema/PandarQT64.schema.json @@ -24,14 +24,8 @@ "packet_mtu_size": { "$ref": "sub/communication.json#/definitions/packet_mtu_size" }, - "launch_hw": { - "$ref": "sub/hardware.json#/definitions/launch_hw" - }, - "setup_sensor": { - "$ref": "sub/hardware.json#/definitions/setup_sensor" - }, - "udp_only": { - "$ref": "sub/hardware.json#/definitions/udp_only" + "connection_mode": { + "$ref": "sub/hardware.json#/definitions/connection_mode" }, "frame_id": { "$ref": "sub/topic.json#/definitions/frame_id" @@ -101,9 +95,7 @@ "data_port", "gnss_port", "packet_mtu_size", - "launch_hw", - "setup_sensor", - "udp_only", + "connection_mode", "frame_id", "diag_span", "cloud_min_angle", diff --git a/nebula_ros/schema/PandarXT32.schema.json b/nebula_ros/schema/PandarXT32.schema.json index 1f0c61133..d38d6d62a 100644 --- a/nebula_ros/schema/PandarXT32.schema.json +++ b/nebula_ros/schema/PandarXT32.schema.json @@ -24,14 +24,8 @@ "packet_mtu_size": { "$ref": "sub/communication.json#/definitions/packet_mtu_size" }, - "launch_hw": { - "$ref": "sub/hardware.json#/definitions/launch_hw" - }, - "setup_sensor": { - "$ref": "sub/hardware.json#/definitions/setup_sensor" - }, - "udp_only": { - "$ref": "sub/hardware.json#/definitions/udp_only" + "connection_mode": { + "$ref": "sub/hardware.json#/definitions/connection_mode" }, "frame_id": { "$ref": "sub/topic.json#/definitions/frame_id" @@ -104,9 +98,7 @@ "data_port", "gnss_port", "packet_mtu_size", - "launch_hw", - "setup_sensor", - "udp_only", + "connection_mode", "frame_id", "diag_span", "cloud_min_angle", diff --git a/nebula_ros/schema/PandarXT32M.schema.json b/nebula_ros/schema/PandarXT32M.schema.json index 7ab960449..772bf4892 100644 --- a/nebula_ros/schema/PandarXT32M.schema.json +++ b/nebula_ros/schema/PandarXT32M.schema.json @@ -24,14 +24,8 @@ "packet_mtu_size": { "$ref": "sub/communication.json#/definitions/packet_mtu_size" }, - "launch_hw": { - "$ref": "sub/hardware.json#/definitions/launch_hw" - }, - "setup_sensor": { - "$ref": "sub/hardware.json#/definitions/setup_sensor" - }, - "udp_only": { - "$ref": "sub/hardware.json#/definitions/udp_only" + "connection_mode": { + "$ref": "sub/hardware.json#/definitions/connection_mode" }, "frame_id": { "$ref": "sub/topic.json#/definitions/frame_id" @@ -104,9 +98,7 @@ "data_port", "gnss_port", "packet_mtu_size", - "launch_hw", - "setup_sensor", - "udp_only", + "connection_mode", "frame_id", "diag_span", "cloud_min_angle", diff --git a/nebula_ros/schema/VLP16.schema.json b/nebula_ros/schema/VLP16.schema.json index 4e656db12..faf42f488 100644 --- a/nebula_ros/schema/VLP16.schema.json +++ b/nebula_ros/schema/VLP16.schema.json @@ -21,14 +21,8 @@ "packet_mtu_size": { "$ref": "sub/communication.json#/definitions/packet_mtu_size" }, - "launch_hw": { - "$ref": "sub/hardware.json#/definitions/launch_hw" - }, - "setup_sensor": { - "$ref": "sub/hardware.json#/definitions/setup_sensor" - }, - "udp_only": { - "$ref": "sub/hardware.json#/definitions/udp_only" + "connection_mode": { + "$ref": "sub/hardware.json#/definitions/connection_mode" }, "frame_id": { "$ref": "sub/topic.json#/definitions/frame_id" @@ -73,9 +67,7 @@ "data_port", "gnss_port", "packet_mtu_size", - "launch_hw", - "setup_sensor", - "udp_only", + "connection_mode", "frame_id", "advanced_diagnostics", "diag_span", diff --git a/nebula_ros/schema/VLP32.schema.json b/nebula_ros/schema/VLP32.schema.json index 6eeccbc2a..b4ed493ac 100644 --- a/nebula_ros/schema/VLP32.schema.json +++ b/nebula_ros/schema/VLP32.schema.json @@ -21,14 +21,8 @@ "packet_mtu_size": { "$ref": "sub/communication.json#/definitions/packet_mtu_size" }, - "launch_hw": { - "$ref": "sub/hardware.json#/definitions/launch_hw" - }, - "setup_sensor": { - "$ref": "sub/hardware.json#/definitions/setup_sensor" - }, - "udp_only": { - "$ref": "sub/hardware.json#/definitions/udp_only" + "connection_mode": { + "$ref": "sub/hardware.json#/definitions/connection_mode" }, "frame_id": { "$ref": "sub/topic.json#/definitions/frame_id" @@ -73,9 +67,7 @@ "data_port", "gnss_port", "packet_mtu_size", - "launch_hw", - "setup_sensor", - "udp_only", + "connection_mode", "frame_id", "advanced_diagnostics", "diag_span", diff --git a/nebula_ros/schema/VLS128.schema.json b/nebula_ros/schema/VLS128.schema.json index 802a3b251..759595594 100644 --- a/nebula_ros/schema/VLS128.schema.json +++ b/nebula_ros/schema/VLS128.schema.json @@ -21,14 +21,8 @@ "packet_mtu_size": { "$ref": "sub/communication.json#/definitions/packet_mtu_size" }, - "launch_hw": { - "$ref": "sub/hardware.json#/definitions/launch_hw" - }, - "setup_sensor": { - "$ref": "sub/hardware.json#/definitions/setup_sensor" - }, - "udp_only": { - "$ref": "sub/hardware.json#/definitions/udp_only" + "connection_mode": { + "$ref": "sub/hardware.json#/definitions/connection_mode" }, "frame_id": { "$ref": "sub/topic.json#/definitions/frame_id" @@ -73,9 +67,7 @@ "data_port", "gnss_port", "packet_mtu_size", - "launch_hw", - "setup_sensor", - "udp_only", + "connection_mode", "frame_id", "advanced_diagnostics", "diag_span", diff --git a/nebula_ros/schema/sub/communication.json b/nebula_ros/schema/sub/communication.json index 3823e1e97..8d144e24c 100644 --- a/nebula_ros/schema/sub/communication.json +++ b/nebula_ros/schema/sub/communication.json @@ -115,6 +115,17 @@ "default": "0.01", "minimum": 0.0, "description": "Timeout for sending data to the CAN bus." + }, + "connection_mode": { + "type": "string", + "enum": [ + "offline", + "udp_only", + "skip_setup", + "full" + ], + "default": "full", + "description": "Whether to connect to no sensor and listen to a packets topic (offline), listen to sensor outputs only with limited diagnostics and no settings sync (udp_only), connect fully but not changing sensor settings (skip_setup) or to connect fully with settings sync." } } } diff --git a/nebula_ros/schema/sub/hardware.json b/nebula_ros/schema/sub/hardware.json index e6f0dcee3..1cb978914 100644 --- a/nebula_ros/schema/sub/hardware.json +++ b/nebula_ros/schema/sub/hardware.json @@ -18,22 +18,10 @@ "readOnly": true, "description": "Sensor model." }, - "setup_sensor": { - "type": "boolean", - "default": "true", - "readOnly": true, - "description": "Enable sensor setup on hardware driver." - }, "retry_hw": { "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)." } } } From 48ddb70e442c4a61dfded843c1d9bfb7cf9d3a34 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Fri, 25 Oct 2024 15:47:13 +0900 Subject: [PATCH 2/5] chore: add ConnectionMode enum Signed-off-by: Max SCHMELLER --- .../include/nebula_common/nebula_common.hpp | 36 +++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/nebula_common/include/nebula_common/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp index 36f7f60c3..f6e686247 100644 --- a/nebula_common/include/nebula_common/nebula_common.hpp +++ b/nebula_common/include/nebula_common/nebula_common.hpp @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -344,6 +345,41 @@ enum class SensorModel { CONTINENTAL_SRR520 }; +enum class ConnectionMode { + /// No network connection, listen to replayed packets on a ROS 2 topic + OFFLINE, + /// Passive mode where Nebula does not inquire any settings or diagnostics, only listens via UDP + UDP_ONLY, + /// Nebula compares settings with the sensor and runs all other functionality, but does not modify + /// sensor settings + SKIP_SETUP, + /// All supported network functionality is enabled + FULL +}; + +inline std::ostream & operator<<(std::ostream & os, ConnectionMode const & arg) +{ + switch (arg) { + case ConnectionMode::OFFLINE: + return os << "offline"; + case ConnectionMode::UDP_ONLY: + return os << "UDP only"; + case ConnectionMode::SKIP_SETUP: + return os << "skip setup"; + case ConnectionMode::FULL: + return os << "full"; + } +} + +inline ConnectionMode connection_mode_from_string(const std::string & str) +{ + if (str == "offline") return ConnectionMode::OFFLINE; + if (str == "udp_only") return ConnectionMode::UDP_ONLY; + if (str == "skip_setup") return ConnectionMode::SKIP_SETUP; + if (str == "full") return ConnectionMode::FULL; + throw std::runtime_error("Unknown connection mode " + str); +} + /// @brief not used? enum class datatype { INT8 = 1, From 2436678e4f159e81fc93883f3f0029ccd2e08c89 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Fri, 25 Oct 2024 15:47:55 +0900 Subject: [PATCH 3/5] refactor(hesai): use ConnectionMode enum Signed-off-by: Max SCHMELLER --- .../nebula_ros/hesai/hesai_ros_wrapper.hpp | 2 +- .../nebula_ros/hesai/hw_interface_wrapper.hpp | 6 ++-- nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 32 +++++++++++-------- nebula_ros/src/hesai/hw_interface_wrapper.cpp | 18 +++++++---- 4 files changed, 34 insertions(+), 24 deletions(-) diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp index f8d221c3f..372ffa405 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp @@ -105,7 +105,7 @@ class HesaiRosWrapper final : public rclcpp::Node rclcpp::Subscription::SharedPtr packets_sub_{}; - bool launch_hw_; + drivers::ConnectionMode connection_mode_; std::optional hw_interface_wrapper_; std::optional hw_monitor_wrapper_; 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 4b2572b1b..c1d1d851f 100644 --- a/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp @@ -15,6 +15,7 @@ #pragma once #include +#include #include #include @@ -28,7 +29,7 @@ class HesaiHwInterfaceWrapper HesaiHwInterfaceWrapper( rclcpp::Node * const parent_node, std::shared_ptr & config, - bool use_udp_only = false); + drivers::ConnectionMode connection_mode); void on_config_change( const std::shared_ptr & new_config); @@ -41,7 +42,6 @@ class HesaiHwInterfaceWrapper std::shared_ptr hw_interface_; rclcpp::Logger logger_; nebula::Status status_; - bool setup_sensor_; - bool use_udp_only_; + drivers::ConnectionMode connection_mode_; }; } // namespace nebula::ros diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index 77267c4a6..ca3c31edd 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -39,34 +39,34 @@ 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()); + { + auto connection_mode = declare_parameter("connection_mode", param_read_only()); + connection_mode_ = drivers::connection_mode_from_string(connection_mode); + } - if (use_udp_only) { + if (connection_mode_ == drivers::ConnectionMode::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_, use_udp_only); - if (!use_udp_only) { // hardware monitor requires TCP connection + if (connection_mode_ != drivers::ConnectionMode::OFFLINE) { + hw_interface_wrapper_.emplace(this, sensor_cfg_ptr_, connection_mode_); + // hardware monitor requires TCP connection + if (connection_mode_ != drivers::ConnectionMode::UDP_ONLY) { hw_monitor_wrapper_.emplace(this, hw_interface_wrapper_->hw_interface(), sensor_cfg_ptr_); } } - 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); + auto calibration_result = get_calibration_data(sensor_cfg_ptr_->calibration_path); if (!calibration_result.has_value()) { throw std::runtime_error( (std::stringstream() << "No valid calibration found: " << calibration_result.error()).str()); } if ( - hw_interface_wrapper_ && !use_udp_only && + hw_interface_wrapper_ && connection_mode_ == drivers::ConnectionMode::FULL && sensor_cfg_ptr_->sensor_model != drivers::SensorModel::HESAI_PANDARAT128) { auto status = hw_interface_wrapper_->hw_interface()->checkAndSetLidarRange(*calibration_result.value()); @@ -76,7 +76,9 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) } } - decoder_wrapper_.emplace(this, sensor_cfg_ptr_, calibration_result.value(), launch_hw_); + decoder_wrapper_.emplace( + this, sensor_cfg_ptr_, calibration_result.value(), + connection_mode_ != drivers::ConnectionMode::OFFLINE); RCLCPP_DEBUG(get_logger(), "Starting stream"); @@ -86,7 +88,7 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) } }); - if (launch_hw_) { + if (connection_mode_ != drivers::ConnectionMode::OFFLINE) { hw_interface_wrapper_->hw_interface()->RegisterScanCallback( std::bind(&HesaiRosWrapper::receive_cloud_packet_callback, this, std::placeholders::_1)); stream_start(); @@ -463,7 +465,9 @@ HesaiRosWrapper::get_calibration_result_t HesaiRosWrapper::get_calibration_data( } // If a sensor is connected, try to download and save its calibration data - if (!ignore_others && launch_hw_) { + if ( + !ignore_others && connection_mode_ != drivers::ConnectionMode::OFFLINE && + connection_mode_ != drivers::ConnectionMode::UDP_ONLY) { try { auto raw_data = hw_interface_wrapper_->hw_interface()->GetLidarCalibrationBytes(); RCLCPP_INFO(logger, "Downloaded calibration data from sensor."); diff --git a/nebula_ros/src/hesai/hw_interface_wrapper.cpp b/nebula_ros/src/hesai/hw_interface_wrapper.cpp index bc746c523..58527c38d 100644 --- a/nebula_ros/src/hesai/hw_interface_wrapper.cpp +++ b/nebula_ros/src/hesai/hw_interface_wrapper.cpp @@ -4,18 +4,24 @@ #include "nebula_ros/common/parameter_descriptors.hpp" +#include + +#include + namespace nebula::ros { HesaiHwInterfaceWrapper::HesaiHwInterfaceWrapper( rclcpp::Node * const parent_node, - std::shared_ptr & config, bool use_udp_only) + std::shared_ptr & config, + drivers::ConnectionMode connection_mode) : hw_interface_(new nebula::drivers::HesaiHwInterface()), logger_(parent_node->get_logger().get_child("HwInterface")), status_(Status::NOT_INITIALIZED), - use_udp_only_(use_udp_only) + connection_mode_(connection_mode) { - setup_sensor_ = parent_node->declare_parameter("setup_sensor", param_read_only()); + assert(connection_mode_ != drivers::ConnectionMode::OFFLINE); + bool retry_connect = parent_node->declare_parameter("retry_hw", param_read_only()); status_ = hw_interface_->SetSensorConfiguration( @@ -29,7 +35,7 @@ HesaiHwInterfaceWrapper::HesaiHwInterfaceWrapper( hw_interface_->SetLogger(std::make_shared(parent_node->get_logger())); hw_interface_->SetTargetModel(config->sensor_model); - if (use_udp_only) { + if (connection_mode_ == drivers::ConnectionMode::UDP_ONLY) { // Do not initialize TCP return; } @@ -55,7 +61,7 @@ HesaiHwInterfaceWrapper::HesaiHwInterfaceWrapper( } catch (...) { RCLCPP_ERROR_STREAM(logger_, "Failed to get model from sensor..."); } - if (setup_sensor_) { + if (connection_mode_ == drivers::ConnectionMode::FULL) { hw_interface_->CheckAndSetConfig(); } } else { @@ -71,7 +77,7 @@ void HesaiHwInterfaceWrapper::on_config_change( { hw_interface_->SetSensorConfiguration( std::static_pointer_cast(new_config)); - if (!use_udp_only_ && setup_sensor_) { + if (connection_mode_ == drivers::ConnectionMode::FULL) { hw_interface_->CheckAndSetConfig(); } } From 1e22704be607c9fda57cba342abbaeb0f7656c73 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Fri, 25 Oct 2024 15:48:42 +0900 Subject: [PATCH 4/5] refactor(velodyne): use ConnectionMode enum Signed-off-by: Max SCHMELLER --- .../velodyne/hw_interface_wrapper.hpp | 6 +++--- .../velodyne/velodyne_ros_wrapper.hpp | 2 +- .../src/velodyne/hw_interface_wrapper.cpp | 19 ++++++++++++------- .../src/velodyne/velodyne_ros_wrapper.cpp | 19 ++++++++++++------- 4 files changed, 28 insertions(+), 18 deletions(-) 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 8ae102e9c..64974db46 100644 --- a/nebula_ros/include/nebula_ros/velodyne/hw_interface_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/velodyne/hw_interface_wrapper.hpp @@ -16,6 +16,7 @@ #include "nebula_ros/common/parameter_descriptors.hpp" +#include #include #include #include @@ -30,7 +31,7 @@ class VelodyneHwInterfaceWrapper VelodyneHwInterfaceWrapper( rclcpp::Node * const parent_node, std::shared_ptr & config, - bool use_udp_only = false); + drivers::ConnectionMode connection_mode); void on_config_change( const std::shared_ptr & new_config); @@ -43,7 +44,6 @@ class VelodyneHwInterfaceWrapper std::shared_ptr hw_interface_; rclcpp::Logger logger_; nebula::Status status_; - bool setup_sensor_; - bool use_udp_only_; + drivers::ConnectionMode connection_mode_; }; } // namespace nebula::ros diff --git a/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp index 9f7fe8758..1cef3edc4 100644 --- a/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp @@ -89,7 +89,7 @@ class VelodyneRosWrapper final : public rclcpp::Node rclcpp::Subscription::SharedPtr packets_sub_{}; - bool launch_hw_; + drivers::ConnectionMode connection_mode_; std::optional hw_interface_wrapper_; std::optional hw_monitor_wrapper_; diff --git a/nebula_ros/src/velodyne/hw_interface_wrapper.cpp b/nebula_ros/src/velodyne/hw_interface_wrapper.cpp index 338f2ab3e..57212eb6a 100644 --- a/nebula_ros/src/velodyne/hw_interface_wrapper.cpp +++ b/nebula_ros/src/velodyne/hw_interface_wrapper.cpp @@ -2,18 +2,23 @@ #include "nebula_ros/velodyne/hw_interface_wrapper.hpp" +#include + +#include + namespace nebula::ros { VelodyneHwInterfaceWrapper::VelodyneHwInterfaceWrapper( rclcpp::Node * const parent_node, - std::shared_ptr & config, bool use_udp_only) + std::shared_ptr & config, + drivers::ConnectionMode connection_mode) : hw_interface_(new nebula::drivers::VelodyneHwInterface()), logger_(parent_node->get_logger().get_child("HwInterfaceWrapper")), status_(Status::NOT_INITIALIZED), - use_udp_only_(use_udp_only) + connection_mode_(connection_mode) { - setup_sensor_ = parent_node->declare_parameter("setup_sensor", param_read_only()); + assert(connection_mode_ != drivers::ConnectionMode::OFFLINE); hw_interface_->set_logger( std::make_shared(parent_node->get_logger().get_child("HwInterface"))); @@ -24,7 +29,7 @@ VelodyneHwInterfaceWrapper::VelodyneHwInterfaceWrapper( (std::stringstream{} << "Could not initialize HW interface: " << status_).str()); } - if (use_udp_only_) { + if (connection_mode_ == drivers::ConnectionMode::UDP_ONLY) { // Do not initialize http client return; } @@ -36,7 +41,7 @@ VelodyneHwInterfaceWrapper::VelodyneHwInterfaceWrapper( (std::stringstream{} << "Could not initialize HTTP client: " << status_).str()); } - if (setup_sensor_) { + if (connection_mode_ == drivers::ConnectionMode::FULL) { RCLCPP_INFO_STREAM(logger_, "Setting sensor configuration"); status_ = hw_interface_->set_sensor_configuration(config); } @@ -53,11 +58,11 @@ void VelodyneHwInterfaceWrapper::on_config_change( const std::shared_ptr & new_config) { hw_interface_->initialize_sensor_configuration(new_config); - if (use_udp_only_) { + if (connection_mode_ == drivers::ConnectionMode::UDP_ONLY) { return; } hw_interface_->init_http_client(); - if (setup_sensor_) { + if (connection_mode_ == drivers::ConnectionMode::FULL) { 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 e06535766..5954360de 100644 --- a/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp +++ b/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp @@ -2,6 +2,8 @@ #include "nebula_ros/velodyne/velodyne_ros_wrapper.hpp" +#include + #pragma clang diagnostic ignored "-Wbitwise-instead-of-logical" namespace nebula::ros @@ -26,19 +28,22 @@ 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()); + { + auto connection_mode = declare_parameter("connection_mode", param_read_only()); + connection_mode_ = drivers::connection_mode_from_string(connection_mode); + } - if (use_udp_only) { + if (connection_mode_ == drivers::ConnectionMode::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_, use_udp_only); - if (!use_udp_only) { // hardware monitor requires HTTP connection + if (connection_mode_ != drivers::ConnectionMode::OFFLINE) { + hw_interface_wrapper_.emplace(this, sensor_cfg_ptr_, connection_mode_); + // hardware monitor requires HTTP connection + if (connection_mode_ != drivers::ConnectionMode::UDP_ONLY) { hw_monitor_wrapper_.emplace(this, hw_interface_wrapper_->hw_interface(), sensor_cfg_ptr_); } } @@ -54,7 +59,7 @@ VelodyneRosWrapper::VelodyneRosWrapper(const rclcpp::NodeOptions & options) } }); - if (launch_hw_) { + if (connection_mode_ != drivers::ConnectionMode::OFFLINE) { hw_interface_wrapper_->hw_interface()->register_scan_callback( std::bind(&VelodyneRosWrapper::receive_cloud_packet_callback, this, std::placeholders::_1)); stream_start(); From e29d0cd9b8c120cc57e9fa1353c76d54d8aaccb4 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Fri, 25 Oct 2024 15:56:44 +0900 Subject: [PATCH 5/5] chore: fix typo in connection_mode schema Signed-off-by: Max SCHMELLER --- nebula_ros/schema/sub/communication.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_ros/schema/sub/communication.json b/nebula_ros/schema/sub/communication.json index 8d144e24c..15cc89a38 100644 --- a/nebula_ros/schema/sub/communication.json +++ b/nebula_ros/schema/sub/communication.json @@ -125,7 +125,7 @@ "full" ], "default": "full", - "description": "Whether to connect to no sensor and listen to a packets topic (offline), listen to sensor outputs only with limited diagnostics and no settings sync (udp_only), connect fully but not changing sensor settings (skip_setup) or to connect fully with settings sync." + "description": "Whether to connect to no sensor and listen to a packets topic (offline), listen to sensor outputs only with limited diagnostics and no settings sync (udp_only), connect fully but not change sensor settings (skip_setup) or to connect fully with settings sync." } } }