局部路径规划器teb_local_planner详解1：初识TEB

nav_core中提供了局部路径规划接口。
teb_local_planner是一个基于优化的局部轨迹规划器。

支持差分模型，car-like模型

一、初步认识

参数enable_homotopy_class_planning表明，是否输出多条轨迹。也就是配置中的HCPlanning

我们来启动查看下效果：

 roslaunch teb_local_planner test_optim_node.launch 

关闭参数查看单条轨迹规划：
rosparam set /test_optim_node/enable_homotopy_class_planning False

注意，要在roscore之后设置上述参数，然后在启动teb算法节点。因为这个参数不是动态配置的。

二、查看速度

第一部分，只能简单查看下轨迹。不知道真实速度的变化。

TEB自定义的feedbackMsg中包含很多有用的轨迹信息。其他node节点可以订阅，用于可视化等等。默认没有pub，可以通过参数：publish_feedback启动

启动节点rosrun teb_local_planner_tutorials visualize_velocity_profile.py可以看到最有路径的速度变化。

代码主要用python将对应轨迹的速度变化画出来

#!/usr/bin/env python

import rospy, math
from teb_local_planner.msg import FeedbackMsg, TrajectoryMsg, TrajectoryPointMsg
from geometry_msgs.msg import PolygonStamped, Point32
import numpy as np
import matplotlib.pyplot as plotter

def feedback_callback(data):
  global trajectory

  if not data.trajectories: # empty
    trajectory = []
    return
  trajectory = data.trajectories[data.selected_trajectory_idx].trajectory
  
  
def plot_velocity_profile(fig, ax_v, ax_omega, t, v, omega):
  ax_v.cla()
  ax_v.grid()
  ax_v.set_ylabel('Trans. velocity [m/s]')
  ax_v.plot(t, v, '-bx')
  ax_omega.cla()
  ax_omega.grid()
  ax_omega.set_ylabel('Rot. velocity [rad/s]')
  ax_omega.set_xlabel('Time [s]')
  ax_omega.plot(t, omega, '-bx')
  fig.canvas.draw()

  
  
def velocity_plotter():
  global trajectory
  rospy.init_node("visualize_velocity_profile", anonymous=True)
  
  topic_name = "/test_optim_node/teb_feedback" # define feedback topic here!
  rospy.Subscriber(topic_name, FeedbackMsg, feedback_callback, queue_size = 1) 

  rospy.loginfo("Visualizing velocity profile published on '%s'.",topic_name) 
  rospy.loginfo("Make sure to enable rosparam 'publish_feedback' in the teb_local_planner.")

  # two subplots sharing the same t axis
  fig, (ax_v, ax_omega) = plotter.subplots(2, sharex=True)
  plotter.ion()
  plotter.show()
  

  r = rospy.Rate(2) # define rate here
  while not rospy.is_shutdown():
    
    t = []
    v = []
    omega = []
    
    for point in trajectory:
      t.append(point.time_from_start.to_sec())
      v.append(point.velocity.linear.x)
      omega.append(point.velocity.angular.z)
          
    plot_velocity_profile(fig, ax_v, ax_omega, np.asarray(t), np.asarray(v), np.asarray(omega))
        
    r.sleep()

if __name__ == '__main__': 
  try:
    trajectory = []
    velocity_plotter()
  except rospy.ROSInterruptException:
    pass

启动之后很消耗计算资源

python使用matplotlib将当前选出来的轨迹的y轴线速度和z轴角速度 。

三、在move_base中使用

在movebase中的配置很简单，将插件替换为teb即可。

        <!--  ************** Navigation ***************  -->
	<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
  	  	<rosparam file="$(find teb_local_planner_tutorials)/cfg/diff_drive/costmap_common_params.yaml" command="load" ns="global_costmap" />
  	 	<rosparam file="$(find teb_local_planner_tutorials)/cfg/diff_drive/costmap_common_params.yaml" command="load" ns="local_costmap" />
  		<rosparam file="$(find teb_local_planner_tutorials)/cfg/diff_drive/local_costmap_params.yaml" command="load" />
  		<rosparam file="$(find teb_local_planner_tutorials)/cfg/diff_drive/global_costmap_params.yaml" command="load" />
  		<rosparam file="$(find teb_local_planner_tutorials)/cfg/diff_drive/teb_local_planner_params.yaml" command="load" />

		<param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
		<param name="controller_frequency" value="5.0" />
		<param name="controller_patience" value="15.0" />
	</node>

关键的是其中的参数配置：

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.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
 complete_global_plan: True
    
 # Obstacles
    
 min_obstacle_dist: 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

---

下一章：局部路径规划器teb_local_planner详解2：关于避障