Merge pull request #124 from 708yamaguchi/yamaguchi-fast-go-to-kitchen

move_baseのカクつきをなおす

jsk_fetch_robot/jsk_fetch.rosinstall.melodic

@@ -110,7 +110,8 @@
     local-name: fetchrobotics/fetch_open_auto_dock
     uri: https://github.com/fetchrobotics/fetch_open_auto_dock.git
     version: melodic-devel
+# Use https://github.com/ros-planning/navigation/pull/839 based on 7f22997e6804d9d7249b8a1d789bf27343b26f75
 - git:
-    local-name: ros-planning/navigation
-    uri: https://github.com/ros-planning/navigation.git
-    version: 7f22997e6804d9d7249b8a1d789bf27343b26f75
+    local-name: 708yamaguchi/navigation
+    uri: https://github.com/708yamaguchi/navigation.git
+    version: fetch15

jsk_fetch_robot/jsk_fetch_startup/launch/navigation/fetch_move_base_common_params.yaml

@@ -2,23 +2,30 @@ move_base:
   controller_frequency: 20.0
   global_costmap:
     inflater:
-      inflation_radius: 1.0 # 0.7
+      inflation_radius: 0.7 # 0.7
       cost_scaling_factor: 10.0 # 10.0
     obstacles:
       min_obstacle_height: 0.05
     footprint_padding: 0.05
-    publish_frequency: 1.0
+    # By reducing update/publish_frequency, move_base control loop rate will be improved
+    # https://moriokalab.com/news/84
+    update_frequency: 1.0
+    publish_frequency: 0.5
   local_costmap:
     inflater:
-      inflation_radius: 1.0 # 0.7
+      inflation_radius: 0.35 # 0.7
       cost_scaling_factor: 10.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)
     footprint_padding: 0.05
-    update_frequency: 5.0
+    # By reducing update/publish_frequency, move_base control loop rate will be improved
+    # https://moriokalab.com/news/84
+    update_frequency: 1.0
+    publish_frequency: 0.5
   base_global_planner: global_planner/GlobalPlanner
   base_local_planner: teb_local_planner/TebLocalPlannerROS
+  # base_local_planner: base_local_planner/TrajectoryPlannerROS
   GlobalPlanner:
     allow_unknown: true
     neural_cost: 50
@@ -28,13 +35,32 @@ move_base:
     orientation_mode: 1
     visualize_potential: false
     publish_potential: false
+  TrajectoryPlannerROS:
+    yaw_goal_tolerance: 0.1
+    xy_goal_tolerance: 0.17
+    max_vel_x: 0.5
+    min_vel_x: 0.1
+    min_in_place_vel_theta: 0.5
+    escape_vel: 0.0
+    sim_time: 1.5
+    sim_granularity: 0.025
+    angular_sim_granularity: 0.025
+    vx_samples: 10
+    vth_samples: 40
+    meter_scoring: true
+    pdist_scale: 4.0
+    gdist_scale: 2.5
+    occdist_scale: 0.00625
+    dwa: true
   TebLocalPlannerROS:
     odom_topic: /odom_combined
     map_frame: /odom
     # Trajectory
     teb_autosize: True
     dt_ref: 0.3
     dt_hysteresis: 0.1
+    # By reducing max_samples, move_base control loop rate will be improved
+    max_samples: 50 # default 500
     global_plan_overwrite_orientation: True
     max_global_plan_lookahead_dist: 3.0
     feasibility_check_no_poses: 5
@@ -53,7 +79,7 @@ move_base:
     yaw_goal_tolerance: 0.1
     free_goal_vel: False
     # Obstacles
-    min_obstacle_dist: 0.1
+    min_obstacle_dist: 0.4 # Enough value not to go too close to obstacle
     include_costmap_obstacles: True
     costmap_obstacles_behind_robot_dist: 1.0
     obstacle_poses_afected: 30