From d470fd8595cc9c0c30242d9e21a427dbcda8a605 Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Wed, 16 Oct 2019 23:14:10 +0900
Subject: [PATCH 01/28] add imu_corrector.py

---
 .../launch/fetch_bringup.launch               |  7 ++++++
 .../scripts/imu_corrector.py                  | 24 +++++++++++++++++++
 2 files changed, 31 insertions(+)
 create mode 100755 jsk_fetch_robot/jsk_fetch_startup/scripts/imu_corrector.py

diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
index 8a2c7660a5..85463582da 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
+++ b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
@@ -172,4 +172,11 @@ aggressive_reset: {reset_distance: 0.2} # 0.5
 move_slow_and_clear: {clearing_distance: 0.5, limited_distance: 0.3, limited_rot_speed: 0.45, limited_trans_speed: 0.25}
     </rosparam>
   </group>
+
+  <!-- /imu has no frame_id information and there is no bug fix release in indigo. -->
+  <!-- see https://github.com/fetchrobotics/fetch_ros/issues/70 -->
+  <node name="imu_corrector" pkg="jsk_fetch_startup" type="imu_corrector.py">
+    <remap from="~input" to="/imu" />
+    <remap from="~output" to="/imu_corrected" />
+  </node>
 </launch>
diff --git a/jsk_fetch_robot/jsk_fetch_startup/scripts/imu_corrector.py b/jsk_fetch_robot/jsk_fetch_startup/scripts/imu_corrector.py
new file mode 100755
index 0000000000..ca8c3d5c66
--- /dev/null
+++ b/jsk_fetch_robot/jsk_fetch_startup/scripts/imu_corrector.py
@@ -0,0 +1,24 @@
+#!/usr/bin/env python
+
+import rospy
+
+from sensor_msgs.msg import Imu
+
+
+class ImuCorrector(object):
+    def __init__(self):
+        super(ImuCorrector, self).__init__()
+        self.sub = rospy.Subscriber(
+            '~input', Imu, self._cb, queue_size=1)
+        self.pub = rospy.Publisher(
+            '~output', Imu, queue_size=1)
+
+    def _cb(self, msg):
+        msg.header.frame_id = 'base_link'
+        self.pub.publish(msg)
+
+
+if __name__ == '__main__':
+    rospy.init_node('imu_corrector')
+    app = ImuCorrector()
+    rospy.spin()

From bc4c04d5eb2535a4084e216e28f277d877b351c1 Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Thu, 17 Oct 2019 01:09:59 +0900
Subject: [PATCH 02/28] add indentation in fetch_bringup parameters

---
 .../launch/fetch_bringup.launch               | 52 +++++++++++--------
 1 file changed, 29 insertions(+), 23 deletions(-)

diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
index 85463582da..2f8334a0ad 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
+++ b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
@@ -143,33 +143,39 @@
     </rosparam>
 
     <rosparam ns="move_base/global_costmap">
-inflater:
-  inflation_radius: 0.30 # 0.7
-  cost_scaling_factor: 10.0 # 10.0
+      inflater:
+        inflation_radius: 0.30 # 0.7
+        cost_scaling_factor: 10.0 # 10.0
     </rosparam>
     <rosparam ns="move_base/local_costmap">
-inflater:
-  inflation_radius: 0.30 # 0.7
-  cost_scaling_factor: 100.0 # 25.0 default 10, increasing factor decrease the cost value
-update_frequency: 10.0 # default 5 (http://wiki.ros.org/navigation/Tutorials/Navigation%20Tuning%20Guide)
+      inflater:
+        inflation_radius: 0.30 # 0.7
+        cost_scaling_factor: 100.0 # 25.0 default 10, increasing factor decrease the cost value
+      update_frequency: 10.0 # default 5 (http://wiki.ros.org/navigation/Tutorials/Navigation%20Tuning%20Guide)
     </rosparam>
     <rosparam ns="move_base">
-base_local_planner: base_local_planner/TrajectoryPlannerROS
-TrajectoryPlannerROS:
-  escape_vel: -0.1 # -0.1
-recovery_behavior_enabled: true
-recovery_behaviors:
-  - name: "conservative_reset"
-    type: "clear_costmap_recovery/ClearCostmapRecovery"
-  - name: "rotate_recovery"
-    type: "rotate_recovery/RotateRecovery"
-    frequency: 20.0
-    sim_granularity: 0.017
-  - name: "aggressive_reset"
-    type: "clear_costmap_recovery/ClearCostmapRecovery"
-conservative_reset: {reset_distance: 1.0} # 3.0
-aggressive_reset: {reset_distance: 0.2} # 0.5
-move_slow_and_clear: {clearing_distance: 0.5, limited_distance: 0.3, limited_rot_speed: 0.45, limited_trans_speed: 0.25}
+      base_local_planner: base_local_planner/TrajectoryPlannerROS
+      TrajectoryPlannerROS:
+        escape_vel: -0.1 # -0.1
+      recovery_behavior_enabled: true
+      recovery_behaviors:
+        - name: "conservative_reset"
+          type: "clear_costmap_recovery/ClearCostmapRecovery"
+        - name: "rotate_recovery"
+          type: "rotate_recovery/RotateRecovery"
+          frequency: 20.0
+          sim_granularity: 0.017
+        - name: "aggressive_reset"
+          type: "clear_costmap_recovery/ClearCostmapRecovery"
+      conservative_reset:
+        - reset_distance: 1.0 # 3.0
+      aggressive_reset:
+        - reset_distance: 0.2 # 0.5
+      move_slow_and_clear:
+        - clearing_distance: 0.5
+        - limited_distance: 0.3
+        - limited_rot_speed: 0.45
+        - limited_trans_speed: 0.25
     </rosparam>
   </group>
 

From 4a1c5580a9d1ebb82d567e859956f3cca0981feb Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Thu, 17 Oct 2019 02:32:41 +0900
Subject: [PATCH 03/28] add odom_corrector.py

---
 .../launch/fetch_bringup.launch               |  6 ++++
 .../scripts/odom_corrector.py                 | 30 +++++++++++++++++++
 2 files changed, 36 insertions(+)
 create mode 100755 jsk_fetch_robot/jsk_fetch_startup/scripts/odom_corrector.py

diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
index 2f8334a0ad..e3179cf015 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
+++ b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
@@ -185,4 +185,10 @@
     <remap from="~input" to="/imu" />
     <remap from="~output" to="/imu_corrected" />
   </node>
+
+  <!-- /odom has no covariance value. -->
+  <node name="odom_corrector" pkg="jsk_fetch_startup" type="odom_corrector.py">
+    <remap from="~input" to="/odom" />
+    <remap from="~output" to="/odom_corrected" />
+  </node>
 </launch>
diff --git a/jsk_fetch_robot/jsk_fetch_startup/scripts/odom_corrector.py b/jsk_fetch_robot/jsk_fetch_startup/scripts/odom_corrector.py
new file mode 100755
index 0000000000..ac07185d08
--- /dev/null
+++ b/jsk_fetch_robot/jsk_fetch_startup/scripts/odom_corrector.py
@@ -0,0 +1,30 @@
+#!/usr/bin/env python
+
+import rospy
+
+from nav_msgs.msg import Odometry
+
+
+class OdomCorrector(object):
+    def __init__(self):
+        super(OdomCorrector, self).__init__()
+        self.sub = rospy.Subscriber(
+            '~input', Odometry, self._cb, queue_size=1)
+        self.pub = rospy.Publisher(
+            '~output', Odometry, queue_size=1)
+        self.covariance = [1e-3, 0, 0, 0, 0, 0,
+                           0, 1e-3, 0, 0, 0, 0,
+                           0, 0, 1e-6, 0, 0, 0,
+                           0, 0, 0, 1e-6, 0, 0,
+                           0, 0, 0, 0, 1e-6, 0,
+                           0, 0, 0, 0, 0, 1e-3]
+
+    def _cb(self, msg):
+        msg.pose.covariance = self.covariance
+        self.pub.publish(msg)
+
+
+if __name__ == '__main__':
+    rospy.init_node('odom_corrector')
+    app = OdomCorrector()
+    rospy.spin()

From 9a0edd1614c2d401db99897a733be3e6ad901a0e Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Thu, 17 Oct 2019 02:38:14 +0900
Subject: [PATCH 04/28] add angular_velocity_covariance in imu_corrector.py

---
 jsk_fetch_robot/jsk_fetch_startup/scripts/imu_corrector.py | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/jsk_fetch_robot/jsk_fetch_startup/scripts/imu_corrector.py b/jsk_fetch_robot/jsk_fetch_startup/scripts/imu_corrector.py
index ca8c3d5c66..1039feba8f 100755
--- a/jsk_fetch_robot/jsk_fetch_startup/scripts/imu_corrector.py
+++ b/jsk_fetch_robot/jsk_fetch_startup/scripts/imu_corrector.py
@@ -15,6 +15,9 @@ def __init__(self):
 
     def _cb(self, msg):
         msg.header.frame_id = 'base_link'
+        msg.angular_velocity_covariance = [0, 0, 0,
+                                           0, 0, 0,
+                                           0, 0, 0.000004]
         self.pub.publish(msg)
 
 

From e7227f91b1143fb0a49d49bc6f09c82fc4be519d Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Thu, 17 Oct 2019 03:43:35 +0900
Subject: [PATCH 05/28] use robot_localization ukf

---
 .../launch/fetch_bringup.launch               | 33 +++++++++++++++++++
 .../jsk_fetch_startup/launch/fetch_teleop.xml |  6 ++++
 2 files changed, 39 insertions(+)

diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
index e3179cf015..6110dd85be 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
+++ b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
@@ -137,9 +137,41 @@
       <arg name="map_keepout_file" value="$(arg keepout_map_file)" />
       <arg name="use_keepout" value="true" />
     </include>
+
+    <!-- robot localization ukf -->
+    <node pkg="robot_localization" type="ukf_localization_node" name="ukf_se" clear_params="true">
+      <rosparam>
+        frequency: 50
+        sensor_timeout: 1.0
+        two_d_mode: true
+        publish_tf: true
+        publish_acceleration: false
+        map_frame: map
+        odom_frame: odom_combined
+        base_link_frame: base_link
+        odom0: /odom_corrected
+        odom0_config: [true, true, false,
+                       false, false, true,
+                       true, true, false,
+                       false, false, true,
+                       false, false, false]
+        odom0_nodelay: true
+        odom0_differential: true
+        imu0: /imu_corrected
+        imu0_config: [true, true, false,
+                      false, false,  true,
+                      true, true, false,
+                      false, false,  true,
+                      false, false, false]
+        imu0_nodelay: true
+        imu0_differential: true
+      </rosparam>
+    </node>
+
     <rosparam ns="amcl">
       update_min_a: 0.01 <!-- update filter every 0.01[m] translation -->
       update_min_d: 0.01 <!-- update filter every 0.01[rad] rotation -->
+      odom_frame_id: odom_combined
     </rosparam>
 
     <rosparam ns="move_base/global_costmap">
@@ -148,6 +180,7 @@
         cost_scaling_factor: 10.0 # 10.0
     </rosparam>
     <rosparam ns="move_base/local_costmap">
+      global_frame: odom_combined
       inflater:
         inflation_radius: 0.30 # 0.7
         cost_scaling_factor: 100.0 # 25.0 default 10, increasing factor decrease the cost value
diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_teleop.xml b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_teleop.xml
index 2f82cba059..af4906837b 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_teleop.xml
+++ b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_teleop.xml
@@ -76,6 +76,12 @@
     <rosparam file="$(find fetch_navigation)/config/fetch/costmap_common.yaml" command="load" ns="local_costmap" />
     <rosparam file="$(find fetch_navigation)/config/costmap_local.yaml" command="load" ns="local_costmap" />
     <rosparam file="$(find fetch_navigation)/config/fetch/costmap_local.yaml" command="load" ns="local_costmap" />
+    <rosparam>
+      local_costmap:
+        global_frame: odom_combined
+        # default 5 (http://wiki.ros.org/navigation/Tutorials/Navigation%20Tuning%20Guide)
+        update_frequency: 10.0
+    </rosparam>
   </node>
 
   <node name="safe_tilt_head" pkg="jsk_fetch_startup" type="safe_tilt_head.py" />

From 64c84269faacc942dce1f48f957a69c0aa110e85 Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Thu, 17 Oct 2019 03:43:58 +0900
Subject: [PATCH 06/28] refactor local_costmap parameters

---
 .../jsk_fetch_startup/launch/fetch_bringup.launch          | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
index 6110dd85be..8f3e359af1 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
+++ b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
@@ -182,9 +182,10 @@
     <rosparam ns="move_base/local_costmap">
       global_frame: odom_combined
       inflater:
-        inflation_radius: 0.30 # 0.7
-        cost_scaling_factor: 100.0 # 25.0 default 10, increasing factor decrease the cost value
-      update_frequency: 10.0 # default 5 (http://wiki.ros.org/navigation/Tutorials/Navigation%20Tuning%20Guide)
+        inflation_radius: 3.0 # 0.7
+        cost_scaling_factor: 3.0 # 25.0 default 10, increasing factor decrease the cost value
+      # default 5 (http://wiki.ros.org/navigation/Tutorials/Navigation%20Tuning%20Guide)
+      update_frequency: 10.0
     </rosparam>
     <rosparam ns="move_base">
       base_local_planner: base_local_planner/TrajectoryPlannerROS

From 34e2e070d07028a643062323568e52a3765801b8 Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Thu, 17 Oct 2019 04:02:12 +0900
Subject: [PATCH 07/28] use odom tf frame

---
 jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch | 4 +---
 jsk_fetch_robot/jsk_fetch_startup/launch/fetch_teleop.xml     | 1 -
 2 files changed, 1 insertion(+), 4 deletions(-)

diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
index 8f3e359af1..4be04f266e 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
+++ b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
@@ -147,7 +147,7 @@
         publish_tf: true
         publish_acceleration: false
         map_frame: map
-        odom_frame: odom_combined
+        odom_frame: odom
         base_link_frame: base_link
         odom0: /odom_corrected
         odom0_config: [true, true, false,
@@ -171,7 +171,6 @@
     <rosparam ns="amcl">
       update_min_a: 0.01 <!-- update filter every 0.01[m] translation -->
       update_min_d: 0.01 <!-- update filter every 0.01[rad] rotation -->
-      odom_frame_id: odom_combined
     </rosparam>
 
     <rosparam ns="move_base/global_costmap">
@@ -180,7 +179,6 @@
         cost_scaling_factor: 10.0 # 10.0
     </rosparam>
     <rosparam ns="move_base/local_costmap">
-      global_frame: odom_combined
       inflater:
         inflation_radius: 3.0 # 0.7
         cost_scaling_factor: 3.0 # 25.0 default 10, increasing factor decrease the cost value
diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_teleop.xml b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_teleop.xml
index af4906837b..38569b60fb 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_teleop.xml
+++ b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_teleop.xml
@@ -78,7 +78,6 @@
     <rosparam file="$(find fetch_navigation)/config/fetch/costmap_local.yaml" command="load" ns="local_costmap" />
     <rosparam>
       local_costmap:
-        global_frame: odom_combined
         # default 5 (http://wiki.ros.org/navigation/Tutorials/Navigation%20Tuning%20Guide)
         update_frequency: 10.0
     </rosparam>

From bb51bcef838c49a29c955c24009815bf75b51c46 Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Thu, 17 Oct 2019 04:13:49 +0900
Subject: [PATCH 08/28] remap ukf output to /odom_combined

---
 jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch | 1 +
 1 file changed, 1 insertion(+)

diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
index 4be04f266e..1eb2fef9db 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
+++ b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
@@ -140,6 +140,7 @@
 
     <!-- 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>
         frequency: 50
         sensor_timeout: 1.0

From 5474f1d7d18218cc4e3d19c52072e89bc5789063 Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Thu, 17 Oct 2019 17:14:03 +0900
Subject: [PATCH 09/28] fix typo in odom_corrector.py

---
 jsk_fetch_robot/jsk_fetch_startup/scripts/odom_corrector.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/jsk_fetch_robot/jsk_fetch_startup/scripts/odom_corrector.py b/jsk_fetch_robot/jsk_fetch_startup/scripts/odom_corrector.py
index ac07185d08..ff0c61be5c 100755
--- a/jsk_fetch_robot/jsk_fetch_startup/scripts/odom_corrector.py
+++ b/jsk_fetch_robot/jsk_fetch_startup/scripts/odom_corrector.py
@@ -20,7 +20,7 @@ def __init__(self):
                            0, 0, 0, 0, 0, 1e-3]
 
     def _cb(self, msg):
-        msg.pose.covariance = self.covariance
+        msg.twist.covariance = self.covariance
         self.pub.publish(msg)
 
 

From 5b7199b955991a22040fa1db14de900bfcc0c6ef Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Thu, 17 Oct 2019 17:40:13 +0900
Subject: [PATCH 10/28] refactor imu_corrector.py

---
 jsk_fetch_robot/jsk_fetch_startup/scripts/imu_corrector.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/jsk_fetch_robot/jsk_fetch_startup/scripts/imu_corrector.py b/jsk_fetch_robot/jsk_fetch_startup/scripts/imu_corrector.py
index 1039feba8f..d6b22d565f 100755
--- a/jsk_fetch_robot/jsk_fetch_startup/scripts/imu_corrector.py
+++ b/jsk_fetch_robot/jsk_fetch_startup/scripts/imu_corrector.py
@@ -17,7 +17,7 @@ def _cb(self, msg):
         msg.header.frame_id = 'base_link'
         msg.angular_velocity_covariance = [0, 0, 0,
                                            0, 0, 0,
-                                           0, 0, 0.000004]
+                                           0, 0, 4e-6]
         self.pub.publish(msg)
 
 

From ed0362c7524d7483dee267cbdecadb81e819163b Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Thu, 17 Oct 2019 17:51:12 +0900
Subject: [PATCH 11/28] add covariance for odom.pose

---
 .../jsk_fetch_startup/scripts/odom_corrector.py            | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/jsk_fetch_robot/jsk_fetch_startup/scripts/odom_corrector.py b/jsk_fetch_robot/jsk_fetch_startup/scripts/odom_corrector.py
index ff0c61be5c..d6f37c8f38 100755
--- a/jsk_fetch_robot/jsk_fetch_startup/scripts/odom_corrector.py
+++ b/jsk_fetch_robot/jsk_fetch_startup/scripts/odom_corrector.py
@@ -14,12 +14,13 @@ def __init__(self):
             '~output', Odometry, queue_size=1)
         self.covariance = [1e-3, 0, 0, 0, 0, 0,
                            0, 1e-3, 0, 0, 0, 0,
-                           0, 0, 1e-6, 0, 0, 0,
-                           0, 0, 0, 1e-6, 0, 0,
-                           0, 0, 0, 0, 1e-6, 0,
+                           0, 0, 0, 0, 0, 0,
+                           0, 0, 0, 0, 0, 0,
+                           0, 0, 0, 0, 0, 0,
                            0, 0, 0, 0, 0, 1e-3]
 
     def _cb(self, msg):
+        msg.pose.covariance = self.covariance
         msg.twist.covariance = self.covariance
         self.pub.publish(msg)
 

From ab7b0bfab7e6c826320daec3ad8f70bbec688839 Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Thu, 17 Oct 2019 17:51:50 +0900
Subject: [PATCH 12/28] fix imu robot localization config

---
 .../jsk_fetch_startup/launch/fetch_bringup.launch      | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
index 1eb2fef9db..01a0e4808d 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
+++ b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
@@ -159,11 +159,11 @@
         odom0_nodelay: true
         odom0_differential: true
         imu0: /imu_corrected
-        imu0_config: [true, true, false,
-                      false, false,  true,
-                      true, true, false,
-                      false, false,  true,
-                      false, false, false]
+        imu0_config: [false, false, false,
+                      false, false, false,
+                      false, false, false,
+                      false, false, true,
+                      true, true, false]
         imu0_nodelay: true
         imu0_differential: true
       </rosparam>

From 6300a9c79251d5de2c065d2caa30c559339461db Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Thu, 17 Oct 2019 19:10:36 +0900
Subject: [PATCH 13/28] fix imu robot localization config

---
 jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch | 1 +
 1 file changed, 1 insertion(+)

diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
index 01a0e4808d..36961aa433 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
+++ b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
@@ -166,6 +166,7 @@
                       true, true, false]
         imu0_nodelay: true
         imu0_differential: true
+        imu0_remove_gravitational_acceleration: true
       </rosparam>
     </node>
 

From 1838deb3252c2e5f0c93691300ea2c84fdd1d560 Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Thu, 17 Oct 2019 20:30:35 +0900
Subject: [PATCH 14/28] update correct_position covariance

---
 .../jsk_fetch_startup/scripts/correct_position.py          | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/jsk_fetch_robot/jsk_fetch_startup/scripts/correct_position.py b/jsk_fetch_robot/jsk_fetch_startup/scripts/correct_position.py
index 22057b7e40..918a32780b 100755
--- a/jsk_fetch_robot/jsk_fetch_startup/scripts/correct_position.py
+++ b/jsk_fetch_robot/jsk_fetch_startup/scripts/correct_position.py
@@ -37,7 +37,12 @@ def _cb_correct_position(self, msg):
             self.pos.header.stamp = rospy.Time.now()
             self.pos.header.frame_id = 'map'
             self.pos.pose.pose = self.dock_pose
-            self.pos.pose.covariance = [0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942]
+            self.pos.pose.covariance = [1e-3, 0.0, 0.0, 0.0, 0.0, 0.0,
+                                        0.0, 1e-3, 0.0, 0.0, 0.0, 0.0,
+                                        0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+                                        0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+                                        0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+                                        0.0, 0.0, 0.0, 0.0, 0.0, 1e-3]
             self.pub.publish(self.pos)
         self.is_docking = msg.is_charging
 

From e5e83a3dbc0b602d262aa751d3ea22f1059fb4ca Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Thu, 17 Oct 2019 22:12:36 +0900
Subject: [PATCH 15/28] set odom_alpha for amcl parameters

---
 jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
index 36961aa433..118180696d 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
+++ b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
@@ -173,6 +173,10 @@
     <rosparam ns="amcl">
       update_min_a: 0.01 <!-- update filter every 0.01[m] translation -->
       update_min_d: 0.01 <!-- update filter every 0.01[rad] rotation -->
+      odom_alpha1: 0.5  # rotation noise per rotation
+      odom_alpha2: 0.5  # rotation noise per translation
+      odom_alpha3: 0.5  # translation noise per translation
+      odom_alpha4: 0.5  # translation noise per rotation
     </rosparam>
 
     <rosparam ns="move_base/global_costmap">

From 6cc9a318cdf95a2f9e09ea0715ae9fda94958c26 Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Sat, 19 Oct 2019 01:23:07 +0900
Subject: [PATCH 16/28] publish initialpose in 1hz

---
 .../jsk_fetch_startup/scripts/correct_position.py  | 14 ++++++++------
 1 file changed, 8 insertions(+), 6 deletions(-)

diff --git a/jsk_fetch_robot/jsk_fetch_startup/scripts/correct_position.py b/jsk_fetch_robot/jsk_fetch_startup/scripts/correct_position.py
index 918a32780b..43d0a01e05 100755
--- a/jsk_fetch_robot/jsk_fetch_startup/scripts/correct_position.py
+++ b/jsk_fetch_robot/jsk_fetch_startup/scripts/correct_position.py
@@ -22,17 +22,20 @@ def __init__(self):
                 self.dock_pose = spot.pose
 
         self.is_docking = False
+        self.timer = rospy.Timer(rospy.Duration(1.0), self._cb_correct_position)
 
     def subscribe(self):
-        self.sub_dock = rospy.Subscriber('/battery_state',
-                                         BatteryState,
-                                         self._cb_correct_position)
+        self.sub_dock = rospy.Subscriber(
+            '/battery_state', BatteryState, self._cb)
 
     def unsubscribe(self):
         self.sub_dock.unregister()
 
-    def _cb_correct_position(self, msg):
-        if msg.is_charging and (not self.is_docking):
+    def _cb(self, msg):
+        self.is_docking = msg.is_charging
+
+    def _cb_correct_position(self, event):
+        if self.is_docking:
             self.pos = PoseWithCovarianceStamped()
             self.pos.header.stamp = rospy.Time.now()
             self.pos.header.frame_id = 'map'
@@ -44,7 +47,6 @@ def _cb_correct_position(self, msg):
                                         0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                         0.0, 0.0, 0.0, 0.0, 0.0, 1e-3]
             self.pub.publish(self.pos)
-        self.is_docking = msg.is_charging
 
 
 if __name__ == '__main__':

From 5bcab424bd36ae9251d6d57109def54099e65b3d Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Sun, 20 Oct 2019 02:52:35 +0900
Subject: [PATCH 17/28] update costmap parameters

---
 .../jsk_fetch_startup/launch/fetch_bringup.launch   |  6 ++++--
 .../jsk_fetch_startup/launch/fetch_teleop.xml       | 13 ++++++++-----
 2 files changed, 12 insertions(+), 7 deletions(-)

diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
index 118180696d..22bfb6a41a 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
+++ b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
@@ -181,8 +181,9 @@
 
     <rosparam ns="move_base/global_costmap">
       inflater:
-        inflation_radius: 0.30 # 0.7
-        cost_scaling_factor: 10.0 # 10.0
+        inflation_radius: 3.0 # 0.7
+        cost_scaling_factor: 3.0 # 10.0
+      footprint_padding: 0.05
     </rosparam>
     <rosparam ns="move_base/local_costmap">
       inflater:
@@ -190,6 +191,7 @@
         cost_scaling_factor: 3.0 # 25.0 default 10, increasing factor decrease the cost value
       # default 5 (http://wiki.ros.org/navigation/Tutorials/Navigation%20Tuning%20Guide)
       update_frequency: 10.0
+      footprint_padding: 0.05
     </rosparam>
     <rosparam ns="move_base">
       base_local_planner: base_local_planner/TrajectoryPlannerROS
diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_teleop.xml b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_teleop.xml
index 38569b60fb..5ec5b938cb 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_teleop.xml
+++ b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_teleop.xml
@@ -76,12 +76,15 @@
     <rosparam file="$(find fetch_navigation)/config/fetch/costmap_common.yaml" command="load" ns="local_costmap" />
     <rosparam file="$(find fetch_navigation)/config/costmap_local.yaml" command="load" ns="local_costmap" />
     <rosparam file="$(find fetch_navigation)/config/fetch/costmap_local.yaml" command="load" ns="local_costmap" />
-    <rosparam>
-      local_costmap:
-        # default 5 (http://wiki.ros.org/navigation/Tutorials/Navigation%20Tuning%20Guide)
-        update_frequency: 10.0
-    </rosparam>
   </node>
+  <rosparam ns="safe_teleop_base/local_costmap">
+    inflater:
+      inflation_radius: 3.0 # 0.7
+      cost_scaling_factor: 3.0 # 25.0 default 10, increasing factor decrease the cost value
+    # default 5 (http://wiki.ros.org/navigation/Tutorials/Navigation%20Tuning%20Guide)
+    update_frequency: 10.0
+    footprint_padding: 0.05
+  </rosparam>
 
   <node name="safe_tilt_head" pkg="jsk_fetch_startup" type="safe_tilt_head.py" />
 

From c9256cb12ff10a3aeca06aa64b429fcc3d2d03d8 Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Sun, 20 Oct 2019 02:52:57 +0900
Subject: [PATCH 18/28] enable dwa in local planner

---
 jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
index 22bfb6a41a..46679e2b28 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
+++ b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
@@ -216,6 +216,8 @@
         - limited_distance: 0.3
         - limited_rot_speed: 0.45
         - limited_trans_speed: 0.25
+      TrajectoryPlannerROS:
+        dwa: true
     </rosparam>
   </group>
 

From c2acfe4d8d44bce652563ff18131695384129c66 Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Mon, 21 Oct 2019 23:26:16 +0900
Subject: [PATCH 19/28] add min_object_height for object layer

---
 jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch | 4 ++++
 jsk_fetch_robot/jsk_fetch_startup/launch/fetch_teleop.xml     | 2 ++
 2 files changed, 6 insertions(+)

diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
index 46679e2b28..87dd61596b 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
+++ b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
@@ -183,12 +183,16 @@
       inflater:
         inflation_radius: 3.0 # 0.7
         cost_scaling_factor: 3.0 # 10.0
+      obstacles:
+        min_obstacle_height: 0.05
       footprint_padding: 0.05
     </rosparam>
     <rosparam ns="move_base/local_costmap">
       inflater:
         inflation_radius: 3.0 # 0.7
         cost_scaling_factor: 3.0 # 25.0 default 10, increasing factor decrease the cost value
+      obstacles:
+        min_obstacle_height: 0.05
       # default 5 (http://wiki.ros.org/navigation/Tutorials/Navigation%20Tuning%20Guide)
       update_frequency: 10.0
       footprint_padding: 0.05
diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_teleop.xml b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_teleop.xml
index 5ec5b938cb..5e8eaff22a 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_teleop.xml
+++ b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_teleop.xml
@@ -81,6 +81,8 @@
     inflater:
       inflation_radius: 3.0 # 0.7
       cost_scaling_factor: 3.0 # 25.0 default 10, increasing factor decrease the cost value
+    obstacles:
+      min_obstacle_height: 0.05
     # default 5 (http://wiki.ros.org/navigation/Tutorials/Navigation%20Tuning%20Guide)
     update_frequency: 10.0
     footprint_padding: 0.05

From 014e62e358df6edc93e3e55f18dd73df479ffa88 Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Tue, 22 Oct 2019 02:11:01 +0900
Subject: [PATCH 20/28] set lower inflation radius

---
 jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch | 4 ++--
 jsk_fetch_robot/jsk_fetch_startup/launch/fetch_teleop.xml     | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
index 87dd61596b..b2006c5fb8 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
+++ b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
@@ -181,7 +181,7 @@
 
     <rosparam ns="move_base/global_costmap">
       inflater:
-        inflation_radius: 3.0 # 0.7
+        inflation_radius: 1.0 # 0.7
         cost_scaling_factor: 3.0 # 10.0
       obstacles:
         min_obstacle_height: 0.05
@@ -189,7 +189,7 @@
     </rosparam>
     <rosparam ns="move_base/local_costmap">
       inflater:
-        inflation_radius: 3.0 # 0.7
+        inflation_radius: 1.0 # 0.7
         cost_scaling_factor: 3.0 # 25.0 default 10, increasing factor decrease the cost value
       obstacles:
         min_obstacle_height: 0.05
diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_teleop.xml b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_teleop.xml
index 5e8eaff22a..dbe9a8382e 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_teleop.xml
+++ b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_teleop.xml
@@ -79,7 +79,7 @@
   </node>
   <rosparam ns="safe_teleop_base/local_costmap">
     inflater:
-      inflation_radius: 3.0 # 0.7
+      inflation_radius: 1.0 # 0.7
       cost_scaling_factor: 3.0 # 25.0 default 10, increasing factor decrease the cost value
     obstacles:
       min_obstacle_height: 0.05

From 21457caf3d21250207631755ae4cd794e29ddef8 Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Tue, 22 Oct 2019 02:12:43 +0900
Subject: [PATCH 21/28] fix typo in fetch_bringup.launch

---
 .../jsk_fetch_startup/launch/fetch_bringup.launch  | 14 ++++++--------
 1 file changed, 6 insertions(+), 8 deletions(-)

diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
index b2006c5fb8..058e59fe21 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
+++ b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
@@ -212,16 +212,14 @@
         - name: "aggressive_reset"
           type: "clear_costmap_recovery/ClearCostmapRecovery"
       conservative_reset:
-        - reset_distance: 1.0 # 3.0
+        reset_distance: 1.0 # 3.0
       aggressive_reset:
-        - reset_distance: 0.2 # 0.5
+        reset_distance: 0.2 # 0.5
       move_slow_and_clear:
-        - clearing_distance: 0.5
-        - limited_distance: 0.3
-        - limited_rot_speed: 0.45
-        - limited_trans_speed: 0.25
-      TrajectoryPlannerROS:
-        dwa: true
+        clearing_distance: 0.5
+        limited_distance: 0.3
+        limited_rot_speed: 0.45
+        limited_trans_speed: 0.25
     </rosparam>
   </group>
 

From 9eca0c6058db4c98ab3ac7d35202413fec1240bc Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Tue, 22 Oct 2019 02:13:24 +0900
Subject: [PATCH 22/28] set correct parameters for base_local_planner

---
 .../jsk_fetch_startup/launch/fetch_bringup.launch           | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
index 058e59fe21..54352e4ad8 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
+++ b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
@@ -200,7 +200,13 @@
     <rosparam ns="move_base">
       base_local_planner: base_local_planner/TrajectoryPlannerROS
       TrajectoryPlannerROS:
+        min_in_place_vel_theta: 0.8
         escape_vel: -0.1 # -0.1
+        meter_scoring: true
+        pdist_scale: 5.0
+        gdist_scale: 3.2
+        occdist_scale: 0.1
+        dwa: true
       recovery_behavior_enabled: true
       recovery_behaviors:
         - name: "conservative_reset"

From ddb737791e1501c586a26962b8bbae8313f90dee Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Wed, 23 Oct 2019 23:25:39 +0900
Subject: [PATCH 23/28] update odom_alpha parameters

---
 .../jsk_fetch_startup/launch/fetch_bringup.launch         | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
index 54352e4ad8..00a0cde786 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
+++ b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
@@ -173,10 +173,10 @@
     <rosparam ns="amcl">
       update_min_a: 0.01 <!-- update filter every 0.01[m] translation -->
       update_min_d: 0.01 <!-- update filter every 0.01[rad] rotation -->
-      odom_alpha1: 0.5  # rotation noise per rotation
-      odom_alpha2: 0.5  # rotation noise per translation
-      odom_alpha3: 0.5  # translation noise per translation
-      odom_alpha4: 0.5  # translation noise per rotation
+      odom_alpha1: 0.05  # rotation noise per rotation
+      odom_alpha2: 0.05  # rotation noise per translation
+      odom_alpha3: 0.05  # translation noise per translation
+      odom_alpha4: 0.05  # translation noise per rotation
     </rosparam>
 
     <rosparam ns="move_base/global_costmap">

From 9979c64149b17fb9d9c6a5403efb612a2b3ddf68 Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Wed, 23 Oct 2019 23:32:03 +0900
Subject: [PATCH 24/28] set proper controller_frequency

---
 jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
index 00a0cde786..9d420dc977 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
+++ b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
@@ -179,6 +179,10 @@
       odom_alpha4: 0.05  # translation noise per rotation
     </rosparam>
 
+    <rosparam ns="move_base">
+      controller_frequency: 10.0
+    </rosparam>
+
     <rosparam ns="move_base/global_costmap">
       inflater:
         inflation_radius: 1.0 # 0.7

From 1fdbfbb2e59fca84ff8c391c5f20bec25a7d1d24 Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Thu, 24 Oct 2019 04:49:38 +0900
Subject: [PATCH 25/28] set higher vx_samples for faster move_base

---
 jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch | 1 +
 1 file changed, 1 insertion(+)

diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
index 9d420dc977..6fec9f8f3d 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
+++ b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
@@ -206,6 +206,7 @@
       TrajectoryPlannerROS:
         min_in_place_vel_theta: 0.8
         escape_vel: -0.1 # -0.1
+        vx_samples: 10
         meter_scoring: true
         pdist_scale: 5.0
         gdist_scale: 3.2

From 3392520625a25485d365cdfb0ed788ddd217bdb7 Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Thu, 24 Oct 2019 07:30:46 +0900
Subject: [PATCH 26/28] update navigation parameters

---
 .../jsk_fetch_startup/launch/fetch_bringup.launch      | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
index 6fec9f8f3d..7ee2c9478c 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
+++ b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
@@ -185,16 +185,16 @@
 
     <rosparam ns="move_base/global_costmap">
       inflater:
-        inflation_radius: 1.0 # 0.7
-        cost_scaling_factor: 3.0 # 10.0
+        inflation_radius: 0.7 # 0.7
+        cost_scaling_factor: 5.0 # 10.0
       obstacles:
         min_obstacle_height: 0.05
       footprint_padding: 0.05
     </rosparam>
     <rosparam ns="move_base/local_costmap">
       inflater:
-        inflation_radius: 1.0 # 0.7
-        cost_scaling_factor: 3.0 # 25.0 default 10, increasing factor decrease the cost value
+        inflation_radius: 0.7 # 0.7
+        cost_scaling_factor: 5.0 # 25.0 default 10, increasing factor decrease the cost value
       obstacles:
         min_obstacle_height: 0.05
       # default 5 (http://wiki.ros.org/navigation/Tutorials/Navigation%20Tuning%20Guide)
@@ -204,7 +204,7 @@
     <rosparam ns="move_base">
       base_local_planner: base_local_planner/TrajectoryPlannerROS
       TrajectoryPlannerROS:
-        min_in_place_vel_theta: 0.8
+        min_in_place_vel_theta: 1.0
         escape_vel: -0.1 # -0.1
         vx_samples: 10
         meter_scoring: true

From 411a5feafe9f28aa5984af970c4853eff952f15f Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Fri, 25 Oct 2019 23:01:36 +0900
Subject: [PATCH 27/28] update amcl parameters

---
 .../jsk_fetch_startup/launch/fetch_bringup.launch         | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
index 7ee2c9478c..30e29fd74f 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
+++ b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
@@ -173,10 +173,10 @@
     <rosparam ns="amcl">
       update_min_a: 0.01 <!-- update filter every 0.01[m] translation -->
       update_min_d: 0.01 <!-- update filter every 0.01[rad] rotation -->
-      odom_alpha1: 0.05  # rotation noise per rotation
-      odom_alpha2: 0.05  # rotation noise per translation
-      odom_alpha3: 0.05  # translation noise per translation
-      odom_alpha4: 0.05  # translation noise per rotation
+      odom_alpha1: 0.2  # rotation noise per rotation
+      odom_alpha2: 0.2  # rotation noise per translation
+      odom_alpha3: 0.2  # translation noise per translation
+      odom_alpha4: 0.2  # translation noise per rotation
     </rosparam>
 
     <rosparam ns="move_base">

From 8d279ca709d599c5816b3cd3e2910356c7603af9 Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Sat, 26 Oct 2019 23:29:29 +0900
Subject: [PATCH 28/28] add robot_localization in jsk_fetch_startup/package.xml

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

diff --git a/jsk_fetch_robot/jsk_fetch_startup/package.xml b/jsk_fetch_robot/jsk_fetch_startup/package.xml
index 2c5481f28f..ee411f62e7 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/package.xml
+++ b/jsk_fetch_robot/jsk_fetch_startup/package.xml
@@ -28,6 +28,7 @@
   <run_depend>amcl</run_depend>
   <run_depend>map_server</run_depend>
   <run_depend>move_base</run_depend>
+  <run_depend>robot_localization</run_depend>
 
   <!-- for fetch teleop -->
   <run_depend>app_manager</run_depend>