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 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/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 7eb4dbcb..33ba7043 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,26 @@ - - - - - - - - - - + + + + + + + + + + + + + - + @@ -214,15 +235,18 @@ - - - - - - - - - + + + + + + + + + + + + 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..641049ab --- /dev/null +++ b/aip_xx1_gen2_launch/scripts/septentrio_heading_converter.py @@ -0,0 +1,93 @@ +#!/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. + +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): + # 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() diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py index b615491a..e51b19d3 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", @@ -278,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", @@ -286,11 +290,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",