diff --git a/aip_xx1_gen2_launch/CMakeLists.txt b/aip_xx1_gen2_launch/CMakeLists.txt index 7f8d9f60..779ac533 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} +) 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/config/mosaic_x5_rover.param.yaml b/aip_xx1_gen2_launch/config/mosaic_x5_rover.param.yaml index 05cac6ee..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 @@ -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..c2ed6dc1 100644 --- a/aip_xx1_gen2_launch/launch/gnss.launch.xml +++ b/aip_xx1_gen2_launch/launch/gnss.launch.xml @@ -1,45 +1,31 @@ - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + - - + + - - diff --git a/aip_xx1_gen2_launch/launch/lidar.launch.py b/aip_xx1_gen2_launch/launch/lidar.launch.py index 81a791b7..a2a6d6e1 100644 --- a/aip_xx1_gen2_launch/launch/lidar.launch.py +++ b/aip_xx1_gen2_launch/launch/lidar.launch.py @@ -105,6 +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) sub_launch_actions = [] for launch in config["launches"]: @@ -181,6 +182,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) diff --git a/aip_xx1_gen2_launch/launch/radar.launch.xml b/aip_xx1_gen2_launch/launch/radar.launch.xml index c917c9a6..9ba41052 100644 --- a/aip_xx1_gen2_launch/launch/radar.launch.xml +++ b/aip_xx1_gen2_launch/launch/radar.launch.xml @@ -25,7 +25,8 @@ - + + @@ -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 @@ - - - - - - - - - + + + + + + + + + + + + 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..aae22217 --- /dev/null +++ b/aip_xx1_gen2_launch/scripts/septentrio_heading_converter.py @@ -0,0 +1,95 @@ +#!/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. + +# cspell: ignore 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( + 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: float) -> Quaternion: + # 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: AttEuler) -> None: + # 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) -> 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()