visualize_velocity_profile.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # This small script subscribes to the FeedbackMsg message of teb_local_planner
00004 # and plots the current velocity.
00005 # publish_feedback must be turned on such that the planner publishes this information.
00006 # Author: christoph.roesmann@tu-dortmund.de
00007 
00008 import rospy, math
00009 from teb_local_planner.msg import FeedbackMsg, TrajectoryMsg, TrajectoryPointMsg
00010 from geometry_msgs.msg import PolygonStamped, Point32
00011 import numpy as np
00012 import matplotlib.pyplot as plotter
00013 
00014 def feedback_callback(data):
00015   global trajectory
00016 
00017   if not data.trajectories: # empty
00018     trajectory = []
00019     return
00020   trajectory = data.trajectories[data.selected_trajectory_idx].trajectory
00021   
00022   
00023 def plot_velocity_profile(fig, ax_v, ax_omega, t, v, omega):
00024   ax_v.cla()
00025   ax_v.grid()
00026   ax_v.set_ylabel('Trans. velocity [m/s]')
00027   ax_v.plot(t, v, '-bx')
00028   ax_omega.cla()
00029   ax_omega.grid()
00030   ax_omega.set_ylabel('Rot. velocity [rad/s]')
00031   ax_omega.set_xlabel('Time [s]')
00032   ax_omega.plot(t, omega, '-bx')
00033   fig.canvas.draw()
00034 
00035   
00036   
00037 def velocity_plotter():
00038   global trajectory
00039   rospy.init_node("visualize_velocity_profile", anonymous=True)
00040   
00041   topic_name = "/test_optim_node/teb_feedback"
00042   rospy.Subscriber(topic_name, FeedbackMsg, feedback_callback, queue_size = 1) # define feedback topic here!
00043 
00044   rospy.loginfo("Visualizing velocity profile published on '%s'.",topic_name) 
00045   rospy.loginfo("Make sure to enable rosparam 'publish_feedback' in the teb_local_planner.")
00046 
00047   # two subplots sharing the same t axis
00048   fig, (ax_v, ax_omega) = plotter.subplots(2, sharex=True)
00049   plotter.ion()
00050   plotter.show()
00051   
00052 
00053   r = rospy.Rate(2) # define rate here
00054   while not rospy.is_shutdown():
00055     
00056     t = []
00057     v = []
00058     omega = []
00059     
00060     for point in trajectory:
00061       t.append(point.time_from_start.to_sec())
00062       v.append(point.velocity.linear.x)
00063       omega.append(point.velocity.angular.z)
00064           
00065     plot_velocity_profile(fig, ax_v, ax_omega, np.asarray(t), np.asarray(v), np.asarray(omega))
00066         
00067     r.sleep()
00068 
00069 if __name__ == '__main__': 
00070   try:
00071     trajectory = []
00072     velocity_plotter()
00073   except rospy.ROSInterruptException:
00074     pass
00075 


teb_local_planner
Author(s): Christoph Rösmann
autogenerated on Mon Oct 24 2016 05:31:15