Delete unnecessary parts

Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp>
autowarefoundation · Feb 9, 2023 · 048ef6b · 048ef6b
1 parent 18d2e17
commit 048ef6b
Show file tree

Hide file tree

Showing 6 changed files with 38 additions and 21 deletions.
diff --git a/launch/tier4_localization_launch/launch/eagleye/eagleye_rt.launch.xml b/launch/tier4_localization_launch/launch/eagleye/eagleye_rt.launch.xml
@@ -1,15 +1,25 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
-  <arg name="use_tf" default="false"/>
+
   <arg name="config_yaml" default="eagleye_config.param.yaml"/>
+  <arg name="config_path" default="$(find-pkg-share autoware_launch)/config/localization/$(var config_yaml)"/>
+  <arg name="localization_mode" default="lidar" description="select localization mode. lidar, gnss"/>
+  <arg name="use_eagleye_twist" default="false" description="Mode to correct wheel speed and IMU by GNSS Doppler velocity"/>
+  <arg name="output_pose_with_cov_name" default="/localization/pose_estimator/pose_with_covariance"/>
+  <arg name="output_twist_with_cov_name" default="/localization/twist_estimator/twist_with_covariance"/>
+  <arg name="directory_with_related_launches" default="$(find-pkg-share tier4_localization_launch)/launch/eagleye"/>
+
+  <arg name="use_tf" default="false"/>
   <arg name="enable_additional_rolling" default="false"/>
   <arg name="use_multi_antenna_mode" default="false"/>
-  <arg name="directory_with_related_launches" default="$(find-pkg-share tier4_localization_launch)/launch/eagleye"/>
-  <arg name="config_path" default="$(find-pkg-share autoware_launch)/config/localization/$(var config_yaml)"/>
 
   <group>
     <push-ros-namespace namespace="eagleye"/>
 
+    <group if="$(var use_eagleye_twist)">
+      <node pkg="topic_tools" exec="relay" name="eagleye_output_twist_relay" args="twist_with_covariance $(var output_twist_with_cov_name)"/>
+    </group>
+
     <arg name="use_rtk_dead_reckoning" default="false"/>
     <arg name="use_rtk_heading" default="false"/>
 
@@ -134,9 +144,15 @@
     <include file="$(var directory_with_related_launches)/gnss_converter.xml">
       <arg name="config_path" value="$(var config_path)"/>
     </include>
+
   </group>
 
   <include file="$(find-pkg-share eagleye_tf)/launch/tf.xml" if="$(var use_tf)"/>
 
-  <include file="$(var directory_with_related_launches)/fix2pose.xml"/>
+  <group if="$(eval &quot;'$(var localization_mode)'=='gnss'&quot;)">
+    <include file="$(var directory_with_related_launches)/fix2pose.xml">
+      <arg name="output_pose_with_cov_name" value="$(var output_pose_with_cov_name)"/>
+    </include>
+  </group>
+
 </launch>
diff --git a/launch/tier4_localization_launch/launch/eagleye/fix2pose.xml b/launch/tier4_localization_launch/launch/eagleye/fix2pose.xml
@@ -1,11 +1,12 @@
 <?xml version="1.0" encoding="UTF-8"?>
 
 <launch>
-
-  <node pkg="eagleye_fix2pose" name="fix2pose_node" exec="fix2pose" output="screen" >
+  <arg name="output_pose_with_cov_name" default="/localization/pose_estimator/pose_with_covariance"/>
+  <node pkg="eagleye_fix2pose" name="fix2pose_node" exec="fix2pose">
     <!-- /eagleye/fix or  /eagleye/rtk_fix(rtk_dead_reckoning mode) or /GNSS/fix(GNSS pose only mode)-->
     <remap from="eagleye/fix" to="eagleye/fix" />
     <remap from="ekf_trigger_node" to="/localization/pose_twist_fusion_filter/trigger_node"/>
+    <remap from="eagleye/pose_with_covariance" to="$(var output_pose_with_cov_name)"/>
 
     <!-- plane rectangular coordinate number -->
     <param name="plane" value="7"/>

diff --git a/launch/tier4_localization_launch/launch/localization.launch.xml b/launch/tier4_localization_launch/launch/localization.launch.xml
@@ -32,6 +32,7 @@
       <push-ros-namespace namespace="pose_estimator"/>
       <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_estimator/pose_estimator.launch.xml">
         <arg name="localization_mode" value="$(var localization_mode)"/>
+        <arg name="use_eagleye_twist" value="$(var use_eagleye_twist)"/>
       </include>
     </group>
     <!-- twist_estimator module -->
@@ -46,8 +47,6 @@
     <group>
       <push-ros-namespace namespace="pose_twist_fusion_filter"/>
       <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml">
-        <arg name="localization_mode" value="$(var localization_mode)"/>
-        <arg name="use_eagleye_twist" value="$(var use_eagleye_twist)"/>
       </include>
     </group>
     <!-- localization_error_monitor_module -->

diff --git a/launch/tier4_localization_launch/launch/pose_estimator/pose_estimator.launch.xml b/launch/tier4_localization_launch/launch/pose_estimator/pose_estimator.launch.xml
@@ -1,6 +1,7 @@
 <?xml version="1.0"?>
 <launch>
   <arg name="localization_mode" default="lidar" description="select localization mode. lidar, gnss"/>
+  <arg name="use_eagleye_twist" default="false" description="Mode to correct wheel speed and IMU by GNSS Doppler velocity"/>
   <group if="$(eval &quot;'$(var localization_mode)'=='lidar'&quot;)">
     <include file="$(find-pkg-share ndt_scan_matcher)/launch/ndt_scan_matcher.launch.xml">
       <arg name="input_map_points_topic" value="/map/pointcloud_map"/>
@@ -15,6 +16,10 @@
   </group>
 
   <group if="$(eval &quot;'$(var localization_mode)'=='gnss'&quot;)">
-    <include file="$(find-pkg-share tier4_localization_launch)/launch/eagleye/eagleye_rt.launch.xml"/>
+    <include file="$(find-pkg-share tier4_localization_launch)/launch/eagleye/eagleye_rt.launch.xml">
+      <arg name="localization_mode" value="$(var localization_mode)"/>
+      <arg name="use_eagleye_twist" value="$(var use_eagleye_twist)"/>
+      <arg name="output_pose_with_cov_name" value="/localization/pose_estimator/pose_with_covariance"/>
+    </include>
   </group>
 </launch>
diff --git a/...4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml b/...4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml
@@ -1,21 +1,13 @@
 <?xml version="1.0"?>
 <launch>
-  <arg name="localization_mode" default="lidar" description="select localization mode. lidar, gnss"/>
-  <arg name="use_eagleye_twist" default="false" description="Mode to correct wheel speed and IMU by GNSS Doppler velocity"/>
-  <let name="use_eagleye_pose" value="$(eval &quot;'$(var localization_mode)'=='gnss'&quot;)"/>
-  <let name="input_pose_with_cov_name" value="/localization/pose_estimator/pose_with_covariance" if="$(eval &quot;'$(var localization_mode)'=='lidar'&quot;)"/>
-  <let name="input_pose_with_cov_name" value="/localization/pose_estimator/eagleye/pose_with_covariance" if="$(eval &quot;'$(var localization_mode)'=='gnss'&quot;)"/>
-  <let name="input_twist_with_cov_name" value="/localization/twist_estimator/twist_with_covariance" unless="$(var use_eagleye_twist)"/>
-  <let name="input_twist_with_cov_name" value="/localization/pose_estimator/eagleye/twist_with_covariance" if="$(var use_eagleye_pose)"/>
-  <let name="input_twist_with_cov_name" value="/localization/twist_estimator/eagleye/twist_with_covariance" if="$(and $(var use_eagleye_twist) $(not $(var use_eagleye_pose)))"/>
   <group>
     <include file="$(find-pkg-share ekf_localizer)/launch/ekf_localizer.launch.xml">
       <arg name="enable_yaw_bias_estimation" value="true"/>
       <arg name="tf_rate" value="50.0"/>
       <arg name="twist_smoothing_steps" value="2"/>
       <arg name="input_initial_pose_name" value="/initialpose3d"/>
-      <arg name="input_pose_with_cov_name" value="$(var input_pose_with_cov_name)"/>
-      <arg name="input_twist_with_cov_name" value="$(var input_twist_with_cov_name)"/>
+      <arg name="input_pose_with_cov_name" value="/localization/pose_estimator/pose_with_covariance"/>
+      <arg name="input_twist_with_cov_name" value="/localization/twist_estimator/twist_with_covariance"/>
       <arg name="output_odom_name" value="kinematic_state"/>
       <arg name="output_pose_name" value="pose"/>
       <arg name="output_pose_with_covariance_name" value="/localization/pose_with_covariance"/>

diff --git a/launch/tier4_localization_launch/launch/twist_estimator/twist_estimator.launch.xml b/launch/tier4_localization_launch/launch/twist_estimator/twist_estimator.launch.xml
@@ -3,15 +3,19 @@
   <arg name="localization_mode" default="lidar" description="select localization mode. lidar, gnss"/>
   <arg name="use_eagleye_twist" default="false" description="Mode to correct wheel speed and IMU by GNSS Doppler velocity"/>
 
-  <include file="$(find-pkg-share gyro_odometer)/launch/gyro_odometer.launch.xml">
+  <include file="$(find-pkg-share gyro_odometer)/launch/gyro_odometer.launch.xml" unless="$(var use_eagleye_twist)">
     <arg name="input_vehicle_twist_with_covariance_topic" value="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
     <arg name="output_twist_with_covariance_topic" value="/localization/twist_estimator/twist_with_covariance"/>
     <arg name="output_twist_with_covariance_raw_topic" value="/localization/twist_estimator/twist_with_covariance_raw"/>
   </include>
 
   <group unless="$(eval &quot;'$(var localization_mode)'=='gnss'&quot;)">
     <group if="$(var use_eagleye_twist)">
-      <include file="$(find-pkg-share tier4_localization_launch)/launch/eagleye/eagleye_rt.launch.xml"/>
+      <include file="$(find-pkg-share tier4_localization_launch)/launch/eagleye/eagleye_rt.launch.xml">
+        <arg name="localization_mode" value="$(var localization_mode)"/>
+        <arg name="use_eagleye_twist" value="$(var use_eagleye_twist)"/>
+        <arg name="output_twist_with_cov_name" value="/localization/twist_estimator/twist_with_covariance"/>
+      </include>
     </group>
   </group>
 </launch>