saved work, odom not right, to be fixed

scout_navigation/param/diff_drive/dyn_obst/costmap_converter_params.yaml

@@ -0,0 +1,41 @@
+###########################################################################################
+## NOTE: Costmap conversion is experimental. Its purpose is to combine many point        ##
+## obstales into clusters, computed in a separate thread in order to improve the overall ## 
+## efficiency of local planning. However, the implemented conversion algorithms are in a ##
+## very early stage of development. Contributions are welcome!                           ##
+###########################################################################################
+
+TebLocalPlannerROS:
+
+  ## Costmap converter plugin   
+  costmap_converter_plugin: "costmap_converter::CostmapToDynamicObstacles"
+  costmap_converter_spin_thread: True
+  costmap_converter_rate: 5
+ 
+ 
+  ## Configure plugins (namespace move_base/CostmapToDynamicObstacles)
+  costmap_converter/CostmapToDynamicObstacles:
+    alpha_slow: 0.3
+    alpha_fast: 0.85
+    beta: 0.85
+    min_sep_between_slow_and_fast_filter: 80
+    min_occupancy_probability: 180
+    max_occupancy_neighbors: 100
+    morph_size: 1
+    filter_by_area: True
+    min_area: 3
+    max_area: 300
+    filter_by_circularity: True
+    min_circularity: 0.2
+    max_circularity: 1.0
+    filter_by_inertia: True
+    min_intertia_ratio: 0.2
+    max_inertia_ratio: 1.0
+    filter_by_convexity: False
+    min_convexity: 0.0
+    max_convexity: 1.0
+    dt: 0.2
+    dist_thresh: 60.0
+    max_allowed_skipped_frames: 3
+    max_trace_length: 10
+    static_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"

scout_navigation/param/diff_drive/dyn_obst/global_costmap_params.yaml

@@ -0,0 +1,16 @@
+global_costmap:
+  global_frame: map
+  robot_base_frame: robot_0/base_link
+  update_frequency: 1.0
+  publish_frequency: 0.5
+  static_map: true
+ 
+  transform_tolerance: 0.5
+  plugins:
+    - {name: static_layer,            type: "costmap_2d::StaticLayer"}
+    - {name: obstacle_layer,          type: "costmap_2d::VoxelLayer"}
+    - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}
+
+
+
+

scout_navigation/param/diff_drive/dyn_obst/local_costmap_params.yaml

@@ -0,0 +1,15 @@
+local_costmap:
+  global_frame: map
+  robot_base_frame: robot_0/base_link
+  update_frequency: 5.0
+  publish_frequency: 5.001 # actually exactly 5.0Hz, see https://github.com/ros-planning/navigation/issues/383
+  static_map: false
+  rolling_window: true
+  width: 12
+  height: 12
+  resolution: 0.1
+  transform_tolerance: 0.5
+  
+  plugins:
+   - {name: static_layer,        type: "costmap_2d::StaticLayer"}
+   - {name: obstacle_layer,      type: "costmap_2d::ObstacleLayer"}

scout_navigation/param/diff_drive/dyn_obst/teb_local_planner_params.yaml

@@ -0,0 +1,83 @@
+TebLocalPlannerROS:
+
+ odom_topic: odom
+    
+ # Trajectory
+  
+ teb_autosize: True
+ dt_ref: 0.3
+ dt_hysteresis: 0.1
+ global_plan_overwrite_orientation: True
+ allow_init_with_backwards_motion: False
+ max_global_plan_lookahead_dist: 3.0
+ feasibility_check_no_poses: 5
+    
+ # Robot
+         
+ max_vel_x: 0.4
+ max_vel_x_backwards: 0.2
+ max_vel_y: 0.0
+ max_vel_theta: 0.3
+ acc_lim_x: 0.5
+ acc_lim_theta: 0.5
+ min_turning_radius: 0.0 # diff-drive robot (can turn on place!)
+
+ footprint_model:
+   type: "point"
+
+ # GoalTolerance
+    
+ xy_goal_tolerance: 0.2
+ yaw_goal_tolerance: 0.1
+ free_goal_vel: False
+    
+ # Obstacles
+    
+ min_obstacle_dist: 0.6 # This value must also include our robot radius, since footprint_model is set to "point".
+ inflation_dist: 0.8
+ dynamic_obstacle_inflation_dist: 0.6
+ include_dynamic_obstacles: True
+ include_costmap_obstacles: True
+ costmap_obstacles_behind_robot_dist: 1.5
+ obstacle_poses_affected: 30
+ # costmap_converter parameters are defined in costmap_converter_params.yaml
+
+ # Optimization
+    
+ no_inner_iterations: 5
+ no_outer_iterations: 4
+ optimization_activate: True
+ optimization_verbose: False
+ penalty_epsilon: 0.1
+ weight_max_vel_x: 3
+ weight_max_vel_theta: 1
+ weight_acc_lim_x: 2
+ weight_acc_lim_theta: 2
+ weight_kinematics_nh: 1000
+ weight_kinematics_forward_drive: 1
+ weight_kinematics_turning_radius: 1
+ weight_optimaltime: 1
+ weight_obstacle: 50
+ weight_inflation: 0.3
+ weight_dynamic_obstacle: 50
+ weight_dynamic_obstacle_inflation: 0.3
+ weight_adapt_factor: 2
+
+ # Homotopy Class Planner
+
+ enable_homotopy_class_planning: True
+ enable_multithreading: True
+ simple_exploration: False
+ max_number_classes: 4
+ selection_cost_hysteresis: 1.0
+ selection_obst_cost_scale: 1.0
+ selection_alternative_time_cost: True
+ 
+ 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
+ visualize_with_time_as_z_axis_scale: 0.2