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


teb_local_planner_tutorials
Author(s): Christoph Rösmann
autogenerated on Thu Jun 6 2019 19:31:31