saved work, odom not right, to be fixed

scout_navigation/param/amcl_params.yaml

@@ -0,0 +1,38 @@
+use_map_topic: true
+
+odom_frame_id: "odom"
+base_frame_id: "base_footprint"
+global_frame_id: "map"
+
+## Publish scans from best pose at a max of 10 Hz
+odom_model_type: "diff"
+odom_alpha5: 0.1
+gui_publish_rate: 10.0
+laser_max_beams: 60
+laser_max_range: 12.0
+min_particles: 500
+max_particles: 2000
+kld_err: 0.05
+kld_z: 0.99
+odom_alpha1: 0.2
+odom_alpha2: 0.2
+## translation std dev, m 
+odom_alpha3: 0.2
+odom_alpha4: 0.2
+laser_z_hit: 0.5
+aser_z_short: 0.05
+laser_z_max: 0.05
+laser_z_rand: 0.5
+laser_sigma_hit: 0.2
+laser_lambda_short: 0.1
+laser_model_type: "likelihood_field" # "likelihood_field" or "beam"
+laser_likelihood_max_dist: 2.0
+update_min_d: 0.25
+update_min_a: 0.2
+
+resample_interval: 1
+
+## Increase tolerance because the computer can get quite busy 
+transform_tolerance: 1.0
+recovery_alpha_slow: 0.001
+recovery_alpha_fast: 0.1

scout_navigation/param/diff_drive/costmap_common_params.yaml

@@ -0,0 +1,33 @@
+
+#---standard pioneer footprint---
+#---(in meters)---
+#robot_radius: 0.25 #0.17
+#footprint_padding: 0.00
+
+footprint: [ [-0.465,-0.350], [0.465,-0.350], [0.465,0.350], [-0.465,0.350] ]
+
+transform_tolerance: 0.2
+map_type: costmap
+
+always_send_full_costmap: true
+
+obstacle_layer:
+ enabled: true
+ obstacle_range: 3.0
+ raytrace_range: 4.0
+ inflation_radius: 0.2
+ track_unknown_space: true
+ combination_method: 1
+
+ observation_sources: laser_scan_sensor
+ laser_scan_sensor: {data_type: LaserScan, topic: scan, marking: true, clearing: true}
+
+
+inflation_layer:
+  enabled:              true
+  cost_scaling_factor:  10.0  # exponential rate at which the obstacle cost drops off (default: 10)
+  inflation_radius:     0.5  # max. distance from an obstacle at which costs are incurred for planning paths.
+
+static_layer:
+  enabled:              true
+  map_topic:            "/map"

scout_navigation/param/diff_drive/costmap_converter_params.yaml

@@ -0,0 +1,31 @@
+###########################################################################################
+## 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::CostmapToPolygonsDBSMCCH"
+  costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSRANSAC"
+  #costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSMCCH"
+  #costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSConcaveHull"
+  costmap_converter_spin_thread: True
+  costmap_converter_rate: 5
+ 
+ 
+  ## Configure plugins (namespace move_base/costmap_to_lines or move_base/costmap_to_polygons)
+  ## costmap_converter/CostmapToLinesDBSRANSAC, costmap_converter/CostmapToLinesDBSMCCH, costmap_converter/CostmapToPolygonsDBSMCCH
+  costmap_converter/CostmapToLinesDBSRANSAC:
+    cluster_max_distance: 0.4
+    cluster_min_pts: 2
+    ransac_inlier_distance: 0.15
+    ransac_min_inliers: 10
+    ransac_no_iterations: 1500
+    ransac_remainig_outliers: 3
+    ransac_convert_outlier_pts: True
+    ransac_filter_remaining_outlier_pts: False
+    convex_hull_min_pt_separation: 0.1
+ 

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

scout_navigation/param/diff_drive/global_costmap_params.yaml

@@ -0,0 +1,16 @@
+global_costmap:
+  global_frame: map
+  robot_base_frame: base_footprint
+  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/local_costmap_params.yaml

@@ -0,0 +1,15 @@
+local_costmap:
+  global_frame: map
+  robot_base_frame: base_footprint
+  update_frequency: 5.0
+  publish_frequency: 2.0
+  static_map: false
+  rolling_window: true
+  width: 5.5
+  height: 5.5
+  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/teb_local_planner_params.yaml

@@ -0,0 +1,112 @@
+TebLocalPlannerROS:
+
+ odom_topic: odom
+    
+ # Trajectory
+  
+ teb_autosize: True
+ dt_ref: 0.3
+ dt_hysteresis: 0.1
+ max_samples: 500
+ global_plan_overwrite_orientation: True
+ allow_init_with_backwards_motion: False
+ max_global_plan_lookahead_dist: 3.0
+ global_plan_viapoint_sep: -1
+ global_plan_prune_distance: 1
+ exact_arc_length: False
+ feasibility_check_no_poses: 5
+ publish_feedback: False
+    
+ # Robot
+         
+ max_vel_x: 0.8 #0.4
+ max_vel_x_backwards: 0.2
+ max_vel_y: 0.0
+ max_vel_theta: 1.8 #0.3
+ acc_lim_x: 0.5
+ acc_lim_theta: 1.8 #0.5
+ min_turning_radius: 0.0 # diff-drive robot (can turn on place!)
+
+ footprint_model:
+   type: "line"
+   line_start: [-0.4, 0.0] 
+   line_end: [0.4, 0.0]   
+
+ # GoalTolerance
+    
+ xy_goal_tolerance: 0.2
+ yaw_goal_tolerance: 0.1
+ free_goal_vel: False
+ complete_global_plan: True
+    
+ # Obstacles
+    
+ min_obstacle_dist: 0.45 #0.25 # This value must also include our robot radius, since footprint_model is set to "point".
+ inflation_dist: 0.6
+ include_costmap_obstacles: True
+ costmap_obstacles_behind_robot_dist: 1.5
+ obstacle_poses_affected: 15
+
+ dynamic_obstacle_inflation_dist: 0.6
+ include_dynamic_obstacles: True
+
+ 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
+ obstacle_cost_exponent: 4
+ weight_max_vel_x: 2
+ weight_max_vel_theta: 1
+ weight_acc_lim_x: 1
+ weight_acc_lim_theta: 1
+ weight_kinematics_nh: 1000
+ weight_kinematics_forward_drive: 1
+ weight_kinematics_turning_radius: 1
+ weight_optimaltime: 1 # must be > 0
+ weight_shortest_path: 0
+ weight_obstacle: 100
+ weight_inflation: 0.2
+ weight_dynamic_obstacle: 10
+ weight_dynamic_obstacle_inflation: 0.2
+ weight_viapoint: 1
+ weight_adapt_factor: 2
+
+ # Homotopy Class Planner
+
+ enable_homotopy_class_planning: True
+ enable_multithreading: True
+ max_number_classes: 4
+ selection_cost_hysteresis: 1.0
+ selection_prefer_initial_plan: 0.9
+ selection_obst_cost_scale: 100.0
+ selection_alternative_time_cost: False
+ 
+ roadmap_graph_no_samples: 15
+ roadmap_graph_area_width: 5
+ roadmap_graph_area_length_scale: 1.0
+ h_signature_prescaler: 0.5
+ h_signature_threshold: 0.1
+ obstacle_heading_threshold: 0.45
+ switching_blocking_period: 0.0
+ viapoints_all_candidates: True
+ delete_detours_backwards: True
+ max_ratio_detours_duration_best_duration: 3.0
+ visualize_hc_graph: False
+ visualize_with_time_as_z_axis_scale: False
+
+# Recovery
+ 
+ shrink_horizon_backup: True
+ shrink_horizon_min_duration: 10
+ oscillation_recovery: True
+ oscillation_v_eps: 0.1
+ oscillation_omega_eps: 0.1
+ oscillation_recovery_min_duration: 10
+ oscillation_filter_duration: 10