标签：HERE 技术 tick display [] size 简介 ott mic
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
该代码可在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
以及结果如下
标签：HERE 技术 tick display [] size 简介 ott mic
原文地址：https://www.cnblogs.com/gary-guo/p/10893224.html
