porting localization (#121)

* porting localization

* fixed odometry in ekf

* fixed vehicle twist topic name

localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml

@@ -2,15 +2,13 @@
 <launch>
   <include file="$(find-pkg-share ekf_localizer)/launch/ekf_localizer.launch.xml">
     <arg name="enable_yaw_bias_estimation" value="False"/>
-    <arg name="use_pose_with_covariance" value="True"/>
-    <arg name="use_twist_with_covariance" value="True"/>
     <arg name="tf_rate" value="50.0"/>
     <arg name="twist_rate" value="25.0"/>
     <arg name="input_initial_pose_name" value="/initialpose3d"/>
-    <arg name="input_pose_name" value="/localization/pose_estimator/pose"/>
+
     <arg name="input_pose_with_cov_name" value="/localization/pose_estimator/pose_with_covariance"/>
-    <arg name="input_twist_name" value="/localization/twist_estimator/twist"/>
     <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_pose_without_yawbias_name" value="pose_without_yawbias"/>
@@ -25,9 +23,8 @@
   </include>
   <include file="$(find-pkg-share stop_filter)/launch/stop_filter.launch.xml">
     <arg name="use_twist_with_covariance" value="True"/>
-    <arg name="input_twist_name" value="/localization/pose_twist_fusion_filter/twist"/>
+    <arg name="input_odom_name" value="/localization/pose_twist_fusion_filter/kinematic_state"/>
     <arg name="input_twist_with_covariance_name" value="/localization/pose_twist_fusion_filter/twist_with_covariance"/>
-    <arg name="output_twist_name" value="/localization/twist"/>
-    <arg name="output_twist_with_covariance_name" value="/localization/twist_with_covariance"/>
+    <arg name="output_odom_name" value="/localization/kinematic_state"/>
   </include>
 </launch>

localization_launch/launch/twist_estimator/twist_estimator.launch.xml

@@ -1,8 +1,11 @@
 <?xml version="1.0"?>
 <launch>
+  <include file="$(find-pkg-share vehicle_velocity_converter)/launch/vehicle_velocity_converter.launch.xml">
+    <arg name="input_vehicle_velocity_topic" value="/vehicle/status/velocity_status"/>
+    <arg name="output_twist_with_covariance" value="/localization/twist_estimator/vehicle_velocity_converter/twist_with_covariance"/>
+  </include>
   <include file="$(find-pkg-share gyro_odometer)/launch/gyro_odometer.launch.xml">
-    <arg name="use_twist_with_covariance" value="false" />
-    <arg name="output_twist_topic" value="/localization/twist_estimator/twist" />
+    <arg name="input_vehicle_twist_with_covariance_topic" value="/localization/twist_estimator/vehicle_velocity_converter/twist_with_covariance"/>
     <arg name="output_twist_with_covariance_topic" value="/localization/twist_estimator/twist_with_covariance" />
   </include>
 </launch>