From ebd2377b70652932c27930367c8cb239905da106 Mon Sep 17 00:00:00 2001
From: Zach Ghera <zpghera00@gmail.com>
Date: Mon, 5 Apr 2021 23:25:11 -0400
Subject: [PATCH] Tune move_base.launch params for successful sim run.

The param files associated with move_base.launch were modified to (mostly) match teb_local_planner_tutorials' param files as the simulated kart needed to 'fit' on the simulation track we took from teb_local_planner_tutorials. Create Issue #19 for determine a way to modify the params to meet our karts physical params while also maintaining our ability to test in simulation.
---
 .../launch/sim_autocross_track.launch         | 27 -------------------
 .../params/costmap_common_params.yaml         | 24 ++++++++++-------
 .../params/global_costmap_params.yaml         |  7 ++---
 .../params/local_costmap_params.yaml          |  7 +++--
 .../params/teb_local_planner_params.yaml      | 18 ++++++++-----
 5 files changed, 32 insertions(+), 51 deletions(-)

diff --git a/src/kart_2dnav/launch/sim_autocross_track.launch b/src/kart_2dnav/launch/sim_autocross_track.launch
index ca3115e..b4eb08e 100644
--- a/src/kart_2dnav/launch/sim_autocross_track.launch
+++ b/src/kart_2dnav/launch/sim_autocross_track.launch
@@ -46,34 +46,7 @@
   <!-- ROS NAVIGATION STACK -->
   <!-- Configure and run move_base to set up the ROS Navigation Stack. -->
   <!-- More details on inputs/outputs can be found in move_base.launch -->
-  <!--
   <include file="$(find kart_2dnav)/launch/move_base.launch" />
-  -->
-  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
-      <rosparam file="$(find teb_local_planner_tutorials)/cfg/carlike/costmap_common_params.yaml" 
-                command="load" ns="global_costmap" />
-      <!-- Modified teb_local_planner_tutorials version to set laserscan input 
-           to move_base come from top/scan -->
-      <rosparam file="$(find kart_2dnav)/params/sim_params/costmap_common_params.yaml" 
-                command="load" ns="local_costmap" />
-      <rosparam file="$(find teb_local_planner_tutorials)/cfg/carlike/local_costmap_params.yaml" 
-                command="load" />
-      <rosparam file="$(find teb_local_planner_tutorials)/cfg/carlike/global_costmap_params.yaml" 
-                command="load" />
-      <rosparam 
-            file="$(find teb_local_planner_tutorials)/cfg/carlike/teb_local_planner_params.yaml" 
-            command="load" />
-      <param name="base_global_planner" value="global_planner/GlobalPlanner" />
-      <param name="planner_frequency" value="1.0" />
-      <param name="planner_patience" value="5.0" />
-
-      <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
-      <param name="controller_frequency" value="5.0" />
-      <param name="controller_patience" value="15.0" />
-      <!-- Our carlike robot is not able to rotate in place -->
-      <param name="clearing_rotation_allowed" value="false" />  
-  </node>
-
 
   <!-- MICROCONTROLLER COMM -->
   <!-- Run the communication script to enable and start communicating with the 
diff --git a/src/kart_2dnav/params/costmap_common_params.yaml b/src/kart_2dnav/params/costmap_common_params.yaml
index e152782..2ec8c51 100644
--- a/src/kart_2dnav/params/costmap_common_params.yaml
+++ b/src/kart_2dnav/params/costmap_common_params.yaml
@@ -2,7 +2,11 @@
 # See http://wiki.ros.org/costmap_2d for details.
 
 # TODO(Issue #8): Correct this values for our kart's specific physical attr/sensors.
-footprint: [ [0.7112, 0.4064], [0.7112, -1.524], [-0.7112, -1.524], [-0.7112, 0.4064] ]
+# robot footprint in meters
+# 1. (maybe) footprint of our go-kart
+# footprint: [ [0.7112, 0.4064], [0.7112, -1.524], [-0.7112, -1.524], [-0.7112, 0.4064] ]
+# 2. footprint used by teb_local_planner tutorials. Use this for simulation as of now
+footprint: [ [-0.1,-0.125], [0.5,-0.125], [0.5,0.125], [-0.1,0.125] ]
 
 transform_tolerance: 0.2 
 map_type: costmap
@@ -10,12 +14,14 @@ map_type: costmap
 obstacle_layer:
     enabled: true 
     # TODO(Issue #8): Correct this values for our kart's specific physical attr/sensors.
-    obstacle_range: 2.5 
-    # TODO(Issue #8): Correct this values for our kart's specific physical attr/sensors.
-    max_obstacle_height: 2.0 
+    obstacle_range: 3.0
     # TODO(Issue #8): Will likely change in tandem with obstacle_range above.
-    raytrace_range: 3.0
-    inflation_radius: 0.5 
+    raytrace_range: 3.5
+    # TODO(Issue #8): Correct this values for our kart's specific physical attr/sensors.
+    max_obstacle_height: 2.0
+    min_obstacle_height: 0.0
+    # ------------------------
+    inflation_radius: 0.2
     track_unknown_space: false
     combination_method: 1
     observation_sources: laser_scan_sensor point_cloud_sensor
@@ -26,10 +32,8 @@ obstacle_layer:
 
 inflation_layer:
     enabled: true
-    cost_scaling_factor: 10.0 # exponential rate at which the obstacle 
-                              # cost drops off [default]
-    inflation_radius: 0.5 # max distance from an obstacle at which costs 
-                          # are incurred for planning paths [default]
+    cost_scaling_factor: 10.0
+    inflation_radius: 0.5
 
 static_layer:
     enabled: true
diff --git a/src/kart_2dnav/params/global_costmap_params.yaml b/src/kart_2dnav/params/global_costmap_params.yaml
index 1bb3b8b..9a6211c 100644
--- a/src/kart_2dnav/params/global_costmap_params.yaml
+++ b/src/kart_2dnav/params/global_costmap_params.yaml
@@ -5,11 +5,12 @@ global_costmap:
     global_frame: "map"
     robot_base_frame: "base_link"
     update_frequency: 5.0
-    public_frequency: 2.5
-    static_map: false
+    public_frequency: 0.5 # match teb_local_planner_tutorials
+    static_map: false     # we are not using a map server / existing map
 
-    transform_tolerance: 0.5
+    transform_tolerance: 0.2
     plugins:
         - {name: static_layer, type:    "costmap_2d::StaticLayer"}
         - {name: obstacle_layer, type:  "costmap_2d::VoxelLayer"}
         - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
+
diff --git a/src/kart_2dnav/params/local_costmap_params.yaml b/src/kart_2dnav/params/local_costmap_params.yaml
index be6a4ca..6f96d8b 100644
--- a/src/kart_2dnav/params/local_costmap_params.yaml
+++ b/src/kart_2dnav/params/local_costmap_params.yaml
@@ -1,7 +1,6 @@
 # All parameters are set to default values unless otherwise noted.
 # See http://wiki.ros.org/costmap_2d for details.
 
-
 local_costmap:
     global_frame: "map"
     robot_base_frame: "base_link"
@@ -11,9 +10,9 @@ local_costmap:
     rolling_window: true
     # TODO: Revisit width, height, resolution, etc. here once we start looking 
     #       at creating a map from first lap.
-    width: 6.0
-    height: 6.0
-    resolution: 0.05
+    width: 5.5
+    height: 5.5
+    resolution: 0.1
     transform_tolerance: 0.5
 
     plugins:
diff --git a/src/kart_2dnav/params/teb_local_planner_params.yaml b/src/kart_2dnav/params/teb_local_planner_params.yaml
index 0b3a04c..27335b6 100644
--- a/src/kart_2dnav/params/teb_local_planner_params.yaml
+++ b/src/kart_2dnav/params/teb_local_planner_params.yaml
@@ -5,14 +5,14 @@ TebLocalPlannerROS:
 
     odom_topic: "odom"
 
-    # Trajectory of the kart
+    # Trajectory
 
     teb_autosize: True
     dt_ref: 1
     dt_hysteresis: 0.1
     global_plan_overwrite_orientation: True
     allow_init_with_backwards_motion: True
-    max_global_plan_lookahead_dist: 5.0
+    max_global_plan_lookahead_dist: 3.0
     global_plan_viapoint_sep: -0.1
     global_plan_prune_distance: 1 
     exact_arc_length: False
@@ -32,14 +32,18 @@ TebLocalPlannerROS:
     acc_lim_x: 0.5
     acc_lim_theta: 0.5
 
-    min_turning_radius: 0.51
-    wheelbase: 1.0541
+    min_turning_radius: 0.5         # (maybe) phyiscal val: 0.51
+    wheelbase: 0.4                  # (maybe) phyiscal val: 1.0541
     cmd_angle_instead_rotvel: True
 
     footprint_model:
         type: "polygon"
-        vertices: [ [0.7112, 0.4064],  [0.7112, -1.524], 
-                    [-0.7112, -1.524], [-0.7112, 0.4064] ]
+        # 1. (maybe) footprint of our go-kart
+        #vertices: [ [0.7112, 0.4064],  [0.7112, -1.524],
+        #            [-0.7112, -1.524], [-0.7112, 0.4064] ]
+        # 2. footprint used by teb_local_planner tutorials.
+        #    Use this for simulation as of now
+        vertices: [[-0.1,-0.125], [0.5,-0.125], [0.5,0.125], [-0.1,0.125]]
     # ---------------------------------
 
     # Goal Tolerance
@@ -51,7 +55,7 @@ TebLocalPlannerROS:
 
     # Obstacles
 
-    min_obstacle_dist: 0.5 
+    min_obstacle_dist: 0.25
     inflation_dist: 0.6
     include_costmap_obstacles: True
     costmap_obstacles_behind_robot_dist: 1.0