From f15f526adfe8abcd51da45bb1c1395bd5ce2b88a Mon Sep 17 00:00:00 2001 From: j4tfwm6z Date: Fri, 29 Nov 2024 17:11:41 +0900 Subject: [PATCH 1/9] update gnss settings for septentrio Signed-off-by: Kento Yabuuchi --- aip_xx1_gen2_launch/CMakeLists.txt | 5 + .../config/mosaic_x5_rover.param.yaml | 2 +- aip_xx1_gen2_launch/launch/gnss.launch.xml | 41 +++----- aip_xx1_gen2_launch/launch/sensing.launch.xml | 1 - .../scripts/septentrio_heading_converter.py | 94 +++++++++++++++++++ 5 files changed, 113 insertions(+), 30 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 775bf214..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 1f389007..e23e668b 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 5ffc1497260914fb423ec999a9179f3638da7f51 Mon Sep 17 00:00:00 2001 From: j4tfwm6z Date: Fri, 29 Nov 2024 17:58:09 +0900 Subject: [PATCH 2/9] 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 61c9b6151e6c1e51cea1fe4fc5fcaa8560743991 Mon Sep 17 00:00:00 2001 From: j4tfwm6z Date: Tue, 3 Dec 2024 16:54:47 +0900 Subject: [PATCH 3/9] 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 5d760bd5b7d25e3a01813cd9c20ea2d80f615099 Mon Sep 17 00:00:00 2001 From: j4tfwm6z Date: Tue, 3 Dec 2024 16:55:19 +0900 Subject: [PATCH 4/9] 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 c917c9a6..3709b2c7 100644 --- a/aip_xx1_gen2_launch/launch/radar.launch.xml +++ b/aip_xx1_gen2_launch/launch/radar.launch.xml @@ -25,7 +25,7 @@ - + From dfd6598c398e1bb8a438a18c9c41c8f3d245fd02 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/9] 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 ba81ebd7612df75219a69937a713f6efdbd7699e Mon Sep 17 00:00:00 2001 From: Kento Yabuuchi Date: Mon, 6 Jan 2025 15:10:59 +0900 Subject: [PATCH 6/9] ignore some spell Signed-off-by: Kento Yabuuchi --- aip_xx1_gen2_launch/launch/gnss.launch.xml | 1 + aip_xx1_gen2_launch/scripts/septentrio_heading_converter.py | 2 ++ 2 files changed, 3 insertions(+) diff --git a/aip_xx1_gen2_launch/launch/gnss.launch.xml b/aip_xx1_gen2_launch/launch/gnss.launch.xml index 5fb3a47b..6a4e65be 100644 --- a/aip_xx1_gen2_launch/launch/gnss.launch.xml +++ b/aip_xx1_gen2_launch/launch/gnss.launch.xml @@ -6,6 +6,7 @@ + diff --git a/aip_xx1_gen2_launch/scripts/septentrio_heading_converter.py b/aip_xx1_gen2_launch/scripts/septentrio_heading_converter.py index 641049ab..0f838a20 100755 --- a/aip_xx1_gen2_launch/scripts/septentrio_heading_converter.py +++ b/aip_xx1_gen2_launch/scripts/septentrio_heading_converter.py @@ -13,6 +13,8 @@ # See the License for the specific language governing permissions and # limitations under the License. +# cspell: ignore atteuler + from autoware_sensing_msgs.msg import GnssInsOrientationStamped from geometry_msgs.msg import Quaternion import numpy as np From 99437781e17053cfff89ed25c4fad2eab47b92f1 Mon Sep 17 00:00:00 2001 From: Kento Yabuuchi Date: Mon, 6 Jan 2025 15:42:23 +0900 Subject: [PATCH 7/9] fix: autoware prefix Signed-off-by: Kento Yabuuchi --- aip_xx1_gen2_launch/launch/gnss.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aip_xx1_gen2_launch/launch/gnss.launch.xml b/aip_xx1_gen2_launch/launch/gnss.launch.xml index 6a4e65be..c2ed6dc1 100644 --- a/aip_xx1_gen2_launch/launch/gnss.launch.xml +++ b/aip_xx1_gen2_launch/launch/gnss.launch.xml @@ -19,7 +19,7 @@ - + From ca1594c77c669e50c4b5135ab3cee10d37307159 Mon Sep 17 00:00:00 2001 From: j4tfwm6z Date: Thu, 5 Dec 2024 11:12:56 +0900 Subject: [PATCH 8/9] fix bug for radar twist/odometry topic contamination --- aip_xx1_gen2_launch/launch/radar.launch.xml | 155 ++++++++++++-------- 1 file changed, 90 insertions(+), 65 deletions(-) diff --git a/aip_xx1_gen2_launch/launch/radar.launch.xml b/aip_xx1_gen2_launch/launch/radar.launch.xml index 3709b2c7..9ba41052 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,27 @@ - - - - - - - - - - + + + + + + + + + + + + + + - + @@ -214,15 +236,18 @@ - - - - - - - - - + + + + + + + + + + + + From 533a5027bef80cdda0cb4eeac1ebdc1dd0283952 Mon Sep 17 00:00:00 2001 From: Kento Yabuuchi Date: Mon, 6 Jan 2025 18:57:03 +0900 Subject: [PATCH 9/9] add type annotation in septentrio_heading_converter.py Signed-off-by: Kento Yabuuchi --- aip_xx1_gen2_launch/scripts/septentrio_heading_converter.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/aip_xx1_gen2_launch/scripts/septentrio_heading_converter.py b/aip_xx1_gen2_launch/scripts/septentrio_heading_converter.py index 0f838a20..aae22217 100755 --- a/aip_xx1_gen2_launch/scripts/septentrio_heading_converter.py +++ b/aip_xx1_gen2_launch/scripts/septentrio_heading_converter.py @@ -34,7 +34,7 @@ def __init__(self): ) self.subscription # prevent unused variable warning - def heading_to_quaternion(self, heading): + def heading_to_quaternion(self, heading: float) -> Quaternion: # The input heading is in a North-East coordinate system and measured in degrees. # Heading values range from [0, 360). # Examples: @@ -56,7 +56,7 @@ def heading_to_quaternion(self, heading): q.w = np.cos(np.deg2rad(90 - heading) / 2.0) return q - def attitude_callback(self, attitude_msg): + def attitude_callback(self, attitude_msg: AttEuler) -> None: # When septentrio driver cannot measure the heading, it will publish -20000000000.0 if attitude_msg.heading < 0: return @@ -77,7 +77,7 @@ def attitude_callback(self, attitude_msg): self.publisher.publish(orientation_msg) -def main(args=None): +def main(args=None) -> None: rclpy.init(args=args) converter = OrientationConverter()