rqt teb参数动态调试工具_teb教程2

rqt teb参数动态调试工具_teb教程2http://wiki.ros.org/teb_local_planner/Tutorials/Inspect%20optimization%20feedback检查优化反馈简介:怎样检查优化的轨迹反馈,例如可视化选择的优化轨迹的速度分布对于进一步参数调试或者评价目的,更感兴趣的是更够访问内部优化状态比如包括实时的状态。因此teb_local_planner提供了一个信息teb_local_pla…

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*。

rqt teb参数动态调试工具_teb教程2

rqt teb参数动态调试工具_teb教程2

1 #!/usr/bin/env python2

3import rospy, math4 fromteb_local_planner.msg import FeedbackMsg, TrajectoryMsg, TrajectoryPointMsg5 fromgeometry_msgs.msg import PolygonStamped, Point326 import numpy asnp7 import matplotlib.pyplot asplotter8

9def feedback_callback(data):10 globaltrajectory11

12 ifnot data.trajectories: # empty13 trajectory =[]14 return

15 trajectory =data.trajectories[data.selected_trajectory_idx].trajectory16

17

18def plot_velocity_profile(fig, ax_v, ax_omega, t, v, omega):19ax_v.cla()20ax_v.grid()21 ax_v.set_ylabel(‘Trans. velocity [m/s]’)22 ax_v.plot(t, v, ‘-bx’)23ax_omega.cla()24ax_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’)28fig.canvas.draw()29

30

31

32def velocity_plotter():33 globaltrajectory34 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 axis43 fig, (ax_v, ax_omega) = plotter.subplots(2, sharex=True)44plotter.ion()45plotter.show()46

47

48 r = rospy.Rate(2) # define rate here49 whilenot rospy.is_shutdown():50

51 t =[]52 v =[]53 omega =[]54

55 for point intrajectory:56t.append(point.time_from_start.to_sec())57v.append(point.velocity.linear.x)58omega.append(point.velocity.angular.z)59

60plot_velocity_profile(fig, ax_v, ax_omega, np.asarray(t), np.asarray(v), np.asarray(omega))61

62r.sleep()63

64 if __name__ == ‘__main__’:65 try:66 trajectory =[]67velocity_plotter()68except rospy.ROSInterruptException:69 pass

View Code

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

运行过程

rqt teb参数动态调试工具_teb教程2

rqt teb参数动态调试工具_teb教程2

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

以及结果如下

rqt teb参数动态调试工具_teb教程2

今天的文章rqt teb参数动态调试工具_teb教程2分享到此就结束了,感谢您的阅读,如果确实帮到您,您可以动动手指转发给其他人。

版权声明:本文内容由互联网用户自发贡献,该文观点仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容, 请发送邮件至 举报,一经查实,本站将立刻删除。
如需转载请保留出处:https://bianchenghao.cn/29464.html

(0)
编程小号编程小号

相关推荐

发表回复

您的电子邮箱地址不会被公开。 必填项已用*标注