teb planner works in sim

vz_stretch_navigation/config/common_costmap_params_gazebo.yaml

@@ -1,8 +1,9 @@
 obstacle_range: 2.5
 raytrace_range: 3.0
-robot_radius: 0.1
+#robot is square ~13.5*13.5 inchfootprint. This 0.2 is a circle with the same area
+robot_radius: 0.20 #robot is square ~13.5*13.5 inchfootprint. This 0.2 is a circle with the same area
 inflation_radius: 0.25
 
 observation_sources: laser_scan_sensor
-
+# the change in this and hardware file is the scan topic used here as opposed to scan_filtered
 laser_scan_sensor: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true}

vz_stretch_navigation/config/teb_local_planner_params_gazebo.yaml

@@ -0,0 +1,85 @@
+# Copy pasted from 
+# http://wiki.ros.org/teb_local_planner/Tutorials/Configure%20and%20run%20Robot%20Navigation
+
+TebLocalPlannerROS:
+# todo: check if these two make sense
+ odom_topic: odom
+ map_frame: /odom
+    
+ # Trajectory
+  
+ teb_autosize: True
+ dt_ref: 0.3
+ dt_hysteresis: 0.1
+ global_plan_overwrite_orientation: True
+ max_global_plan_lookahead_dist: 3.0
+ feasibility_check_no_poses: 5
+    
+ # Robot
+         
+ max_vel_x: 1.5 #0.4 todo: ananya change this back
+ max_vel_x_backwards: 0.2
+ max_vel_theta: 0.3
+ acc_lim_x: 0.2 #0.5
+ acc_lim_theta: 0.25 #0.5 changed this ananya
+ min_turning_radius: 0.0
+ footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
+   type: "point"
+   radius: 0.2 # for type "circular"
+   line_start: [-0.3, 0.0] # for type "line"
+   line_end: [0.3, 0.0] # for type "line"
+   front_offset: 0.2 # for type "two_circles"
+   front_radius: 0.2 # for type "two_circles"
+   rear_offset: 0.2 # for type "two_circles"
+   rear_radius: 0.2 # for type "two_circles"
+   vertices: [ [0.25, 0.05] ] # for type "polygon"
+  #  vertices: [ [0.25, -0.05], [0.18, -0.05], [0.18, -0.18], [-0.19, -0.18], [-0.25, 0], [-0.19, 0.18], [0.18, 0.18], [0.18, 0.05], [0.25, 0.05] ] # for type "polygon"
+
+ # GoalTolerance
+    
+ xy_goal_tolerance: 0.2
+ yaw_goal_tolerance: 0.1
+ free_goal_vel: False
+    
+ # Obstacles
+    
+ min_obstacle_dist: 0.4 #this needs to change todo ananya
+ include_costmap_obstacles: True
+ costmap_obstacles_behind_robot_dist: 1.0
+ obstacle_poses_affected: 30
+ costmap_converter_plugin: ""
+ costmap_converter_spin_thread: True
+ costmap_converter_rate: 5
+
+ # Optimization
+    
+ no_inner_iterations: 5
+ no_outer_iterations: 4
+ optimization_activate: True
+ optimization_verbose: False
+ penalty_epsilon: 0.1 #todo : ananya this has something to do with allowing cost violations. look into this
+ weight_max_vel_x: 2
+ weight_max_vel_theta: 1
+ weight_acc_lim_x: 1
+ weight_acc_lim_theta: 10 #1 changed this ananya
+ weight_kinematics_nh: 1000
+ weight_kinematics_forward_drive: 1
+ weight_kinematics_turning_radius: 1
+ weight_optimaltime: 1
+ weight_obstacle: 50 #todo : ananya this has something to do with allowing cost violations. look into this
+ weight_dynamic_obstacle: 10 # not in use yet
+ alternative_time_cost: False # not in use yet
+
+ # Homotopy Class Planner
+
+ enable_homotopy_class_planning: True
+ enable_multithreading: True
+ simple_exploration: False
+ max_number_classes: 4
+ roadmap_graph_no_samples: 15
+ roadmap_graph_area_width: 5
+ h_signature_prescaler: 0.5
+ h_signature_threshold: 0.1
+ obstacle_keypoint_offset: 0.1
+ obstacle_heading_threshold: 0.45
+ visualize_hc_graph: False

vz_stretch_navigation/launch/navigation_gazebo.launch

@@ -27,7 +27,12 @@
     
     <rosparam file="$(find vz_stretch_navigation)/config/local_costmap_params.yaml" command="load" />
     <rosparam file="$(find vz_stretch_navigation)/config/global_costmap_params_withmap.yaml" command="load" />
-    <rosparam file="$(find vz_stretch_navigation)/config/base_local_planner_params.yaml" command="load" />
+    
+    <!-- Only in this file we are loading teb_local_planner_params.yaml instead of base_local_planner_params.yaml -->
+    <!-- Eventually we should load teb params from the base_local_planner_params.yaml file -->
+    <!-- <rosparam file="$(find vz_stretch_navigation)/config/base_local_planner_params.yaml" command="load" /> -->
+    <rosparam file="$(find vz_stretch_navigation)/config/teb_local_planner_params_gazebo.yaml" command="load" />
+    
     <remap from="/cmd_vel" to="/stretch_diff_drive_controller/cmd_vel" />
 
     <!-- Switch base local planners-->
@@ -37,7 +42,9 @@
 
          
     <!-- <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" /> -->
-    <!-- <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" /> -->
+    <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
+    <param name="controller_frequency" value="10.0" />
+
 
   </node>