Feat/swarinfo 317/dwa planner (#72)

* Simulation launch file update

- Removed communication broker
- Added a garage world for more realistic obstacle avoidance
- Added dependency to the turlebot3_gazebo that contains the garage map

* Removed Teleop pluggin in Rviz

- The teleop pluggin issues null cmd vel all the time even when the move_base is moving around. It has the behavior to slow down the behavior move_base and prevent it to working correctly

* Updated rviz config to support dwa

- Changed local plan and global plan name

* Add DWA Planner

- Added DWA planner params
- Added move_base config file for conveniancy 
- Adjusted navigation.launch to support DWA

* Resolution and base global planner

- Include base Global planner
- Decreased the cell sizes of the costmap (increase resolution)

* WIP planner frequency and increased footprint

* Footprint clearing and local map decrease

-Removed the behavior of footprint clearing since sometimes some obstacle
-  Decreased local map size to remove an oscillation problem

* Reduce costmap, disable footprint clearing and re adding global planner

* Bump roboclaw with velocity fix

* Bump roboclaw

* Fix name of the namespace of the global planner param

* bump submodules

* Added comm broker back

* Added comment back

Co-authored-by: Etienne <vile2094@usherbrooke.ca>

Author: xgroleau

contrib/roboclaw

@@ -1,14 +1,14 @@
-BaseGlobalPlanner:
+GlobalPlanner:
   allow_unknow: True          # Specifies if the unknow grid cells should be use to plan the trajectory
   default_tolerance: 0.0      # Tolerance around the goal
 
-  use_dijkstra: false          # If true, use Dijkstra's algorithm. Otherwise, use A*.
+  use_dijkstra: false         # If true, use Dijkstra's algorithm. Otherwise, use A*.
   use_quadratic: true         # If true, use the quadratic approximation of the potential. Otherwise, use a simpler calculation
   use_grid_path: false        # If true, create a path that follows the grid boundaries. Otherwise, use a gradient descent method
   old_navfn_behavior: false   # If true, the planner will mirror the behavior of navfn
 
   lethal_cost: 253            # Lethal cost (can be changed with dynamic reconfiguring)
-  neutral_cost : 50           # Neutral Cost 
+  neutral_cost : 66           # Neutral Cost 
   cost_factor: 0.8            # Factor to multiply each cost from costmap by (can be changed with dynamic reconfiguring)
 
   publish_potential: true     # Publish Potential Costmap (can be changed with dynamic reconfiguring)

src/swarmus_ros_navigation/config/base_global_planner_params.yaml

@@ -1,12 +1,12 @@
 #---(in meters)---
-footprint: [ [-0.3425,-0.195], [0.1425,-0.195], [0.1425,0.195], [-0.3425,0.195] ]
+footprint: [ [-0.3625,-0.215], [0.1625,-0.215], [0.1625,0.215], [-0.3625,0.215] ]
 
 obstacle_layer:
  enabled: true
  obstacle_range: 2.5
  raytrace_range: 3.0
  track_unknown_space: false
- footprint_clearing_enabled: true
+ footprint_clearing_enabled: false
  combination_method: 1
 
  observation_sources: laser_scan_sensor

src/swarmus_ros_navigation/config/base_local_planner_params.yaml

@@ -0,0 +1,51 @@
+# See here for documentation: http://wiki.ros.org/dwa_local_planner
+DWAPlannerROS:
+
+  # Robot Configuration Parameters
+  max_vel_x: 0.3
+  min_vel_x: -0.2
+
+  max_vel_y: 0.0
+  min_vel_y: 0.0
+
+  # The velocity when robot is moving in a straight line
+  max_vel_trans:  0.3
+  min_vel_trans:  0.06
+
+  # weirdly enough, parameters don't work with names given in documentation
+  max_vel_theta: 1.0
+  min_vel_theta: 0.05
+
+  acc_lim_x: 0.6
+  acc_lim_y: 0.0
+  acc_lim_theta: 3.7
+
+  # Goal Tolerance Parametes
+  xy_goal_tolerance: 0.1
+  yaw_goal_tolerance: 0.2
+  latch_xy_goal_tolerance: true
+
+  # Forward Simulation Parameters
+  sim_time: 2.0
+  vx_samples: 10
+  vy_samples: 0
+  vth_samples: 40
+  sim_granularity: 0.025
+  controller_frequency: 20.0
+
+  # Trajectory Scoring Parameters
+  path_distance_bias: 32.0
+  goal_distance_bias: 20.0
+  occdist_scale: 0.02
+  forward_point_distance: 0.325
+  stop_time_buffer: 0.15
+  scaling_speed: 0.2
+  max_scaling_factor: 0.2
+  prune_plan: true
+
+  # Oscillation Prevention Parameters
+  oscillation_reset_dist: 0.01
+
+  # Debugging
+  publish_traj_pc : false
+  publish_cost_grid: true

src/swarmus_ros_navigation/config/costmap_common_params.yaml

@@ -6,7 +6,7 @@ global_costmap:
   rolling_window: true
   width: 10.0
   height: 10.0
-  resolution: 0.05
+  resolution: 0.025
   transform_tolerance: 0.5
 
   plugins:

src/swarmus_ros_navigation/config/dwa_local_planner_params_pioneer.yaml

@@ -6,7 +6,7 @@ global_costmap:
   rolling_window: false
   width: 10.0
   height: 10.0
-  resolution: 0.05
+  resolution: 0.025
   transform_tolerance: 0.5
 
   plugins:

src/swarmus_ros_navigation/config/global_costmap_params.yaml

@@ -5,9 +5,9 @@ local_costmap:
   publish_frequency: 10
   static_map: false
   rolling_window: true
-  width: 6.0
-  height: 6.0
-  resolution: 0.05
+  width: 3.0
+  height: 3.0
+  resolution: 0.025
   transform_tolerance: 0.5
 
   plugins:

src/swarmus_ros_navigation/config/global_costmap_with_map_params.yaml

@@ -0,0 +1,9 @@
+shutdown_costmaps: false
+controller_frequency: 20.0
+planner_patience: 5.0
+controller_patience: 15.0
+conservative_reset_dist: 3.0
+planner_frequency: 0 #1
+oscillation_timeout: 15.0
+oscillation_distance: 0.05
+holonomic_robot: false

src/swarmus_ros_navigation/config/local_costmap_params.yaml

@@ -6,6 +6,9 @@
   <!-- Navigation -->
   <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
     <!-- Load common parameters of the local and global planner-->
+    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS"/>
+    <param name="base_global_planner" value="global_planner/GlobalPlanner"/>
+    
     <rosparam file="$(find swarmus_ros_navigation)/config/costmap_common_params.yaml" command="load" ns="global_costmap" subst_value="true"/>
     <rosparam file="$(find swarmus_ros_navigation)/config/costmap_common_params.yaml" command="load" ns="local_costmap" subst_value="true"/>
 
@@ -14,9 +17,9 @@
 
     <rosparam file="$(find swarmus_ros_navigation)/config/local_costmap_params.yaml" command="load" subst_value="true"/>
 
-    <rosparam file="$(find swarmus_ros_navigation)/config/base_local_planner_params.yaml" command="load" subst_value="true"/>
+    <rosparam file="$(find swarmus_ros_navigation)/config/move_base_params_pioneer.yaml" command="load" />
 
     <rosparam file="$(find swarmus_ros_navigation)/config/base_global_planner_params.yaml" command="load" subst_value="true"/>
-  
+    <rosparam file="$(find swarmus_ros_navigation)/config/dwa_local_planner_params_pioneer.yaml" command="load"/>
   </node>
 </launch>