使用teb导航算法使小车在分捡赛地图中从起点导航至终点;修改车轮轮间距为两个车轮中间点距离.

src/robot_navigation/config/teb_cfg/amcl_params.yaml

@@ -0,0 +1,41 @@
+use_map_topic: true
+
+odom_frame_id: "odom_combined"
+base_frame_id: "base_footprint"
+global_frame_id: "map"
+use_map_topic: false
+first_map_only: true
+
+## Publish scans from best pose at a max of 10 Hz
+odom_model_type: "diff"
+odom_alpha5: 0.0
+gui_publish_rate: 10.0
+laser_max_beams: 50
+laser_min_range: 0.14
+laser_max_range: 8.0
+min_particles: 1000
+max_particles: 5000
+kld_err: 0.05
+kld_z: 0.99
+odom_alpha1: 0.2
+odom_alpha2: 0.1
+## translation std dev, m 
+odom_alpha3: 0.1
+odom_alpha4: 0.2
+laser_z_hit: 0.5
+laser_z_short: 0.05
+laser_z_max: 0.05
+laser_z_rand: 0.5
+laser_sigma_hit: 0.1
+laser_lambda_short: 0.1
+laser_model_type: "likelihood_field" # "likelihood_field" or "beam"
+laser_likelihood_max_dist: 2.0
+update_min_d: 0.03
+update_min_a: 0.05
+
+resample_interval: 1
+
+## Increase tolerance because the computer can get quite busy 
+transform_tolerance: 3
+recovery_alpha_slow: 0.0
+recovery_alpha_fast: 0.0

src/robot_navigation/config/teb_cfg/base_global_planner_params.yaml

@@ -0,0 +1,24 @@
+# Copyright: 2016-2020 ROS小课堂 www.corvin.cn
+# Description: 三轮全向移动小车的全局路径规划参数配置文件.
+# Author: corvin
+# History:
+#   20200821: init this file.
+
+# Global Planner Selection
+
+GlobalPlanner:
+    allow_unknown: false
+    default_tolerance: 0.2
+    visualize_potential: false
+    use_dijkstra: true
+    use_quadratic: false
+    use_grid_path: false
+    old_navfn_behavior: false
+
+    lethal_cost: 253
+    neutral_cost: 50
+    cost_factor: 3
+    publish_potential: true
+    orientation_mode: 0
+    orientation_window_size: 1
+

src/robot_navigation/config/teb_cfg/costmap_common_params.yaml

@@ -0,0 +1,31 @@
+
+#---standard pioneer footprint---
+#---(in meters)---
+robot_radius: 0.1
+footprint_padding: 0.00
+
+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.1  # max. distance from an obstacle at which costs are incurred for planning paths.
+
+static_layer:
+  enabled:              true
+  map_topic:            "/map"

src/robot_navigation/config/teb_cfg/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
+ 

src/robot_navigation/config/teb_cfg/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: 10
+  plugins:
+    - {name: static_layer,            type: "costmap_2d::StaticLayer"}
+    - {name: obstacle_layer,          type: "costmap_2d::VoxelLayer"}
+    - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}
+
+
+
+

src/robot_navigation/config/teb_cfg/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: 3
+  height: 3
+  resolution: 0.05
+  transform_tolerance: 10
+  
+  plugins:
+   - {name: static_layer,        type: "costmap_2d::StaticLayer"}
+   - {name: obstacle_layer,      type: "costmap_2d::ObstacleLayer"}

src/robot_navigation/config/teb_local_planner_params.yaml → src/robot_navigation/config/teb_cfg/teb_local_planner_params.yaml

@@ -1,6 +1,6 @@
 TebLocalPlannerROS:
 
- odom_topic: odom
+ odom_topic: odom_ekf
     
  # Trajectory
   
@@ -8,10 +8,10 @@ TebLocalPlannerROS:
  dt_ref: 0.3
  dt_hysteresis: 0.1
  max_samples: 500
- global_plan_overwrite_orientation: False
+ global_plan_overwrite_orientation: True
  allow_init_with_backwards_motion: False
- max_global_plan_lookahead_dist: 3.0
- global_plan_viapoint_sep: 0.05
+ max_global_plan_lookahead_dist: 3
+ global_plan_viapoint_sep: -1
  global_plan_prune_distance: 1
  exact_arc_length: False
  feasibility_check_no_poses: 5
@@ -19,49 +19,31 @@ TebLocalPlannerROS:
     
  # Robot
          
- max_vel_x: 1.53 #1.5
+ max_vel_x: 0.7
  max_vel_x_backwards: 0.2
- max_vel_y: 1.0
- max_vel_theta: 3.30 #3.0 
- acc_lim_x: 0.35 #0.55
- acc_lim_y: 0.5
- acc_lim_theta: 1.1 #1.2
+ max_vel_y: 0.0
+ max_vel_theta: 2.0
+ acc_lim_x: 0.5
+ acc_lim_theta: 1.1
  min_turning_radius: 0.0 # diff-drive robot (can turn on place!)
 
  footprint_model:
    type: "point"
-   #vertices: [[0.16, 0.12], [0.16, -0.12], [-0.16, -0.12], [-0.16, 0.12]]
 
  # GoalTolerance
     
- xy_goal_tolerance: 0.78
+ xy_goal_tolerance: 0.75
  yaw_goal_tolerance: 0.8
- ifree_goal_vel: False
+ free_goal_vel: False
  complete_global_plan: True
     
-
- # 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: 0.05
- global_plan_prune_distance: 1
- exact_arc_length: False
- feasibility_check_no_poses: 2
- publish_feedback: False
-
  # Obstacles
     
- min_obstacle_dist: 0.3 # This value must also include our robot radius, since footprint_model is set to "point".
+ min_obstacle_dist: 0.15 # 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.0
- obstacle_poses_affected: 500.0
+ obstacle_poses_affected: 15
 
  dynamic_obstacle_inflation_dist: 0.6
  include_dynamic_obstacles: True
@@ -82,12 +64,12 @@ TebLocalPlannerROS:
  weight_max_vel_theta: 1
  weight_acc_lim_x: 1
  weight_acc_lim_theta: 1
- weight_kinematics_nh: 10
+ weight_kinematics_nh: 1000
  weight_kinematics_forward_drive: 1
  weight_kinematics_turning_radius: 1
- weight_optimaltime: 4 # must be > 0 
+ weight_optimaltime: 4 # must be > 0
  weight_shortest_path: 0
- weight_obstacle: 50
+ weight_obstacle: 100
  weight_inflation: 0.2
  weight_dynamic_obstacle: 10
  weight_dynamic_obstacle_inflation: 0.2
@@ -101,7 +83,7 @@ TebLocalPlannerROS:
  max_number_classes: 4
  selection_cost_hysteresis: 1.0
  selection_prefer_initial_plan: 0.9
- selection_obst_cost_scale: 1.0
+ selection_obst_cost_scale: 100.0
  selection_alternative_time_cost: False
  
  roadmap_graph_no_samples: 15
@@ -126,4 +108,3 @@ TebLocalPlannerROS:
  oscillation_omega_eps: 0.1
  oscillation_recovery_min_duration: 10
  oscillation_filter_duration: 10
-

src/robot_navigation/launch/amcl_teb.launch

@@ -0,0 +1,43 @@
+<!-- 
+  Simulate a differential drive robot with the teb_local_planner in stage:
+  - stage
+  - map_server
+  - move_base
+  - static map
+  - amcl
+  - rviz view
+ -->
+<launch>
+        <include file="$(find robot_bringup)/launch/robot_bringup.launch" />
+        
+        <!--  ****** Maps *****  -->
+	<node name="map_server" pkg="map_server" type="map_server" args="$(find robot_navigation)/maps/3.yaml" output="screen" />
+
+        <include file="$(find robot_navigation)/launch/ydlidar_X2L.launch" />
+
+        <!--  ************** Navigation ***************  -->
+	<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+  	  	<rosparam file="$(find robot_navigation)/config/teb_cfg/costmap_common_params.yaml" command="load" ns="global_costmap" />
+  	 	<rosparam file="$(find robot_navigation)/config/teb_cfg/costmap_common_params.yaml" command="load" ns="local_costmap" />
+  		<rosparam file="$(find robot_navigation)/config/teb_cfg/local_costmap_params.yaml" command="load" />
+  		<rosparam file="$(find robot_navigation)/config/teb_cfg/global_costmap_params.yaml" command="load" />
+  		<rosparam file="$(find robot_navigation)/config/teb_cfg/teb_local_planner_params.yaml" command="load" />
+                <rosparam file="$(find robot_navigation)/config/teb_cfg/base_global_planner_params.yaml" command="load" />
+
+		<param name="base_global_planner" value="global_planner/GlobalPlanner" />
+		<param name="planner_frequency" value="1.0" />
+		<param name="planner_patience" value="5.0" />
+
+		<param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
+		<param name="controller_frequency" value="15.0" />
+		<param name="controller_patience" value="15.0" />
+	</node>
+
+
+	
+
+	<node pkg="amcl" type="amcl" name="amcl" output="screen">
+		<rosparam file="$(find robot_navigation)/config/teb_cfg/amcl_params.yaml" command="load" />
+	</node>
+
+</launch>

src/ros_arduino_bridge/ros_arduino_python/cfg/my_arduino_params.yaml

@@ -29,7 +29,7 @@ odom_name: odom
 
 # Robot drivetrain parameters
 wheel_diameter: 0.067   #车轮的直径
-wheel_track: 0.205      #两个车轮之间距离
+wheel_track: 0.17     #两个车轮之间距离
 encoder_resolution: 11  #12V DC motors
 gear_reduction: 30      #空载转速330rpm电机的减速比
 linear_scale_correction: 1.00