From c1fb78a3f9a720be18a9d15c2d6add850640d846 Mon Sep 17 00:00:00 2001
From: abastie <arnaud.bastie@stereolabs.com>
Date: Tue, 21 Aug 2018 16:49:03 +0200
Subject: [PATCH] Replace DWA Planner with Trajectory Planner

---
 .../launch/includes/move_base.launch.xml         |  4 +++-
 .../param/move_base_params.yaml                  |  2 +-
 .../param/trajectory_planner_params.yaml         | 16 ++++++++++++++++
 zed_navigation_tutorial/rviz/navigation.rviz     |  8 ++++----
 4 files changed, 24 insertions(+), 6 deletions(-)
 create mode 100644 zed_navigation_tutorial/param/trajectory_planner_params.yaml

diff --git a/zed_navigation_tutorial/launch/includes/move_base.launch.xml b/zed_navigation_tutorial/launch/includes/move_base.launch.xml
index 86719694..b3910c52 100644
--- a/zed_navigation_tutorial/launch/includes/move_base.launch.xml
+++ b/zed_navigation_tutorial/launch/includes/move_base.launch.xml
@@ -18,6 +18,7 @@
     <rosparam file="$(find zed_navigation_tutorial)/param/local_costmap_params.yaml" command="load" />   
     <rosparam file="$(find zed_navigation_tutorial)/param/global_costmap_params.yaml" command="load" />
     <rosparam file="$(find zed_navigation_tutorial)/param/dwa_local_planner_params.yaml" command="load" />
+    <rosparam file="$(find zed_navigation_tutorial)/param/trajectory_planner_params.yaml" command="load" />
     <rosparam file="$(find zed_navigation_tutorial)/param/move_base_params.yaml" command="load" />
     <rosparam file="$(find zed_navigation_tutorial)/param/global_planner_params.yaml" command="load" />
     <rosparam file="$(find zed_navigation_tutorial)/param/navfn_global_planner_params.yaml" command="load" />
@@ -29,7 +30,8 @@
     <!--param name="global_costmap/robot_base_frame" value="$(arg base_frame_id)"/-->
     <!--param name="local_costmap/global_frame" value="$(arg global_frame_id)"/-->
     <!--param name="local_costmap/robot_base_frame" value="$(arg base_frame_id)"/-->
-    <param name="DWAPlannerROS/global_frame_id" value="$(arg global_frame_id)"/>
+    <!--<param name="DWAPlannerROS/global_frame_id" value="$(arg global_frame_id)"/>-->
+    <param name="TrajectoryPlannerROS/global_frame_id" value="$(arg global_frame_id)"/>
 
     <remap from="cmd_vel" to="navigation_velocity_smoother/raw_cmd_vel"/>
     <remap from="odom" to="$(arg odom_topic)"/>
diff --git a/zed_navigation_tutorial/param/move_base_params.yaml b/zed_navigation_tutorial/param/move_base_params.yaml
index 8e941774..757686af 100644
--- a/zed_navigation_tutorial/param/move_base_params.yaml
+++ b/zed_navigation_tutorial/param/move_base_params.yaml
@@ -15,7 +15,7 @@ oscillation_timeout: 10.0
 oscillation_distance: 0.2
 
 # local planner - default is trajectory rollout
-base_local_planner: "dwa_local_planner/DWAPlannerROS"
+#base_local_planner: "dwa_local_planner/DWAPlannerROS"
 
 base_global_planner: "navfn/NavfnROS" #alternatives: global_planner/GlobalPlanner, carrot_planner/CarrotPlanner
 
diff --git a/zed_navigation_tutorial/param/trajectory_planner_params.yaml b/zed_navigation_tutorial/param/trajectory_planner_params.yaml
new file mode 100644
index 00000000..312e15ba
--- /dev/null
+++ b/zed_navigation_tutorial/param/trajectory_planner_params.yaml
@@ -0,0 +1,16 @@
+TrajectoryPlannerROS:
+  max_vel_x: 0.5
+  min_vel_x: 0.1
+  max_vel_theta: 0.25
+  min_in_place_vel_theta: 0.4
+
+  acc_lim_theta: 0.25
+  acc_lim_x: 2.5
+  acc_lim_Y: 2.5
+
+  holonomic_robot: false
+
+  meter_scoring: true
+
+  xy_goal_tolerance: 0.25
+  yaw_goal_tolerance: 0.2
diff --git a/zed_navigation_tutorial/rviz/navigation.rviz b/zed_navigation_tutorial/rviz/navigation.rviz
index b78afe32..f3519adc 100644
--- a/zed_navigation_tutorial/rviz/navigation.rviz
+++ b/zed_navigation_tutorial/rviz/navigation.rviz
@@ -204,7 +204,7 @@ Visualization Manager:
           Radius: 0.0299999993
           Shaft Diameter: 0.100000001
           Shaft Length: 0.100000001
-          Topic: move_base/DWAPlannerROS/global_plan
+          Topic: move_base/TrajectoryPlannerROS/global_plan
           Unreliable: false
           Value: true
         - Class: rviz/Marker
@@ -249,7 +249,7 @@ Visualization Manager:
           Radius: 0.0299999993
           Shaft Diameter: 0.100000001
           Shaft Length: 0.100000001
-          Topic: move_base/DWAPlannerROS/local_plan
+          Topic: move_base/TrajectoryPlannerROS/local_plan
           Unreliable: false
           Value: true
         - Alpha: 0.800000012
@@ -277,7 +277,7 @@ Visualization Manager:
           Size (Pixels): 3
           Size (m): 0.0399999991
           Style: Flat Squares
-          Topic: move_base/DWAPlannerROS/cost_cloud
+          Topic: move_base/TrajectoryPlannerROS/cost_cloud
           Unreliable: false
           Use Fixed Frame: true
           Use rainbow: true
@@ -307,7 +307,7 @@ Visualization Manager:
           Size (Pixels): 3
           Size (m): 0.0399999991
           Style: Flat Squares
-          Topic: move_base/DWAPlannerROS/trajectory_cloud
+          Topic: /zed/map/loc_map_height_cloud
           Unreliable: false
           Use Fixed Frame: true
           Use rainbow: false