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

    #!/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 = ) 

     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(, sharex=True)

     plotter.ion()

     plotter.show()

     r = rospy.Rate() # 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

该代码可在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

以及结果如下

巴特西

teb教程2

最新文章

热门文章