export_to_mat.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 exports data to a mat file.
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, Quaternion
11 from tf.transformations import euler_from_quaternion
12 import numpy as np
13 import scipy.io as sio
14 import time
15 
17  global got_data
18 
19  if not data.trajectories: # empty
20  trajectory = []
21  return
22 
23  if got_data:
24  return
25 
26  got_data = True
27 
28  # copy trajectory
29  trajectories = []
30  for traj in data.trajectories:
31  trajectory = []
32 # # store as struct and cell array
33 # for point in traj.trajectory:
34 # (roll,pitch,yaw) = euler_from_quaternion([point.pose.orientation.x,point.pose.orientation.y,point.pose.orientation.z,point.pose.orientation.w])
35 # pose = {'x': point.pose.position.x, 'y': point.pose.position.y, 'theta': yaw}
36 # velocity = {'v': point.velocity.linear.x, 'omega': point.velocity.angular.z}
37 # time_from_start = point.time_from_start.to_sec()
38 # trajectory.append({'pose': pose, 'velocity': velocity, 'time_from_start': time_from_start})
39 
40  # store as all-in-one mat
41  arr = np.zeros([6, len(traj.trajectory)], dtype='double'); # x, y, theta, v, omega, t
42  for index, point in enumerate(traj.trajectory):
43  arr[0,index] = point.pose.position.x
44  arr[1,index] = point.pose.position.y
45  (roll,pitch,yaw) = euler_from_quaternion([point.pose.orientation.x,point.pose.orientation.y,point.pose.orientation.z,point.pose.orientation.w])
46  arr[2,index] = yaw
47  arr[3,index] = point.velocity.linear.x
48  arr[4,index] = point.velocity.angular.z
49  arr[5,index] = point.time_from_start.to_sec()
50 
51 # trajectories.append({'raw': trajectory, 'mat': arr})
52  trajectories.append({'data': arr, 'legend': ['x','y','theta','v','omega','t']})
53 
54  # copy obstacles
55  obstacles = []
56  for obst_id, obst in enumerate(data.obstacle_msg.obstacles):
57  #polygon = []
58  #for point in obst.polygon.points:
59  # polygon.append({'x': point.x, 'y': point.y, 'z': point.z})
60  obst_arr = np.zeros([4, len(obst.polygon.points)], dtype='double'); # x, y
61  for index, point in enumerate(obst.polygon.points):
62  obst_arr[0, index] = point.x
63  obst_arr[1, index] = point.y
64  obst_arr[2, index] = data.obstacle_msg.velocities[obst_id].twist.linear.x
65  obst_arr[3, index] = data.obstacle_msg.velocities[obst_id].twist.linear.y
66 
67  #obstacles.append(polygon)
68  obstacles.append({'data': obst_arr, 'legend': ['x','y', 'v_x', 'v_y']})
69 
70 
71  # create main struct:
72  mat = {'selected_trajectory_idx': data.selected_trajectory_idx, 'trajectories': trajectories, 'obstacles': obstacles}
73 
74  timestr = time.strftime("%Y%m%d_%H%M%S")
75  filename = '/home/albers/MasterThesis/Matlab/Homotopie/test_optim_node/' + 'teb_data_' + timestr + '.mat'
76 
77  rospy.loginfo("Saving mat-file '%s'.", filename)
78  sio.savemat(filename, mat)
79 
80 
81 
82 
83 
85  global got_data
86 
87  rospy.init_node("export_to_mat", anonymous=True)
88 
89 
90  topic_name = "/test_optim_node/teb_feedback" # define feedback topic here!
91 
92  rospy.Subscriber(topic_name, FeedbackMsg, feedback_callback, queue_size = 1)
93 
94  rospy.loginfo("Waiting for feedback message on topic %s.", topic_name)
95 
96  r = rospy.Rate(2) # define rate here
97  while not rospy.is_shutdown():
98 
99  if got_data:
100  rospy.loginfo("Data export completed.")
101  return
102 
103  r.sleep()
104 
105 if __name__ == '__main__':
106  try:
107  global got_data
108  got_data = False
110  except rospy.ROSInterruptException:
111  pass
112 
def feedback_callback(data)
def feedback_exporter()


teb_local_planner
Author(s): Christoph Rösmann
autogenerated on Wed Jun 5 2019 19:25:10