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轴角速度 。
在movebase中的配置很简单,将插件替换为teb即可。
<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:关于避障