feat(xx1_gen2): update XX1 gen2 sensor configurations

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}
+)

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

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

aip_xx1_gen2_launch/launch/gnss.launch.xml

@@ -1,45 +1,31 @@
 <launch>
 
   <arg name="launch_driver" default="true" />
-  <arg name="gnss_receiver" default="ublox" description="ublox(default) or septentrio"/>
 
   <group>
     <push-ros-namespace namespace="gnss"/>
 
-    <!-- Switch topic name -->
-    <let name="navsatfix_topic_name" value="ublox/nav_sat_fix" if="$(eval &quot;'$(var gnss_receiver)'=='ublox'&quot;)" />
-    <let name="navpvt_topic_name" value="ublox/navpvt" if="$(eval &quot;'$(var gnss_receiver)'=='ublox'&quot;)" />
-    <let name="navsatfix_topic_name" value="septentrio/nav_sat_fix" if="$(eval &quot;'$(var gnss_receiver)'=='septentrio'&quot;)"/>
-    <let name="navpvt_topic_name" value="septentrio/navpvt/unused" if="$(eval &quot;'$(var gnss_receiver)'=='septentrio'&quot;)"/>
-
-    <!-- Ublox Driver -->
-    <group if="$(eval &quot;'$(var gnss_receiver)'=='ublox'&quot;)">
-      <node pkg="ublox_gps" name="ublox" exec="ublox_gps_node" if="$(var launch_driver)" respawn="true" respawn_delay="10.0">
-        <remap from="~/fix" to="~/nav_sat_fix" />
-        <param from="$(find-pkg-share ublox_gps)/config/c94_f9p_rover.yaml"/>
-      </node>
-    </group>
-
     <!-- Septentrio GNSS Driver -->
-    <group if="$(eval &quot;'$(var launch_driver)' and '$(var gnss_receiver)'=='septentrio'&quot;)">
-      <node pkg="septentrio_gnss_driver" name="septentrio" exec="septentrio_gnss_driver_node" if="$(var launch_driver)">
-        <param from="$(find-pkg-share aip_xx1_gen2_launch)/config/mosaic_x5_rover.param.yaml"/>
-        <remap from="navsatfix" to="~/nav_sat_fix"/>
-        <remap from="poscovgeodetic" to="~/poscovgeodetic"/>
-        <remap from="pvtgeodetic" to="~/pvtgeodetic"/>
-      </node>
-    </group>
+    <!-- cspell: ignore atteuler -->
+    <node pkg="septentrio_gnss_driver" name="septentrio" exec="septentrio_gnss_driver_node" if="$(var launch_driver)">
+      <param from="$(find-pkg-share aip_xx1_gen2_launch)/config/mosaic_x5_rover.param.yaml"/>
+      <remap from="navsatfix" to="~/nav_sat_fix"/>
+      <remap from="poscovgeodetic" to="~/poscovgeodetic"/>
+      <remap from="pvtgeodetic" to="~/pvtgeodetic"/>
+      <remap from="atteuler" to="~/atteuler"/>
+    </node>
+
+    <!-- Septentrio Heading Converter -->
+    <node pkg="aip_xx1_gen2_launch" name="septentrio_heading_converter" exec="septentrio_heading_converter.py"/>
 
     <!-- NavSatFix to MGRS Pose -->
     <include file="$(find-pkg-share autoware_gnss_poser)/launch/gnss_poser.launch.xml">
-      <arg name="input_topic_fix" value="$(var navsatfix_topic_name)" />
-      <arg name="input_topic_navpvt" value="$(var navpvt_topic_name)" />
+      <arg name="input_topic_fix" value="septentrio/nav_sat_fix" />
+      <arg name="input_topic_orientation" value="septentrio/orientation"/>
 
       <arg name="output_topic_gnss_pose" value="pose" />
       <arg name="output_topic_gnss_pose_cov" value="pose_with_covariance" />
       <arg name="output_topic_gnss_fixed" value="fixed" />
-
-      <arg name="use_ublox_receiver" value="true" />
     </include>
 
   </group>

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)

aip_xx1_gen2_launch/launch/radar.launch.xml

@@ -25,7 +25,7 @@
   </group>
 
   <group if="$(eval &quot;'$(var ars_version)'=='ars548'&quot;)">
-    <let name="odometry_topic" value="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
+    <let name="odometry_topic" value="/localization/kinematic_state"/>
     <let name="acceleration_topic" value="/localization/acceleration"/>
     <let name="steering_angle_topic" value="/vehicle/status/steering_status_scalar"/>
     <arg name="radar_tracks_msgs_converter_param_path" default="$(find-pkg-share common_sensor_launch)/config/radar_tracks_msgs_converter.param.yaml"/>

aip_xx1_gen2_launch/launch/sensing.launch.xml

@@ -29,7 +29,6 @@
     <!-- GNSS Driver -->
     <include file="$(find-pkg-share aip_xx1_gen2_launch)/launch/gnss.launch.xml">
       <arg name="launch_driver" value="$(var launch_driver)" />
-      <arg name="gnss_receiver" value="septentrio"/>
     </include>
 
     <!-- Radar Driver -->

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):
+        # 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()