From 7fa7b9e7376b19a972836ae854e91d1044b3dc6f Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Fri, 18 Oct 2019 15:29:50 +0900
Subject: [PATCH 1/3] use robot_localization instead of graft

---
 fetch_bringup/config/robot_localization.yaml  | 27 +++++++++++++++++++
 fetch_bringup/launch/fetch.launch             |  4 ++-
 .../include/robot_localization.launch.xml     |  9 +++++++
 3 files changed, 39 insertions(+), 1 deletion(-)
 create mode 100644 fetch_bringup/config/robot_localization.yaml
 create mode 100644 fetch_bringup/launch/include/robot_localization.launch.xml

diff --git a/fetch_bringup/config/robot_localization.yaml b/fetch_bringup/config/robot_localization.yaml
new file mode 100644
index 0000000..ca4b4a8
--- /dev/null
+++ b/fetch_bringup/config/robot_localization.yaml
@@ -0,0 +1,27 @@
+frequency: 50
+sensor_timeout: 1.0
+two_d_mode: true
+publish_tf: true
+publish_acceleration: false
+map_frame: map
+odom_frame: odom
+base_link_frame: base_link
+# odom
+odom0: /odom
+odom0_config: [true, true, false,
+               false, false, true,
+               true, true, false,
+               false, false, true,
+               false, false, false]
+odom0_nodelay: true
+odom0_differential: true
+# imu
+imu0: /imu
+imu0_config: [false, false, false,
+              false, false, false,
+              false, false, false,
+              false, false, true,
+              true, true, false]
+imu0_nodelay: true
+imu0_differential: true
+imu0_remove_gravitational_acceleration: true
diff --git a/fetch_bringup/launch/fetch.launch b/fetch_bringup/launch/fetch.launch
index 542a3be..1551780 100644
--- a/fetch_bringup/launch/fetch.launch
+++ b/fetch_bringup/launch/fetch.launch
@@ -20,7 +20,9 @@
 
   <!-- Odometry -->
   <param name="base_controller/publish_tf" value="false" />
-  <include file="$(find fetch_bringup)/launch/include/graft.launch.xml" />
+  <!-- switch to use robot_localization -->
+  <!-- <include file="$(find fetch_bringup)/launch/include/graft.launch.xml" /> -->
+  <include file="$(find fetch_bringup)/launch/include/robot_localization.launch.xml" />
 
   <!-- URDF -->
   <param name="robot_description" textfile="$(find fetch_description)/robots/fetch.urdf" />
diff --git a/fetch_bringup/launch/include/robot_localization.launch.xml b/fetch_bringup/launch/include/robot_localization.launch.xml
new file mode 100644
index 0000000..aa79707
--- /dev/null
+++ b/fetch_bringup/launch/include/robot_localization.launch.xml
@@ -0,0 +1,9 @@
+<launch>
+
+  <!-- robot localization ukf -->
+  <node pkg="robot_localization" type="ukf_localization_node" name="ukf_se" clear_params="true">
+    <remap from="odometry/filtered" to="/odom_combined" />
+    <rosparam file="$(find fetch_bringup)/config/robot_localization.yaml" command="load" />
+  </node>
+
+</launch>

From 38efb8cec8ee8ee39dcf305ca319af602bf2a174 Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Fri, 18 Oct 2019 15:32:56 +0900
Subject: [PATCH 2/3] add robot_localization in package.xml

---
 fetch_bringup/package.xml | 1 +
 1 file changed, 1 insertion(+)

diff --git a/fetch_bringup/package.xml b/fetch_bringup/package.xml
index 2caaea8..c1be299 100644
--- a/fetch_bringup/package.xml
+++ b/fetch_bringup/package.xml
@@ -26,6 +26,7 @@
   <exec_depend>joy</exec_depend>
   <exec_depend>openni2_launch</exec_depend>
   <exec_depend>ps3joy</exec_depend>
+  <exec_depend>robot_localization</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
   <exec_depend>sensor_msgs</exec_depend>
   <exec_depend version_gte="0.0.4">sick_tim</exec_depend>

From 3e25e1890fa6766724972efd3959f989505094ff Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <knorth55@users.noreply.github.com>
Date: Fri, 27 Dec 2019 03:50:54 +0900
Subject: [PATCH 3/3] update robot_localization parameters

---
 fetch_bringup/config/robot_localization.yaml | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/fetch_bringup/config/robot_localization.yaml b/fetch_bringup/config/robot_localization.yaml
index ca4b4a8..335690d 100644
--- a/fetch_bringup/config/robot_localization.yaml
+++ b/fetch_bringup/config/robot_localization.yaml
@@ -18,10 +18,10 @@ odom0_differential: true
 # imu
 imu0: /imu
 imu0_config: [false, false, false,
-              false, false, false,
+              false, false, true,
               false, false, false,
               false, false, true,
-              true, true, false]
+              true, false, false]
 imu0_nodelay: true
 imu0_differential: true
 imu0_remove_gravitational_acceleration: true