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  topic_name = rospy.get_param('~feedback_topic', topic_name)
43  rospy.Subscriber(topic_name, FeedbackMsg, feedback_callback, queue_size = 1) # define feedback topic here!
44 
45  rospy.loginfo("Visualizing velocity profile published on '%s'.",topic_name)
46  rospy.loginfo("Make sure to enable rosparam 'publish_feedback' in the teb_local_planner.")
47 
48  # two subplots sharing the same t axis
49  fig, (ax_v, ax_omega) = plotter.subplots(2, sharex=True)
50  plotter.ion()
51  plotter.show()
52 
53 
54  r = rospy.Rate(2) # define rate here
55  while not rospy.is_shutdown():
56 
57  t = []
58  v = []
59  omega = []
60 
61  for point in trajectory:
62  t.append(point.time_from_start.to_sec())
63  v.append(point.velocity.linear.x)
64  omega.append(point.velocity.angular.z)
65 
66  plot_velocity_profile(fig, ax_v, ax_omega, np.asarray(t), np.asarray(v), np.asarray(omega))
67 
68  r.sleep()
69 
70 if __name__ == '__main__':
71  try:
72  trajectory = []
74  except rospy.ROSInterruptException:
75  pass
76 
def plot_velocity_profile(fig, ax_v, ax_omega, t, v, omega)


teb_local_planner
Author(s): Christoph Rösmann
autogenerated on Wed Jun 3 2020 04:03:08