teb-安装

源码：https://github.com/rst-tu-dortmund/teb_local_planner.git

以husky为例子：

1.在gazebo里面配置好机器人底盘的环境

roslaunch husky_gazebo husky_playpen.launch

https://github.com/husky/husky/tree/kinetic-devel/husky_gazebo

2.配置teb

launch文件的内容

<launch>
        <master auto = "start"/>
        <!--Run footprint laser filter-->
        <!--node name="laser_filter" pkg="tfrbt_navigation" type="laser_footprint_filter"-->

        <!--Run the map server-->
        <!--arg name = "map_file" default = "$(find tfrbt_navigation)/maps/tfrbt_map.yaml"/-->
        <!--arg name = "map_file" default = "$(env TFRBT_MAP_FILE)"/-->
        <node name = "map_server" pkg = "map_server" type = "map_server" args = "$(find tfrbt_navigation)/maps/tfrbt_map.yaml">
    <param name="frame_id" value="/map"/>
  </node>

        <!--Run AMCL-->
        <arg name = "custom_amcl_launch_file" default = "$(find tfrbt_navigation)/launch/includes/amcl/front_back_lasers.launch.xml"/>
        <arg name = "initial_pose_x" default = "0.0"/>
        <arg name = "initial_pose_y" default = "0.0"/>
        <arg name = "initial_pose_a" default = "0.0"/>
        <include file = "$(arg custom_amcl_launch_file)">
                <arg name = "initial_pose_x" value = "arg initial_pose_x"/>
                <arg name = "initial_pose_y" value = "arg initial_pose_y"/>
                <arg name = "initial_pose_a" value = "arg initial_pose_a"/>
        </include>

        <!--Run Move Base-->
        <arg name = "custom_param_file" default = "$(find tfrbt_navigation)/param/laser_costmap_params.yaml"/>
        <include file = "$(find tfrbt_navigation)/launch/includes/move_base_teb.launch.xml"> <!--move_base_dwa.launch.xml-->
                <arg name = "custom_param_file" value = "$(arg custom_param_file)"/>
        </include>

        <!--node name="rviz" pkg="rviz" type="rviz" args="-d $(find teb_local_planner_tutorials)/cfg/rviz_navigation.rviz"/-->
</launch>

主要是文件move_base_teb.launch.xml的配置，查看其内容

<launch>
        <!--include file = "$(find tfrbt_navigation)/launch/includes/velocity_smoother.launch.xml"/-->
        <!--include file = "$(find tfrbt_navigation)/launch/includes/safety_controller.launch.xml"/-->
        <arg name = "odom_frame_id"     default = "odom"/>
        <arg name = "base_frame_id"     default = "base_link"/>
        <arg name = "global_frame_id"   default = "map"/>
        <arg name = "odom_topic"        default = "odom"/>
        <arg name = "laser_topic"       default = "scan"/>
        <arg name = "custom_param_file" default = "$(find tfrbt_navigation)/param/dummy.yaml"/>

        <node pkg = "move_base" type = "move_base" respawn = "false" name = "move_base" output = "screen">
                <rosparam file = "$(find tfrbt_navigation)/param/teb/costmap_common_params.yaml" command = "load" ns = "global_costmap"/>
                <rosparam file = "$(find tfrbt_navigation)/param/teb/costmap_common_params.yaml" command = "load" ns = "local_costmap"/>
                <rosparam file = "$(find tfrbt_navigation)/param/teb/local_costmap_params.yaml" command = "load"/>
                <rosparam file = "$(find tfrbt_navigation)/param/teb/global_costmap_params.yaml" command = "load"/>
                <rosparam file = "$(find tfrbt_navigation)/param/teb/teb_local_planner_params.yaml" command = "load"/>
                <!--rosparam file = "$(find tfrbt_navigation)/param/teb/move_base_params.yaml" command = "load"/-->
                <!--rosparam file = "$(find tfrbt_navigation)/param/teb/global_planner_params.yaml" command = "load"/-->
                <!--rosparam file = "$(find tfrbt_navigation)/param/teb/navfn_global_planner_params.yaml" command = "load"/-->
                <rosparam file = "$(arg custom_param_file)" 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="5.0" />
                            <param name="controller_patience" value="15.0" />

                <param name = "global_costmap/global_frame" value = "$(arg global_frame_id)"/>
                <param name = "global_costmap/robot_base_frame" value = "$(arg base_frame_id)"/>
                <param name = "local_costmap/global_frame" value = "$(arg odom_frame_id)"/>
                <param name = "local_costmap/robot_base_frame" value = "$(arg base_frame_id)"/>
                <!--param name = "DWAPlannerROS/global_frame_id" value = "$(arg odom_frame_id)"/-->

                <!--remap from = "cmd_vel" to = "navigation_velocity_smoother/raw_cmd_vel"/-->
                <remap from = "odom" to = "$(arg odom_topic)"/>
                <remap from = "scan" to = "$(arg laser_topic)"/>
        </node>

</launch>

costmap_common_params.yaml内容

#max_obstacle_height: 2.5 #assume something like an arm is mounted on top of the robot

robot_radius: 0.63
footprint: [[-0.50, -0.38], [-0.50, 0.38], [0.50, 0.38], [0.50, -0.38]]
footprint_padding: 0.02
transform_tolerance: 0.2
#map_type: voxel
map_type: costmap
always_send_full_costmap: true

obstacle_layer:
 enabled: true
 obstacle_range: 3.0
 raytrace_range: 4.0
 inflation_radius: 0.4
 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.65  # max. distance from an obstacle at which costs are incurred for planning paths.

static_layer:
  enabled:              true
  map_topic:            "map"

local_costmap_params.yaml内容

local_costmap:
  global_frame: map
  robot_base_frame: base_link
  update_frequency: 5.0
  publish_frequency: 2.0
  static_map: false
  rolling_window: true
  width: 4
  height: 4
  resolution: 0.05
  transform_tolerance: 0.5

  inflation_layer:
    enabled:              true
    cost_scaling_factor:  10.0  # exponential rate at which the obstacle cost drops off (default: 10)
    inflation_radius:     0.65  # max. distance from an obstacle at which costs are incurred for planning paths.

  plugins:
    - {name: static_layer,        type: "costmap_2d::StaticLayer"}
    - {name: obstacle_layer,      type: "costmap_2d::ObstacleLayer"}
    - {name: inflation_layer,     type: "costmap_2d::InflationLayer"}

global_costmap_params.yaml内容

global_costmap:
  global_frame: map
  robot_base_frame: 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"}
~                                                                    

teb_local_planner_params.yaml内容

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.5
 max_vel_x_backwards: 0.5
 max_vel_y: 0.0
 max_vel_theta: 1.5
 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.65 # This value must also include our robot radius, since footprint_model is set to "point".
 inflation_dist: 0.65
 include_costmap_obstacles: True
 costmap_obstacles_behind_robot_dist: 1.5
 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.01
 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
 weight_obstacle: 50
 weight_dynamic_obstacle: 10 # not in use yet
 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: False

 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