From f5fa00ce9eb33ec77d8af5349bf3b55f1488bd77 Mon Sep 17 00:00:00 2001 From: j4tfwm6z Date: Fri, 29 Nov 2024 17:11:41 +0900 Subject: [PATCH 1/8] use gnss orientation & add septentrio_heading_converter --- aip_xx1_gen2_launch/CMakeLists.txt | 5 + .../config/mosaic_x5_rover.param.yaml | 2 +- aip_xx1_gen2_launch/launch/gnss.launch.xml | 39 +++----- aip_xx1_gen2_launch/launch/sensing.launch.xml | 1 - .../scripts/septentrio_heading_converter.py | 94 +++++++++++++++++++ 5 files changed, 112 insertions(+), 29 deletions(-) create mode 100755 aip_xx1_gen2_launch/scripts/septentrio_heading_converter.py diff --git a/aip_xx1_gen2_launch/CMakeLists.txt b/aip_xx1_gen2_launch/CMakeLists.txt index 7f8d9f60..54e2d2b0 100644 --- a/aip_xx1_gen2_launch/CMakeLists.txt +++ b/aip_xx1_gen2_launch/CMakeLists.txt @@ -14,3 +14,8 @@ ament_auto_package(INSTALL_TO_SHARE data config ) + +install(PROGRAMS + scripts/septentrio_heading_converter.py + DESTINATION lib/${PROJECT_NAME} +) \ No newline at end of file diff --git a/aip_xx1_gen2_launch/config/mosaic_x5_rover.param.yaml b/aip_xx1_gen2_launch/config/mosaic_x5_rover.param.yaml index 05cac6ee..bd50fede 100644 --- a/aip_xx1_gen2_launch/config/mosaic_x5_rover.param.yaml +++ b/aip_xx1_gen2_launch/config/mosaic_x5_rover.param.yaml @@ -83,7 +83,7 @@ poscovcartesian: false poscovgeodetic: true velcovgeodetic: false - atteuler: false + atteuler: true attcoveuler: false pose: false twist: false diff --git a/aip_xx1_gen2_launch/launch/gnss.launch.xml b/aip_xx1_gen2_launch/launch/gnss.launch.xml index be5ffc12..5fb3a47b 100644 --- a/aip_xx1_gen2_launch/launch/gnss.launch.xml +++ b/aip_xx1_gen2_launch/launch/gnss.launch.xml @@ -1,45 +1,30 @@ - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + - - + + - - diff --git a/aip_xx1_gen2_launch/launch/sensing.launch.xml b/aip_xx1_gen2_launch/launch/sensing.launch.xml index 87b928b2..50c100eb 100644 --- a/aip_xx1_gen2_launch/launch/sensing.launch.xml +++ b/aip_xx1_gen2_launch/launch/sensing.launch.xml @@ -29,7 +29,6 @@ - diff --git a/aip_xx1_gen2_launch/scripts/septentrio_heading_converter.py b/aip_xx1_gen2_launch/scripts/septentrio_heading_converter.py new file mode 100755 index 00000000..205e2316 --- /dev/null +++ b/aip_xx1_gen2_launch/scripts/septentrio_heading_converter.py @@ -0,0 +1,94 @@ +#!/usr/bin/env python3 +# Copyright 2024 TIER IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import rclpy +from rclpy.node import Node +from septentrio_gnss_driver.msg import AttEuler +from autoware_sensing_msgs.msg import GnssInsOrientationStamped +from geometry_msgs.msg import Quaternion +import numpy as np + + +class OrientationConverter(Node): + + def __init__(self): + super().__init__("septentrio_orientation_converter") + self.publisher = self.create_publisher( + GnssInsOrientationStamped, "/sensing/gnss/septentrio/orientation", 10 + ) + self.subscription = self.create_subscription( + AttEuler, "/sensing/gnss/septentrio/atteuler", self.attitude_callback, 10 + ) + self.subscription # prevent unused variable warning + + def heading_to_quaternion(self, heading): + # The input heading is in a North-East coordinate system and measured in degrees. + # Heading values range from [0, 360). + # Examples: + # - Heading is 0[deg] when the vehicle faces North. + # - Heading is 90[deg] when the vehicle faces East. + # - Heading is 180[deg] when the vehicle faces South. + # - Heading is 270[deg] when the vehicle faces West. + + # The output quaternion represents orientation in an East-North-Up (ENU) coordinate system. + # The quaternion is of the form [x, y, z, w], where: + # - Facing East: [x, y, z, w] = [0, 0, 0, 1] = [0, 0, sin( 0[deg]), cos( 0[deg])] + # - Facing North: [x, y, z, w] = [0, 0, 0.7, 0.7] = [0, 0, sin( 45[deg]), cos( 45[deg])] + # - Facing West: [x, y, z, w] = [0, 0, 1, 0] = [0, 0, sin( 90[deg]), cos( 90[deg])] + # - Facing South: [x, y, z, w] = [0, 0, -0.7, 0.7] = [0, 0, sin(135[deg]), cos(135[deg])] + q = Quaternion() + q.x = 0.0 + q.y = 0.0 + q.z = np.sin(np.deg2rad(90 - heading) / 2.0) + q.w = np.cos(np.deg2rad(90 - heading) / 2.0) + return q + + def attitude_callback(self, attitude_msg): + # When septentrio driver cannot measure the heading, it will publish -20000000000.0 + if attitude_msg.heading < 0: + return + + orientation_msg = GnssInsOrientationStamped() + orientation_msg.header = attitude_msg.header + + # Even if using dual antenna, the roll is not estimated by the septentrio driver. + # Therefore, this assume the roll & pitch are 0 and convert the heading to quaternion. + orientation_msg.orientation.orientation = self.heading_to_quaternion(attitude_msg.heading) + + # Septentrio driver does not provide the covariance of the heading. + # Therefore, this assumes the covariance of the heading is 1.0. + orientation_msg.orientation.rmse_rotation_x = 1.0 + orientation_msg.orientation.rmse_rotation_y = 1.0 + orientation_msg.orientation.rmse_rotation_z = 1.0 + + self.publisher.publish(orientation_msg) + + +def main(args=None): + rclpy.init(args=args) + + converter = OrientationConverter() + + rclpy.spin(converter) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + converter.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() From abd40dd2a22fa55a5805964fd81bc9b877f16d65 Mon Sep 17 00:00:00 2001 From: j4tfwm6z Date: Fri, 29 Nov 2024 17:58:09 +0900 Subject: [PATCH 2/8] set lidar config --- aip_xx1_gen2_launch/config/lidar_gen2.yaml | 7 ++----- aip_xx1_gen2_launch/launch/lidar.launch.py | 4 ++++ 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/aip_xx1_gen2_launch/config/lidar_gen2.yaml b/aip_xx1_gen2_launch/config/lidar_gen2.yaml index 8480292f..c081a7fa 100644 --- a/aip_xx1_gen2_launch/config/lidar_gen2.yaml +++ b/aip_xx1_gen2_launch/config/lidar_gen2.yaml @@ -7,23 +7,20 @@ launches: sensor_ip: 192.168.1.201 data_port: 2368 scan_phase: 160.0 - vertical_bins: 128 - sensor_type: hesai_XT32 namespace: front_left parameters: - max_range: 300.0 + max_range: 80.0 sensor_frame: hesai_front_left sensor_ip: 192.168.1.21 data_port: 2369 scan_phase: 50.0 cloud_min_angle: 50 cloud_max_angle: 320 - vertical_bins: 16 - horizontal_ring_id: 0 - sensor_type: hesai_XT32 namespace: front_right parameters: - max_range: 300.0 + max_range: 80.0 sensor_frame: hesai_front_right sensor_ip: 192.168.1.22 data_port: 2370 diff --git a/aip_xx1_gen2_launch/launch/lidar.launch.py b/aip_xx1_gen2_launch/launch/lidar.launch.py index 81a791b7..24ffcbe3 100644 --- a/aip_xx1_gen2_launch/launch/lidar.launch.py +++ b/aip_xx1_gen2_launch/launch/lidar.launch.py @@ -105,6 +105,9 @@ def load_yaml(yaml_file_path): base_parameters["enable_blockage_diag"] = LaunchConfiguration("enable_blockage_diag").perform( context ) + base_parameters["return_mode"] = LaunchConfiguration("return_mode").perform( + context + ) sub_launch_actions = [] for launch in config["launches"]: @@ -181,6 +184,7 @@ def add_launch_arg(name: str, default_value=None, **kwargs): add_launch_arg("use_pointcloud_container", "false", description="launch pointcloud container") add_launch_arg("pointcloud_container_name", "pointcloud_container") add_launch_arg("enable_blockage_diag", "false") + add_launch_arg("return_mode", "Dual") # Create launch description with the config_file argument ld = LaunchDescription(launch_arguments) From c55bdda2cdea35a1687424613a6c4c908a0c1eac Mon Sep 17 00:00:00 2001 From: j4tfwm6z Date: Tue, 3 Dec 2024 16:54:47 +0900 Subject: [PATCH 3/8] set att_offset for septentio --- aip_xx1_gen2_launch/config/mosaic_x5_rover.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aip_xx1_gen2_launch/config/mosaic_x5_rover.param.yaml b/aip_xx1_gen2_launch/config/mosaic_x5_rover.param.yaml index bd50fede..6f0ce8d0 100644 --- a/aip_xx1_gen2_launch/config/mosaic_x5_rover.param.yaml +++ b/aip_xx1_gen2_launch/config/mosaic_x5_rover.param.yaml @@ -27,7 +27,7 @@ datum: Default att_offset: - heading: 0.0 + heading: -90.0 pitch: 0.0 ant_type: Unknown From 82fb236c8e072d84279f74be9a8c1c9b4a52dc70 Mon Sep 17 00:00:00 2001 From: j4tfwm6z Date: Tue, 3 Dec 2024 16:55:19 +0900 Subject: [PATCH 4/8] use odometry for radar --- aip_xx1_gen2_launch/launch/radar.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aip_xx1_gen2_launch/launch/radar.launch.xml b/aip_xx1_gen2_launch/launch/radar.launch.xml index 7eb4dbcb..9b6f1363 100644 --- a/aip_xx1_gen2_launch/launch/radar.launch.xml +++ b/aip_xx1_gen2_launch/launch/radar.launch.xml @@ -25,7 +25,7 @@ - + From f423cb6d73d540f1eacb76dbebd81a0ee4542e0c Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 25 Dec 2024 09:09:52 +0000 Subject: [PATCH 5/8] ci(pre-commit): autofix --- aip_xx1_gen2_launch/CMakeLists.txt | 2 +- aip_xx1_gen2_launch/launch/lidar.launch.py | 4 +--- .../scripts/septentrio_heading_converter.py | 7 +++---- 3 files changed, 5 insertions(+), 8 deletions(-) diff --git a/aip_xx1_gen2_launch/CMakeLists.txt b/aip_xx1_gen2_launch/CMakeLists.txt index 54e2d2b0..779ac533 100644 --- a/aip_xx1_gen2_launch/CMakeLists.txt +++ b/aip_xx1_gen2_launch/CMakeLists.txt @@ -18,4 +18,4 @@ ament_auto_package(INSTALL_TO_SHARE install(PROGRAMS scripts/septentrio_heading_converter.py DESTINATION lib/${PROJECT_NAME} -) \ No newline at end of file +) diff --git a/aip_xx1_gen2_launch/launch/lidar.launch.py b/aip_xx1_gen2_launch/launch/lidar.launch.py index 24ffcbe3..a2a6d6e1 100644 --- a/aip_xx1_gen2_launch/launch/lidar.launch.py +++ b/aip_xx1_gen2_launch/launch/lidar.launch.py @@ -105,9 +105,7 @@ def load_yaml(yaml_file_path): base_parameters["enable_blockage_diag"] = LaunchConfiguration("enable_blockage_diag").perform( context ) - base_parameters["return_mode"] = LaunchConfiguration("return_mode").perform( - context - ) + base_parameters["return_mode"] = LaunchConfiguration("return_mode").perform(context) sub_launch_actions = [] for launch in config["launches"]: diff --git a/aip_xx1_gen2_launch/scripts/septentrio_heading_converter.py b/aip_xx1_gen2_launch/scripts/septentrio_heading_converter.py index 205e2316..641049ab 100755 --- a/aip_xx1_gen2_launch/scripts/septentrio_heading_converter.py +++ b/aip_xx1_gen2_launch/scripts/septentrio_heading_converter.py @@ -13,16 +13,15 @@ # See the License for the specific language governing permissions and # limitations under the License. -import rclpy -from rclpy.node import Node -from septentrio_gnss_driver.msg import AttEuler from autoware_sensing_msgs.msg import GnssInsOrientationStamped from geometry_msgs.msg import Quaternion import numpy as np +import rclpy +from rclpy.node import Node +from septentrio_gnss_driver.msg import AttEuler class OrientationConverter(Node): - def __init__(self): super().__init__("septentrio_orientation_converter") self.publisher = self.create_publisher( From 415f8ace8af0728f1d35e4eba80c94e003271def Mon Sep 17 00:00:00 2001 From: j4tfwm6z Date: Thu, 5 Dec 2024 10:43:17 +0900 Subject: [PATCH 6/8] TEMP: adopt for single node nebula --- .../launch/nebula_node_container.launch.py | 190 +++++++++++------- 1 file changed, 116 insertions(+), 74 deletions(-) diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py index b615491a..565d2385 100644 --- a/common_sensor_launch/launch/nebula_node_container.launch.py +++ b/common_sensor_launch/launch/nebula_node_container.launch.py @@ -92,78 +92,78 @@ def create_parameter_dict(*args): ) ) - nodes.append( - ComposableNode( - package="nebula_ros", - plugin=sensor_make + "DriverRosWrapper", - name=sensor_make.lower() + "_driver_ros_wrapper_node", - parameters=[ - { - "calibration_file": sensor_calib_fp, - "sensor_model": sensor_model, - **create_parameter_dict( - "host_ip", - "sensor_ip", - "data_port", - "return_mode", - "min_range", - "max_range", - "frame_id", - "scan_phase", - "cloud_min_angle", - "cloud_max_angle", - "dual_return_distance_threshold", - "setup_sensor", - "retry_hw", - ), - }, - ], - remappings=[ - ("aw_points", "pointcloud_raw"), - ("aw_points_ex", "pointcloud_raw_ex"), - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - # There is an issue where hw_monitor crashes due to data race, - # so the monitor will now only be launched when explicitly specified with a launch command. - launch_hw_monitor: bool = IfCondition(LaunchConfiguration("launch_hw_monitor")).evaluate( - context - ) - launch_driver: bool = IfCondition(LaunchConfiguration("launch_driver")).evaluate(context) - if launch_hw_monitor and launch_driver: - nodes.append( - ComposableNode( - package="nebula_ros", - plugin=sensor_make + "HwMonitorRosWrapper", - name=sensor_make.lower() + "_hw_monitor_ros_wrapper_node", - parameters=[ - { - "sensor_model": sensor_model, - **create_parameter_dict( - "return_mode", - "frame_id", - "scan_phase", - "sensor_ip", - "host_ip", - "data_port", - "gnss_port", - "packet_mtu_size", - "rotation_speed", - "cloud_min_angle", - "cloud_max_angle", - "diag_span", - "dual_return_distance_threshold", - "delay_monitor_ms", - ), - }, - ], - extra_arguments=[ - {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} - ], - ) - ) + # nodes.append( + # ComposableNode( + # package="nebula_ros", + # plugin=sensor_make + "DriverRosWrapper", + # name=sensor_make.lower() + "_driver_ros_wrapper_node", + # parameters=[ + # { + # "calibration_file": sensor_calib_fp, + # "sensor_model": sensor_model, + # **create_parameter_dict( + # "host_ip", + # "sensor_ip", + # "data_port", + # "return_mode", + # "min_range", + # "max_range", + # "frame_id", + # "scan_phase", + # "cloud_min_angle", + # "cloud_max_angle", + # "dual_return_distance_threshold", + # "setup_sensor", + # "retry_hw", + # ), + # }, + # ], + # remappings=[ + # ("aw_points", "pointcloud_raw"), + # ("aw_points_ex", "pointcloud_raw_ex"), + # ], + # extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + # ) + # ) + + # # There is an issue where hw_monitor crashes due to data race, + # # so the monitor will now only be launched when explicitly specified with a launch command. + # launch_hw_monitor: bool = IfCondition(LaunchConfiguration("launch_hw_monitor")).evaluate( + # context + # ) + # launch_driver: bool = IfCondition(LaunchConfiguration("launch_driver")).evaluate(context) + # if launch_hw_monitor and launch_driver: + # nodes.append( + # ComposableNode( + # package="nebula_ros", + # plugin=sensor_make + "HwMonitorRosWrapper", + # name=sensor_make.lower() + "_hw_monitor_ros_wrapper_node", + # parameters=[ + # { + # "sensor_model": sensor_model, + # **create_parameter_dict( + # "return_mode", + # "frame_id", + # "scan_phase", + # "sensor_ip", + # "host_ip", + # "data_port", + # "gnss_port", + # "packet_mtu_size", + # "rotation_speed", + # "cloud_min_angle", + # "cloud_max_angle", + # "diag_span", + # "dual_return_distance_threshold", + # "delay_monitor_ms", + # ), + # }, + # ], + # extra_arguments=[ + # {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + # ], + # ) + # ) cropbox_parameters = create_parameter_dict("input_frame", "output_frame") cropbox_parameters["negative"] = True @@ -258,15 +258,17 @@ def create_parameter_dict(*args): output="both", ) + launch_hw: bool = IfCondition(LaunchConfiguration("launch_driver")).evaluate(context) driver_component = ComposableNode( package="nebula_ros", - plugin=sensor_make + "HwInterfaceRosWrapper", + plugin=sensor_make + "RosWrapper", # node is created in a global context, need to avoid name clash - name=sensor_make.lower() + "_hw_interface_ros_wrapper_node", + name=sensor_make.lower() + "_ros_wrapper_node", parameters=[ { "sensor_model": sensor_model, "calibration_file": sensor_calib_fp, + "launch_hw": launch_hw, **create_parameter_dict( "sensor_ip", "host_ip", @@ -286,11 +288,51 @@ def create_parameter_dict(*args): "ptp_switch_type", "ptp_domain", "retry_hw", + "diag_span", + "delay_monitor_ms", ), } ], + remappings=[ + ("aw_points", "pointcloud_raw"), + ("aw_points_ex", "pointcloud_raw_ex"), + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) + # driver_component = ComposableNode( + # package="nebula_ros", + # plugin=sensor_make + "HwInterfaceRosWrapper", + # # node is created in a global context, need to avoid name clash + # name=sensor_make.lower() + "_hw_interface_ros_wrapper_node", + # parameters=[ + # { + # "sensor_model": sensor_model, + # "calibration_file": sensor_calib_fp, + # **create_parameter_dict( + # "sensor_ip", + # "host_ip", + # "scan_phase", + # "return_mode", + # "frame_id", + # "rotation_speed", + # "data_port", + # "gnss_port", + # "cloud_min_angle", + # "cloud_max_angle", + # "packet_mtu_size", + # "dual_return_distance_threshold", + # "setup_sensor", + # "ptp_profile", + # "ptp_transport_type", + # "ptp_switch_type", + # "ptp_domain", + # "retry_hw", + # ), + # } + # ], + # ) + blockage_diag_component = ComposableNode( package="pointcloud_preprocessor", plugin="pointcloud_preprocessor::BlockageDiagComponent", From e97c954147d45d6e11a368879aedca3d0be08abe Mon Sep 17 00:00:00 2001 From: j4tfwm6z Date: Thu, 5 Dec 2024 11:12:56 +0900 Subject: [PATCH 7/8] fix bug for radar twist/odometry topic contamination --- aip_xx1_gen2_launch/launch/radar.launch.xml | 154 +++++++++++--------- 1 file changed, 89 insertions(+), 65 deletions(-) diff --git a/aip_xx1_gen2_launch/launch/radar.launch.xml b/aip_xx1_gen2_launch/launch/radar.launch.xml index 9b6f1363..33ba7043 100644 --- a/aip_xx1_gen2_launch/launch/radar.launch.xml +++ b/aip_xx1_gen2_launch/launch/radar.launch.xml @@ -26,6 +26,7 @@ + @@ -36,9 +37,10 @@ + - + @@ -54,23 +56,27 @@ - - - - - - - - - - + + + + + + + + + + + + + + - + @@ -86,23 +92,27 @@ - - - - - - - - - - + + + + + + + + + + + + + + - + @@ -118,23 +128,27 @@ - - - - - - - - - - + + + + + + + + + + + + + + - + @@ -150,23 +164,27 @@ - - - - - - - - - - + + + + + + + + + + + + + + - + @@ -182,23 +200,26 @@ - - - - - - - - - - + + + + + + + + + + + + + - + @@ -214,15 +235,18 @@ - - - - - - - - - + + + + + + + + + + + + From 4a793b586abe2d12625ba1c61168b35265bbcbd4 Mon Sep 17 00:00:00 2001 From: j4tfwm6z Date: Mon, 6 Jan 2025 17:40:34 +0900 Subject: [PATCH 8/8] pass min/max_range to ros_wrapper --- common_sensor_launch/launch/nebula_node_container.launch.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py index 565d2385..e51b19d3 100644 --- a/common_sensor_launch/launch/nebula_node_container.launch.py +++ b/common_sensor_launch/launch/nebula_node_container.launch.py @@ -280,6 +280,8 @@ def create_parameter_dict(*args): "gnss_port", "cloud_min_angle", "cloud_max_angle", + "min_range", + "max_range", "packet_mtu_size", "dual_return_distance_threshold", "setup_sensor",