ROS-melodic：源码安裝teb_local_planner算法、替换DWA算法

一.安裝teb_local_planner算法

源码下载地址：GitHub - rst-tu-dortmund/teb_local_planner: An optimal trajectory planner considering distinctive topologies for mobile robots based on Timed-Elastic-Bands (ROS Package)

注意选择对应ROS版本的代码。

放在navigation目录下（或者自己创建一个）：

安装缺失依赖：

rosdep install teb_local_planner

在src当前目录下运行：

	  rosdep install --from-paths src --ignore-src --rosdistro=<ros_version> -y

编译：

      catkin_make -DCATKIN_WHITELIST_PACKAGES="teb_local_planner"

在编译过程中会出现缺包，对应安装就好了。

查看是否安装成功：

rospack plugins --attrib=plugin nav_core

会出现以下相关信息：

teb_local_planner /home/catkin_navigation/src/navigation/teb_local_planner-melodic-devel/teb_local_planner_plugin.xml

测试：

roslaunch teb_local_planner test_optim_node.launch

二.teb替代DWA算法

move_base.launch文件中添加teb配置内容：

<launch>
  <!-- Arguments -->
  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
  <arg name="cmd_vel_topic" default="/cmd_vel" />
  <arg name="odom_topic" default="odom" />
  <arg name="move_forward_only" default="false"/><!--true机器人只能向前运动-->

  <!-- move_base -->
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <!--加载Dijkstra、A star算法-->
    <param name="base_global_planner" value="global_planner/GlobalPlanner"/>
    <param name="planner_frequency" value="1.0" />
    <param name="planner_patience" value="5.0" />
    <!--加载DWA算法
    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />-->
    <!--加载TEB算法-->
    <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
    <!--配置文件-->
    <rosparam file="$(find turtlebot3_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find turtlebot3_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find turtlebot3_navigation)/param/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find turtlebot3_navigation)/param/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find turtlebot3_navigation)/param/move_base_params.yaml" command="load" />
    <!--DWA配置文件-->
    <rosparam file="$(find turtlebot3_navigation)/param/dwa_local_planner_params_$(arg model).yaml" command="load" />
    <!--TEB配置文件-->
    <rosparam file="$(find turtlebot3_navigation)/param/teb_local_planner_params.yaml" command="load" />
    <remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
    <remap from="odom" to="$(arg odom_topic)"/>
    <param name="DWAPlannerROS/min_vel_x" value="0.0" if="$(arg move_forward_only)" />
  </node>
</launch>

在turtlebot3_navigation/param文件夹下创建teb_local_planner_params.yaml文件：

TebLocalPlannerROS:
  odom_topic: odom
  map_frame: map

  # Trajectory
  teb_autosize: True
  dt_ref: 0.5
  dt_hysteresis: 0.05
  min_samples: 3
  global_plan_overwrite_orientation: True
  global_plan_viapoint_sep: 0.1 # negative, do not use viapoints. positive, use them. the actual value does not matter
  max_global_plan_lookahead_dist: 1.5
  global_plan_prune_distance: 0.6
  force_reinit_new_goal_dist: 1.0
  feasibility_check_no_poses: 3
  publish_feedback: false
  allow_init_with_backwards_motion: true
  exact_arc_length: false
  shrink_horizon_backup: true
  shrink_horizon_min_duration: 10

  # Robot
  max_vel_x: 0.22
  max_vel_x_backwards: 0.5
  max_vel_theta: 2.75
  max_vel_y: 0.0
  acc_lim_y: 0.0
  acc_lim_x: 2.5
  acc_lim_theta: 3.2
  min_turning_radius: 0.0
  wheelbase: 0.0 # not used, is differential
  cmd_angle_instead_rotvel: false # not used, is differential
  footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
#    type: "circular"
#    radius: 0.5 # for type "circular"
#    type: "line"
#    line_start: [-0.0545, 0.0] # for type "line"
#    line_end: [0.0545, 0.0] # for type "line"
# front_offset: 0.2 # for type "two_circles"
# front_radius: 0.2 # for type "two_circles"
# rear_offset: 0.2 # for type "two_circles"
# rear_radius: 0.2 # for type "two_circles"
    type: "polygon"
    vertices: [[-0.105, -0.105], [-0.105, 0.105], [0.041, 0.105], [0.041, -0.105]] # for type "polygon"

  # GoalTolerance
  xy_goal_tolerance: 0.05
  yaw_goal_tolerance: 0.17
  free_goal_vel: False

  # Obstacles
  min_obstacle_dist: 0.1 # minimum distance to obstacle: it depends on the footprint_model
  inflation_dist: 0.0 # greater than min_obstacle_dist to take effect
  include_costmap_obstacles: True # use the local costmap
  costmap_obstacles_behind_robot_dist: 1.0 # distance at which obstacles behind the robot are taken into account
  legacy_obstacle_association: false
  obstacle_poses_affected: 30 # unused if legacy_obstacle_association is false
  obstacle_association_force_inclusion_factor: 10.0 # the obstacles that will be taken into account are those closer than min_obstacle_dist*factor, if legacy is false
  obstacle_association_cutoff_factor: 40.0 # the obstacles that are further than min_obstacle_dist * factor will not be taken into account, if legacy is false
#  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_plugin: "" # deactivate plugin
  costmap_converter_spin_thread: True
  costmap_converter_rate: 10

  # Optimization
  no_inner_iterations: 5
  no_outer_iterations: 4
  optimization_activate: True # optimize
  optimization_verbose: False
  penalty_epsilon: 0.1
  weight_max_vel_x: 2
  weight_max_vel_y: 1
  weight_max_vel_theta: 1
  weight_acc_lim_x: 1
  weight_acc_lim_y: 1
  weight_acc_lim_theta: 1
  weight_kinematics_nh: 1 # is a holonomic robot
  weight_kinematics_forward_drive: 10 # prefer forward driving, for differential
  weight_kinematics_turning_radius: 0 # prefer turns that respect the min_turning_radius, not used if differential (min_turning_radius = 0)
  weight_optimaltime: 1.0 # prefer trajectories with less transition time
  weight_obstacle: 50.0 # prefer trajectories that respect the min_obstacle_dist
  weight_inflation: 0.1 # prefer trajectories that respect the inflation of the obstacles
  #weight_dynamic_obstacle: 10 # not in use yet
  weight_viapoint: 1.0 # prefer trajectories that respect the viapoints in the global path
  weight_adapt_factor: 2 # factor to multiply some weights (currently only weight_obstacle) at each iteration (gives better results than a huge value for the weight)

  # Homotopy Class Planner
  enable_homotopy_class_planning: True # currently not used

效果展示：