feat: add localization component (#5)

Signed-off-by: Masahiro Kubota <norikenpi@gmail.com>
AutomotiveAIChallenge · Aug 15, 2024 · d02a2ab · d02a2ab
1 parent d2124ba
commit d02a2ab
Show file tree

Hide file tree

Showing 2 changed files with 47 additions and 0 deletions.
diff --git a/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge.launch.xml b/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge.launch.xml
@@ -39,6 +39,50 @@
       </include>
     </group>
   </group>
+
+    <!-- Localization -->
+  <group>
+    <push-ros-namespace namespace="localization"/>
+    <include file="$(find-pkg-share gyro_odometer)/launch/gyro_odometer.launch.xml">
+      <arg name="input_vehicle_twist_with_covariance_topic" value="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
+      <arg name="input_imu_topic" value="/sensing/imu/imu_data"/>
+      <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>
+
+    <node pkg="imu_gnss_poser" exec="imu_gnss_poser_node" name="imu_gnss_poser" output="screen"/>
+
+    <include file="$(find-pkg-share ekf_localizer)/launch/ekf_localizer.launch.xml">
+      <arg name="enable_yaw_bias_estimation" value="false"/>
+      <arg name="tf_rate" value="50.0"/>
+      <arg name="twist_smoothing_steps" value="1"/>
+      <arg name="input_initial_pose_name" value="/localization/initial_pose3d"/>
+      <arg name="input_pose_with_cov_name" value="/localization/imu_gnss_poser/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"/>
+      <arg name="output_biased_pose_name" value="biased_pose"/>
+      <arg name="output_biased_pose_with_covariance_name" value="biased_pose_with_covariance"/>
+      <arg name="output_twist_name" value="twist"/>
+      <arg name="output_twist_with_covariance_name" value="twist_with_covariance"/>
+      <arg name="proc_stddev_vx_c" value="10.0"/>
+      <arg name="proc_stddev_wz_c" value="5.0"/>
+    </include>
+
+    <!-- twist2accel -->
+    <group>
+      <node pkg="twist2accel" exec="twist2accel" name="twist2accel" output="screen">
+        <param name="accel_lowpass_gain" value="0.9"/>
+        <param name="use_odom" value="true"/>
+        <remap from="input/odom" to="/localization/kinematic_state"/>
+        <remap from="input/twist" to="/localization/twist_estimator/twist_with_covariance"/>
+        <remap from="output/accel" to="/localization/acceleration"/>
+      </node>
+    </group>
+
+  </group>
+
   <!-- vehicle -->
   <include file="$(find-pkg-share raw_vehicle_cmd_converter)/launch/raw_vehicle_converter.launch.xml">
     <arg name="converter_param_path" value="$(find-pkg-share aichallenge_launch)/config/converter.param.yaml"/>

diff --git a/aichallenge/workspace/src/aichallenge_launch/package.xml b/aichallenge/workspace/src/aichallenge_launch/package.xml
@@ -14,6 +14,9 @@
   <exec_depend>global_parameter_loader</exec_depend>
   <exec_depend>imu_corrector</exec_depend>
   <exec_depend>imu_gnss_poser</exec_depend>
+  <exec_depend>gyro_odometer</exec_depend>
+  <exec_depend>ekf_localizer</exec_depend>
+  <exec_depend>twist2accel</exec_depend>
   <exec_depend>raw_vehicle_cmd_converter</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
   <exec_depend>vehicle_velocity_converter</exec_depend>