teb教程2

Posted gary-guo

tags:

篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了teb教程2相关的知识,希望对你有一定的参考价值。

http://wiki.ros.org/teb_local_planner/Tutorials/Inspect%20optimization%20feedback

检查优化反馈

简介:怎样检查优化的轨迹反馈,例如可视化选择的优化轨迹的速度分布

对于进一步参数调试或者评价目的,更感兴趣的是更够访问内部优化状态比如包括实时的状态。因此teb_local_planner提供了一个信息teb_local_planner/FeedbackMsg,其包含了内部所有的状态以及一些推断的变量(如速度分布)。对于加速度分布当前是空的。该消息也包含了在拓扑结构中所有可替代的轨迹。当前可选择的轨迹索引被存储在变量selected_trajectory_idx中。

反馈的topic可以被任何节点订阅,可用于数据输出到文件,或者自定义的可视化。

默认情况下,反馈消息被关闭了,以便减少计算资源。可以通过参数服务器变量publish_feedback设置为真,或者通过rqt_reconfigure来使能。

 

以下代码用于订阅test_optim_node节点发布的速度相关信息,并通过plots可视化出来,plots依赖*pypose*。

技术图片
   1 #!/usr/bin/env python
   2 
   3 import rospy, math
   4 from teb_local_planner.msg import FeedbackMsg, TrajectoryMsg, TrajectoryPointMsg
   5 from geometry_msgs.msg import PolygonStamped, Point32
   6 import numpy as np
   7 import matplotlib.pyplot as plotter
   8 
   9 def feedback_callback(data):
  10   global trajectory
  11 
  12   if not data.trajectories: # empty
  13     trajectory = []
  14     return
  15   trajectory = data.trajectories[data.selected_trajectory_idx].trajectory
  16   
  17   
  18 def plot_velocity_profile(fig, ax_v, ax_omega, t, v, omega):
  19   ax_v.cla()
  20   ax_v.grid()
  21   ax_v.set_ylabel(Trans. velocity [m/s])
  22   ax_v.plot(t, v, -bx)
  23   ax_omega.cla()
  24   ax_omega.grid()
  25   ax_omega.set_ylabel(Rot. velocity [rad/s])
  26   ax_omega.set_xlabel(Time [s])
  27   ax_omega.plot(t, omega, -bx)
  28   fig.canvas.draw()
  29 
  30   
  31   
  32 def velocity_plotter():
  33   global trajectory
  34   rospy.init_node("visualize_velocity_profile", anonymous=True)
  35   
  36   topic_name = "/test_optim_node/teb_feedback" # define feedback topic here!
  37   rospy.Subscriber(topic_name, FeedbackMsg, feedback_callback, queue_size = 1) 
  38 
  39   rospy.loginfo("Visualizing velocity profile published on ‘%s‘.",topic_name) 
  40   rospy.loginfo("Make sure to enable rosparam ‘publish_feedback‘ in the teb_local_planner.")
  41 
  42   # two subplots sharing the same t axis
  43   fig, (ax_v, ax_omega) = plotter.subplots(2, sharex=True)
  44   plotter.ion()
  45   plotter.show()
  46   
  47 
  48   r = rospy.Rate(2) # define rate here
  49   while not rospy.is_shutdown():
  50     
  51     t = []
  52     v = []
  53     omega = []
  54     
  55     for point in trajectory:
  56       t.append(point.time_from_start.to_sec())
  57       v.append(point.velocity.linear.x)
  58       omega.append(point.velocity.angular.z)
  59           
  60     plot_velocity_profile(fig, ax_v, ax_omega, np.asarray(t), np.asarray(v), np.asarray(omega))
  61         
  62     r.sleep()
  63 
  64 if __name__ == __main__: 
  65   try:
  66     trajectory = []
  67     velocity_plotter()
  68   except rospy.ROSInterruptException:
  69     pass
View Code

该代码可在teb_local_planner_tutorials的visualize_velocity_profile.py中找到,

运行过程

技术图片
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
View Code

以及结果如下

技术图片

 

以上是关于teb教程2的主要内容,如果未能解决你的问题,请参考以下文章

teb教程5

访问 x64 TEB C++ 和汇编

VIM 代码片段插件 ultisnips 使用教程

ROS局部运动规划器Teb/DWA

markdown 打字稿...编码说明,提示,作弊,指南,代码片段和教程文章

teb切换