export_to_mat.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 exports data to a mat file.
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, Quaternion
00011 from tf.transformations import euler_from_quaternion
00012 import numpy as np
00013 import scipy.io as sio
00014 import time
00015 
00016 def feedback_callback(data):
00017   global got_data
00018 
00019   if not data.trajectories: # empty
00020     trajectory = []
00021     return
00022   
00023   if got_data:          
00024     return
00025 
00026   got_data = True
00027   
00028   # copy trajectory
00029   trajectories = []
00030   for traj in data.trajectories:
00031     trajectory = []
00032 #    # store as struct and cell array
00033 #    for point in traj.trajectory:
00034 #      (roll,pitch,yaw) = euler_from_quaternion([point.pose.orientation.x,point.pose.orientation.y,point.pose.orientation.z,point.pose.orientation.w])
00035 #      pose = {'x': point.pose.position.x, 'y': point.pose.position.y, 'theta': yaw}
00036 #      velocity = {'v': point.velocity.linear.x, 'omega': point.velocity.angular.z}
00037 #      time_from_start = point.time_from_start.to_sec()
00038 #      trajectory.append({'pose': pose, 'velocity': velocity, 'time_from_start': time_from_start})
00039       
00040     # store as all-in-one mat
00041     arr = np.zeros([6, len(traj.trajectory)], dtype='double'); # x, y, theta, v, omega, t
00042     for index, point in enumerate(traj.trajectory):
00043       arr[0,index] = point.pose.position.x
00044       arr[1,index] = point.pose.position.y
00045       (roll,pitch,yaw) = euler_from_quaternion([point.pose.orientation.x,point.pose.orientation.y,point.pose.orientation.z,point.pose.orientation.w])
00046       arr[2,index] = yaw
00047       arr[3,index] = point.velocity.linear.x
00048       arr[4,index] = point.velocity.angular.z
00049       arr[5,index] = point.time_from_start.to_sec()
00050       
00051 #   trajectories.append({'raw': trajectory, 'mat': arr})
00052     trajectories.append({'data': arr, 'legend': ['x','y','theta','v','omega','t']})
00053   
00054   # copy obstacles
00055   obstacles = []
00056   for obst in data.obstacles:
00057     #polygon = []
00058     #for point in obst.polygon.points:
00059     #  polygon.append({'x': point.x, 'y': point.y, 'z': point.z})
00060     obst_arr = np.zeros([2, len(obst.polygon.points)], dtype='double'); # x, y
00061     for index, point in enumerate(obst.polygon.points):
00062       obst_arr[0, index] = point.x
00063       obst_arr[1, index] = point.y
00064     #obstacles.append(polygon)
00065     obstacles.append({'data': obst_arr, 'legend': ['x','y']})
00066   
00067   
00068   # create main struct:
00069   mat = {'selected_trajectory_idx': data.selected_trajectory_idx, 'trajectories': trajectories, 'obstacles': obstacles}
00070 
00071   timestr = time.strftime("%Y%m%d_%H%M%S")
00072   filename = "teb_data_" + timestr + '.mat'
00073   
00074   rospy.loginfo("Saving mat-file '%s'.", filename)
00075   sio.savemat(filename, mat)
00076   
00077 
00078   
00079   
00080   
00081 def feedback_exporter():
00082   global got_data
00083 
00084   rospy.init_node("export_to_mat", anonymous=True)
00085   
00086   
00087   topic_name = "/test_optim_node/teb_feedback" # define feedback topic here!
00088 
00089   rospy.Subscriber(topic_name, FeedbackMsg, feedback_callback, queue_size = 1) 
00090 
00091   rospy.loginfo("Waiting for feedback message on topic %s.", topic_name)
00092  
00093   r = rospy.Rate(2) # define rate here
00094   while not rospy.is_shutdown():
00095     
00096     if got_data:
00097       rospy.loginfo("Data export completed.")
00098       return
00099 
00100     r.sleep()
00101 
00102 if __name__ == '__main__': 
00103   try:
00104     global got_data
00105     got_data = False
00106     feedback_exporter()
00107   except rospy.ROSInterruptException:
00108     pass
00109 


teb_local_planner
Author(s): Christoph Rösmann
autogenerated on Sat Jun 8 2019 20:21:34