feat(xx1_gen2): update XX1 gen2 sensor configurations (#365)

* update gnss settings for septentrio Signed-off-by: Kento Yabuuchi <[email protected]> * set lidar config * set att_offset for septentio * use odometry for radar * ci(pre-commit): autofix * ignore some spell Signed-off-by: Kento Yabuuchi <[email protected]> * fix: autoware prefix Signed-off-by: Kento Yabuuchi <[email protected]> * fix bug for radar twist/odometry topic contamination * add type annotation in septentrio_heading_converter.py Signed-off-by: Kento Yabuuchi <[email protected]> --------- Signed-off-by: Kento Yabuuchi <[email protected]> Co-authored-by: j4tfwm6z <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
tier4 · Jan 7, 2025 · e7f1a5c · e7f1a5c
1 parent 19c7a48
commit e7f1a5c
Show file tree

Hide file tree

Showing 8 changed files with 210 additions and 101 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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>

diff --git 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
@@ -25,7 +25,8 @@
   </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="twist_topic" value="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
     <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"/>
@@ -36,9 +37,10 @@
 
     <group>
       <push-ros-namespace namespace="front_center"/>
+      <group>
         <include file="$(find-pkg-share nebula_ros)/launch/continental_launch_all_hw.xml" if="$(var launch_driver)">
 
-          <arg name="odometry_topic" value="$(var odometry_topic)"/>
+          <arg name="odometry_topic" value="$(var twist_topic)"/>
           <arg name="acceleration_topic" value="$(var acceleration_topic)"/>
           <arg name="steering_angle_topic" value="$(var steering_angle_topic)"/>
 
@@ -54,23 +56,27 @@
           <arg name="configuration_sensor_port" value="42101"/>
 
           <arg name="new_plug_orientation" value="0"/>
-      </include>
-
-      <include file="$(find-pkg-share autoware_radar_tracks_msgs_converter)/launch/radar_tracks_msgs_converter.launch.xml">
-        <arg name="input/radar_objects" value="objects_raw"/>
-        <arg name="input/odometry" value="$(var odometry_topic)"/>
-        <arg name="output/radar_detected_objects" value="detected_objects"/>
-        <arg name="output/radar_tracked_objects" value="tracked_objects"/>
-        <arg name="use_twist_compensation" value="false"/>
-        <arg name="param_path" value="$(var radar_tracks_msgs_converter_param_path)"/>
-      </include>
+        </include>
+      </group>
+
+      <group>
+        <include file="$(find-pkg-share autoware_radar_tracks_msgs_converter)/launch/radar_tracks_msgs_converter.launch.xml">
+          <arg name="input/radar_objects" value="objects_raw"/>
+          <arg name="input/odometry" value="$(var odometry_topic)"/>
+          <arg name="output/radar_detected_objects" value="detected_objects"/>
+          <arg name="output/radar_tracked_objects" value="tracked_objects"/>
+          <arg name="use_twist_compensation" value="false"/>
+          <arg name="param_path" value="$(var radar_tracks_msgs_converter_param_path)"/>
+        </include>
+      </group>
     </group>
 
     <group>
       <push-ros-namespace namespace="front_left"/>
+      <group>
         <include file="$(find-pkg-share nebula_ros)/launch/continental_launch_all_hw.xml" if="$(var launch_driver)">
 
-          <arg name="odometry_topic" value="$(var odometry_topic)"/>
+          <arg name="odometry_topic" value="$(var twist_topic)"/>
           <arg name="acceleration_topic" value="$(var acceleration_topic)"/>
           <arg name="steering_angle_topic" value="$(var steering_angle_topic)"/>
 
@@ -86,23 +92,27 @@
           <arg name="configuration_sensor_port" value="42101"/>
 
           <arg name="new_plug_orientation" value="0"/>
-      </include>
-
-      <include file="$(find-pkg-share autoware_radar_tracks_msgs_converter)/launch/radar_tracks_msgs_converter.launch.xml">
-        <arg name="input/radar_objects" value="objects_raw"/>
-        <arg name="input/odometry" value="$(var odometry_topic)"/>
-        <arg name="output/radar_detected_objects" value="detected_objects"/>
-        <arg name="output/radar_tracked_objects" value="tracked_objects"/>
-        <arg name="use_twist_compensation" value="false"/>
-        <arg name="param_path" value="$(var radar_tracks_msgs_converter_param_path)"/>
-      </include>
+        </include>
+      </group>
+
+      <group>
+        <include file="$(find-pkg-share autoware_radar_tracks_msgs_converter)/launch/radar_tracks_msgs_converter.launch.xml">
+          <arg name="input/radar_objects" value="objects_raw"/>
+          <arg name="input/odometry" value="$(var odometry_topic)"/>
+          <arg name="output/radar_detected_objects" value="detected_objects"/>
+          <arg name="output/radar_tracked_objects" value="tracked_objects"/>
+          <arg name="use_twist_compensation" value="false"/>
+          <arg name="param_path" value="$(var radar_tracks_msgs_converter_param_path)"/>
+        </include>
+      </group>
     </group>
 
     <group>
       <push-ros-namespace namespace="front_right"/>
+      <group>
         <include file="$(find-pkg-share nebula_ros)/launch/continental_launch_all_hw.xml" if="$(var launch_driver)">
 
-          <arg name="odometry_topic" value="$(var odometry_topic)"/>
+          <arg name="odometry_topic" value="$(var twist_topic)"/>
           <arg name="acceleration_topic" value="$(var acceleration_topic)"/>
           <arg name="steering_angle_topic" value="$(var steering_angle_topic)"/>
 
@@ -118,23 +128,27 @@
           <arg name="configuration_sensor_port" value="42101"/>
 
           <arg name="new_plug_orientation" value="0"/>
-      </include>
-
-      <include file="$(find-pkg-share autoware_radar_tracks_msgs_converter)/launch/radar_tracks_msgs_converter.launch.xml">
-        <arg name="input/radar_objects" value="objects_raw"/>
-        <arg name="input/odometry" value="$(var odometry_topic)"/>
-        <arg name="output/radar_detected_objects" value="detected_objects"/>
-        <arg name="output/radar_tracked_objects" value="tracked_objects"/>
-        <arg name="use_twist_compensation" value="false"/>
-        <arg name="param_path" value="$(var radar_tracks_msgs_converter_param_path)"/>
-      </include>
+        </include>
+      </group>
+
+      <group>
+        <include file="$(find-pkg-share autoware_radar_tracks_msgs_converter)/launch/radar_tracks_msgs_converter.launch.xml">
+          <arg name="input/radar_objects" value="objects_raw"/>
+          <arg name="input/odometry" value="$(var odometry_topic)"/>
+          <arg name="output/radar_detected_objects" value="detected_objects"/>
+          <arg name="output/radar_tracked_objects" value="tracked_objects"/>
+          <arg name="use_twist_compensation" value="false"/>
+          <arg name="param_path" value="$(var radar_tracks_msgs_converter_param_path)"/>
+        </include>
+      </group>
     </group>
 
     <group>
       <push-ros-namespace namespace="rear_center"/>
+      <group>
         <include file="$(find-pkg-share nebula_ros)/launch/continental_launch_all_hw.xml" if="$(var launch_driver)">
 
-          <arg name="odometry_topic" value="$(var odometry_topic)"/>
+          <arg name="odometry_topic" value="$(var twist_topic)"/>
           <arg name="acceleration_topic" value="$(var acceleration_topic)"/>
           <arg name="steering_angle_topic" value="$(var steering_angle_topic)"/>
 
@@ -150,23 +164,27 @@
           <arg name="configuration_sensor_port" value="42101"/>
 
           <arg name="new_plug_orientation" value="0"/>
-      </include>
-
-      <include file="$(find-pkg-share autoware_radar_tracks_msgs_converter)/launch/radar_tracks_msgs_converter.launch.xml">
-        <arg name="input/radar_objects" value="objects_raw"/>
-        <arg name="input/odometry" value="$(var odometry_topic)"/>
-        <arg name="output/radar_detected_objects" value="detected_objects"/>
-        <arg name="output/radar_tracked_objects" value="tracked_objects"/>
-        <arg name="use_twist_compensation" value="false"/>
-        <arg name="param_path" value="$(var radar_tracks_msgs_converter_param_path)"/>
-      </include>
+        </include>
+      </group>
+
+      <group>
+        <include file="$(find-pkg-share autoware_radar_tracks_msgs_converter)/launch/radar_tracks_msgs_converter.launch.xml">
+          <arg name="input/radar_objects" value="objects_raw"/>
+          <arg name="input/odometry" value="$(var odometry_topic)"/>
+          <arg name="output/radar_detected_objects" value="detected_objects"/>
+          <arg name="output/radar_tracked_objects" value="tracked_objects"/>
+          <arg name="use_twist_compensation" value="false"/>
+          <arg name="param_path" value="$(var radar_tracks_msgs_converter_param_path)"/>
+        </include>
+      </group>
     </group>
 
     <group>
       <push-ros-namespace namespace="rear_left"/>
+      <group>
         <include file="$(find-pkg-share nebula_ros)/launch/continental_launch_all_hw.xml" if="$(var launch_driver)">
 
-          <arg name="odometry_topic" value="$(var odometry_topic)"/>
+          <arg name="odometry_topic" value="$(var twist_topic)"/>
           <arg name="acceleration_topic" value="$(var acceleration_topic)"/>
           <arg name="steering_angle_topic" value="$(var steering_angle_topic)"/>
 
@@ -182,23 +200,27 @@
           <arg name="configuration_sensor_port" value="42101"/>
 
           <arg name="new_plug_orientation" value="0"/>
-      </include>
-
-      <include file="$(find-pkg-share autoware_radar_tracks_msgs_converter)/launch/radar_tracks_msgs_converter.launch.xml">
-        <arg name="input/radar_objects" value="objects_raw"/>
-        <arg name="input/odometry" value="$(var odometry_topic)"/>
-        <arg name="output/radar_detected_objects" value="detected_objects"/>
-        <arg name="output/radar_tracked_objects" value="tracked_objects"/>
-        <arg name="use_twist_compensation" value="false"/>
-        <arg name="param_path" value="$(var radar_tracks_msgs_converter_param_path)"/>
-      </include>
+        </include>
+      </group>
+
+      <group>
+        <include file="$(find-pkg-share autoware_radar_tracks_msgs_converter)/launch/radar_tracks_msgs_converter.launch.xml">
+          <arg name="input/radar_objects" value="objects_raw"/>
+          <arg name="input/odometry" value="$(var odometry_topic)"/>
+          <arg name="output/radar_detected_objects" value="detected_objects"/>
+          <arg name="output/radar_tracked_objects" value="tracked_objects"/>
+          <arg name="use_twist_compensation" value="false"/>
+          <arg name="param_path" value="$(var radar_tracks_msgs_converter_param_path)"/>
+        </include>
+      </group>
     </group>
 
     <group>
       <push-ros-namespace namespace="rear_right"/>
+      <group>
         <include file="$(find-pkg-share nebula_ros)/launch/continental_launch_all_hw.xml" if="$(var launch_driver)">
 
-          <arg name="odometry_topic" value="$(var odometry_topic)"/>
+          <arg name="odometry_topic" value="$(var twist_topic)"/>
           <arg name="acceleration_topic" value="$(var acceleration_topic)"/>
           <arg name="steering_angle_topic" value="$(var steering_angle_topic)"/>
 
@@ -214,15 +236,18 @@
           <arg name="configuration_sensor_port" value="42101"/>
 
           <arg name="new_plug_orientation" value="0"/>
-      </include>
-
-      <include file="$(find-pkg-share autoware_radar_tracks_msgs_converter)/launch/radar_tracks_msgs_converter.launch.xml">
-        <arg name="input/radar_objects" value="objects_raw"/>
-        <arg name="input/odometry" value="$(var odometry_topic)"/>
-        <arg name="output/radar_detected_objects" value="detected_objects"/>
-        <arg name="output/radar_tracked_objects" value="tracked_objects"/>
-        <arg name="param_path" value="$(var radar_tracks_msgs_converter_param_path)"/>
-      </include>
+        </include>
+      </group>
+
+      <group>
+        <include file="$(find-pkg-share autoware_radar_tracks_msgs_converter)/launch/radar_tracks_msgs_converter.launch.xml">
+          <arg name="input/radar_objects" value="objects_raw"/>
+          <arg name="input/odometry" value="$(var odometry_topic)"/>
+          <arg name="output/radar_detected_objects" value="detected_objects"/>
+          <arg name="output/radar_tracked_objects" value="tracked_objects"/>
+          <arg name="param_path" value="$(var radar_tracks_msgs_converter_param_path)"/>
+        </include>
+      </group>
     </group>
 
     <!-- merge radar objects -->

diff --git a/aip_xx1_gen2_launch/launch/sensing.launch.xml b/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 -->