Home » TEB轨迹规划算法教程 » TEB轨迹规划算法教程-优化反馈

TEB轨迹规划算法教程-优化反馈

TEB轨迹规划算法教程-优化反馈

说明:

  • 介绍如何检查优化轨迹的反馈; 提供了一个示例,其可视化当前所选轨迹的速度分布
  • 在前面的示例中,您学习了如何测试优化,如何调整参数以及如何在rviz中可视化生成的轨迹。
  • 然而,在rviz中可视化的轨迹不包含任何时间信息,仅显示其空间状态。
  • 为了进一步参数调整或评估目的,可能有兴趣获得对底层优化状态的访问(涉及时间优化状态)。
  • 因此teb_local_planner提供反馈消息teb_local_planner/FeedbackMsg,其包含所有内部状态和一些推断变量(如速度曲线)。
  • 请注意,加速度曲线当前为空。
  • 此外,该消息包含在独特拓扑中找到的所有备选轨迹。
  • 当前选择的轨迹索引存储在变量selected_trajectory_idx中。
  • 反馈主题(参见规划器API)可以由任何节点订阅(例如,将数据导出到文件,编写一些自定义可视化,......)。

注意:

  • 默认情况下关闭发送反馈消息以减轻计算负担。
  • 可以通过将rosparam ~/publish_feedback设置为true或通过调用rqt_reconfigure来启用它。

可视化速度曲线

  • 在下文中,提供了一个小的python脚本,它订阅了上一个教程中介绍的test_optim_node。
  • 脚本取决于pypose以创建绘图。
  • 仅显示当前所选轨迹的速度分布。
  • 脚本如下:
#!/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
  • 该脚本已作为visualize_velocity_profile.py包含在teb_local_planner_tutorials包中。
  • 假设roscore已经运行,可以将速度可视化如下:
rosparam set /test_optim_node/publish_feedback true # or use rqt_reconfigure later
roslaunch teb_local_planner test_optim_node.launch
rosrun teb_local_planner_tutorials visualize_velocity_profile.py # or call your own script here
  • 您应该能够在运行时检查速度曲线,如下图所示:

请输入图片描述

纠错,疑问,交流: 请进入讨论区点击加入Q群

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号


标签: teb轨迹规划算法教程