src/robot_navigation/config/costmap_common_params.yaml

@@ -23,10 +23,10 @@
 #robot_radius: 0.13
 
 #圆形机器人前面带上下移动夹爪的参数
-footprint: [[ 0.22,  0.00],[ 0.22,  0.055],[ 0.10,  0.055],
+footprint: [[ 0.30,  0.00],[ 0.30,  0.055],[ 0.10,  0.055],
             [ 0.09,  0.09],[ 0.00,  0.13],[-0.09,  0.09],
             [-0.13,  0.00],[-0.09, -0.09],[ 0.00, -0.13],
-            [ 0.09, -0.09],[ 0.10, -0.055],[ 0.22, -0.055]]
+            [ 0.09, -0.09],[ 0.10, -0.055],[ 0.30, -0.055]]
 map_type: costmap
 
 obstacle_layer:
@@ -41,7 +41,7 @@ obstacle_layer:
 inflation_layer:
  enabled: true
  cost_scaling_factor: 5
- inflation_radius:     0.15
+ inflation_radius:     0.28
 
 
 

src/robot_navigation/config/dwa_local_planner_params.yaml

@@ -11,31 +11,31 @@ DWAPlannerROS:
 #robot configuration params
   acc_lim_x: 0.3
   acc_lim_y: 0
-  acc_lim_th: 0.3
+  acc_lim_th: 0.5
 
-  max_vel_trans: 0.3
+  max_vel_trans: 0.4
   min_vel_trans: 0.25
 
-  max_vel_x: 0.35
-  min_vel_x: -0.25
+  max_vel_x: 0.4
+  min_vel_x: 0.3
 
   max_vel_y: 0
   min_vel_y: 0
 
-  max_vel_theta: 1
+  max_vel_theta: 0.8
   min_vel_theta: 0.5
 
 # Goal Tolerance Parametes
-  xy_goal_tolerance: 0.10
-  yaw_goal_tolerance: 0.17
+  xy_goal_tolerance: 0.30
+  yaw_goal_tolerance: 0.3
   latch_xy_goal_tolerance: false
 
 # Forward Simulation Parametesrs
-  sim_time: 1.5
-  vx_samples: 10
+  sim_time: 1
+  vx_samples: 30
   vy_samples: 0
-  vth_samples: 20
-  controller_frequency: 3
+  vth_samples: 40
+  controller_frequency: 8
 
 # Trajectory Scoring Parameters
   path_distance_bias: 32.0

src/robot_navigation/config/global_costmap_params.yaml

@@ -16,11 +16,12 @@ global_costmap:
    global_frame: map
    robot_base_frame: base_footprint
 
-   update_frequency: 0.1
+   update_frequency: 1.0
    publish_frequency: 1.0
-   static_map: true
+   #static_map: true
    rolling_window: false
-   transform_tolerance: 2.0
+   resolution: 0.05
+   transform_tolerance: 10.0
 
    plugins:
    - {name: static_layer,     type: "costmap_2d::StaticLayer"}

src/robot_navigation/config/local_costmap_params.yaml

@@ -17,17 +17,17 @@
 #  20200413:更新本地代价地图配置参数.
 #  20200927:添加plugins参数,配置障碍物层和膨胀层.
 local_costmap:
-    global_frame: odom_combined
+    global_frame: map
     robot_base_frame: base_footprint
-    update_frequency: 5.0
+    update_frequency: 3.0
     publish_frequency: 3.0
 
-    static_map: false
+    #static_map: false
     rolling_window: true
     width: 2.0
     height: 2.0
-    resolution: 0.02   #should same as map.yaml file
-    transform_tolerance: 2.0
+    resolution: 0.05   #should same as map.yaml file
+    transform_tolerance: 10.0
 
     plugins:
     - {name: obstacle_layer,   type: "costmap_2d::VoxelLayer"}

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/robot_navigation/launch/amcl_ydlidar_X2L.launch

@@ -8,7 +8,7 @@
 -->
 <launch>
   <!-- Run the map server with a map -->
-  <node name="map_server" pkg="map_server" type="map_server" args="$(find robot_navigation)/maps/mymap.yaml" />
+  <node name="map_server" pkg="map_server" type="map_server" args="$(find robot_navigation)/maps/3.yaml" />
 
   <!-- startup ydlidar X2L node -->
   <include file="$(find robot_navigation)/launch/ydlidar_X2L.launch" />
@@ -23,26 +23,31 @@
     <param name="base_frame_id" value="base_footprint"/>
     <param name="global_frame_id" value="map"/>
     <param name="odom_frame_id" value="odom_combined"/>
+    <param name="tf_broadcast"    value="true"/>
 
     <param name="use_map_topic" value="false"/>
-    <param name="transform_tolerance" value="0.1" />
+    <param name="first_map_only" value="false" />
+
+    <param name="transform_tolerance" value="3" />
     <param name="gui_publish_rate" value="10.0"/>
     <param name="laser_max_beams" value="300"/>
     <param name="min_particles" value="500"/>
-    <param name="max_particles" value="5000"/>
+    <param name="max_particles" value="2500"/>
     <param name="kld_err" value="0.1"/>
     <param name="kld_z" value="0.99"/>
     <param name="odom_alpha1" value="0.2"/>
-    <param name="odom_alpha2" value="0.2"/>
-    <param name="odom_alpha3" value="0.2"/>
+    <param name="odom_alpha2" value="0.1"/>
+    <param name="odom_alpha3" value="0.1"/>
     <param name="odom_alpha4" value="0.2"/>
-    <param name="odom_alpha5" value="0.2"/>
+    <param name="odom_alpha5" value="0.0"/>
+
+    <param name="save_pose_rate" value="0.5" />
 
-    <param name="laser_z_hit" value="0.9"/>
+    <param name="laser_z_hit" value="0.5"/>
     <param name="laser_z_short" value="0.05"/>
     <param name="laser_z_max" value="0.05"/>
-    <param name="laser_z_rand" value="0.05"/>
-    <param name="laser_sigma_hit" value="0.2"/>
+    <param name="laser_z_rand" value="0.5"/>
+    <param name="laser_sigma_hit" value="0.1"/>
     <param name="laser_lambda_short" value="0.1"/>
     <param name="laser_model_type" value="likelihood_field"/>
     <param name="laser_model_type" value="beam"/>
@@ -50,11 +55,11 @@
     <param name="laser_max_range" value="7.0"/>
     <param name="laser_likelihood_max_dist" value="2.0"/>
     <param name="update_min_d" value="0.05"/>
-    <param name="update_min_a" value="0.5"/>
-    <param name="resample_interval" value="2"/>
-    <param name="transform_tolerance" value="3.0"/>
-    <param name="recovery_alpha_slow" value="0.0"/>
-    <param name="recovery_alpha_fast" value="0.0"/>
+    <param name="update_min_a" value="0.08"/>
+    <param name="resample_interval" value="1"/>
+    <param name="transform_tolerance" value="0.8"/>
+    <param name="recovery_alpha_slow" value="0.001"/>
+    <param name="recovery_alpha_fast" value="0.1"/>
   </node>
 </launch>
 

src/robot_navigation/launch/gmapping_ydlidar_X2L.launch

@@ -48,7 +48,7 @@
     <param name="ymin" value="-5.0"/>
     <param name="xmax" value="5.0"/>
     <param name="ymax" value="5.0"/>
-    <param name="delta" value="0.02"/> <!--resolution of the map --><!--大环境建图范围为0.05-0.1 -->
+    <param name="delta" value="0.05"/> <!--resolution of the map --><!--大环境建图范围为0.05-0.1 -->
 
     <param name="llsamplerange" value="0.01"/>
     <param name="llsamplestep"  value="0.01"/>

src/robot_navigation/launch/move_base.launch

@@ -14,8 +14,8 @@
 -->
 <launch>
   <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
-    <param name="controller_frequency" value="15" />
-    <param name="controller_patiente"  value="3.0" />
+    <param name="controller_frequency" value="10" />
+    <param name="controller_patiente"  value="4.0" />
 
     <param name="planner_patience"  value="5.0" />
     <param name="planner_frequency" value="0.0" />

src/robot_navigation/maps/3.yaml

@@ -0,0 +1,7 @@
+image: 3.pgm
+resolution: 0.050000
+origin: [-0.04725, -4.214745, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

src/robot_navigation/maps/mymap.yaml

@@ -1,6 +1,6 @@
 image: mymap.pgm
-resolution: 0.020000
-origin: [-5.000000, -5.000000, 0.000000]
+resolution: 0.050000
+origin: [-16.200000, -5.000000, 0.000000]
 negate: 0
 occupied_thresh: 0.65
 free_thresh: 0.196

src/robot_navigation/maps/test.yaml

@@ -1,6 +1,6 @@
 image: test.pgm
-resolution: 0.020000
-origin: [-15.240000, -15.880000, 0.000000]
+resolution: 0.050000
+origin: [-5.000000, -16.200000, 0.000000]
 negate: 0
 occupied_thresh: 0.65
 free_thresh: 0.196

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