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
13 import scipy.io
as sio
19 if not data.trajectories:
30 for traj
in data.trajectories:
41 arr = np.zeros([6, len(traj.trajectory)], dtype=
'double');
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])
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()
52 trajectories.append({
'data': arr,
'legend': [
'x',
'y',
'theta',
'v',
'omega',
't']})
56 for obst
in data.obstacles:
60 obst_arr = np.zeros([2, len(obst.polygon.points)], dtype=
'double');
61 for index, point
in enumerate(obst.polygon.points):
62 obst_arr[0, index] = point.x
63 obst_arr[1, index] = point.y
65 obstacles.append({
'data': obst_arr,
'legend': [
'x',
'y']})
69 mat = {
'selected_trajectory_idx': data.selected_trajectory_idx,
'trajectories': trajectories,
'obstacles': obstacles}
71 timestr = time.strftime(
"%Y%m%d_%H%M%S")
72 filename =
"teb_data_" + timestr +
'.mat' 74 rospy.loginfo(
"Saving mat-file '%s'.", filename)
75 sio.savemat(filename, mat)
84 rospy.init_node(
"export_to_mat", anonymous=
True)
87 topic_name =
"/test_optim_node/teb_feedback" 89 rospy.Subscriber(topic_name, FeedbackMsg, feedback_callback, queue_size = 1)
91 rospy.loginfo(
"Waiting for feedback message on topic %s.", topic_name)
94 while not rospy.is_shutdown():
97 rospy.loginfo(
"Data export completed.")
102 if __name__ ==
'__main__':
107 except rospy.ROSInterruptException:
def feedback_callback(data)