下面的代码是干什么用的，请生成说明注释，：
【import rospy, math
from teb_local_planner.msg import FeedbackMsg, TrajectoryMsg, TrajectoryPointMsg
from geometry_msgs.msg import PolygonStamped, Point32, Quaternion
from tf.transformations import euler_from_quaternion
import numpy as np
import scipy.io as sio
import time
def feedback_callback(data):
global got_data
if not data.trajectories: # empty
trajectory = []
return
if got_data:
return
got_data = True
# copy trajectory
trajectories = []
for traj in data.trajectories:
trajectory = []
# # store as struct and cell array
# for point in traj.trajectory:
# (roll,pitch,yaw) = euler_from_quaternion([point.pose.orientation.x,point.pose.orientation.y,point.pose.orientation.z,point.pose.orientation.w])
# pose = {'x': point.pose.position.x, 'y': point.pose.position.y, 'theta': yaw}
# velocity = {'v': point.velocity.linear.x, 'omega': point.velocity.angular.z}
# time_from_start = point.time_from_start.to_sec()
# trajectory.append({'pose': pose, 'velocity': velocity, 'time_from_start': time_from_start})
# store as all-in-one mat
arr = np.zeros([6, len(traj.trajectory)], dtype='double'); # x, y, theta, v, omega, t
for index, point in enumerate(traj.trajectory):
arr[0,index] = point.pose.position.x
arr[1,index] = point.pose.position.y
(roll,pitch,yaw) = euler_from_quaternion([point.pose.orientation.x,point.pose.orientation.y,point.pose.orientation.z,point.pose.orientation.w])
arr[2,index] = yaw
arr[3,index] = point.velocity.linear.x
arr[4,index] = point.velocity.angular.z
arr[5,index] = point.time_from_start.to_sec()
# trajectories.append({'raw': trajectory, 'mat': arr})
trajectories.append({'data': arr, 'legend': ['x','y','theta','v','omega','t']})
# copy obstacles
obstacles = []
for obst_id, obst in enumerate(data.obstacle_msg.obstacles):
#polygon = []
#for point in obst.polygon.points:
# polygon.append({'x': point.x, 'y': point.y, 'z': point.z})
obst_arr = np.zeros([4, len(obst.polygon.points)], dtype='double'); # x, y
for index, point in enumerate(obst.polygon.points):
obst_arr[0, index] = point.x
obst_arr[1, index] = point.y
obst_arr[2, index] = data.obstacle_msg.velocities[obst_id].twist.linear.x
obst_arr[3, index] = data.obstacle_msg.velocities[obst_id].twist.linear.y
#obstacles.append(polygon)
obstacles.append({'data': obst_arr, 'legend': ['x','y', 'v_x', 'v_y']})
# create main struct:
mat = {'selected_trajectory_idx': data.selected_trajectory_idx, 'trajectories': trajectories, 'obstacles': obstacles}
timestr = time.strftime("%Y%m%d_%H%M%S")
filename = '/home/albers/MasterThesis/Matlab/Homotopie/test_optim_node/' + 'teb_data_' + timestr + '.mat'
rospy.loginfo("Saving mat-file '%s'.", filename)
sio.savemat(filename, mat)
def feedback_exporter():
global got_data
rospy.init_node("export_to_mat", anonymous=True)
topic_name = "/test_optim_node/teb_feedback" # define feedback topic here!
rospy.Subscriber(topic_name, FeedbackMsg, feedback_callback, queue_size = 1)
rospy.loginfo("Waiting for feedback message on topic %s.", topic_name)
r = rospy.Rate(2) # define rate here
while not rospy.is_shutdown():
if got_data:
rospy.loginfo("Data export completed.")
return
r.sleep()
if __name__ == '__main__':
try:
global got_data
got_data = False
feedback_exporter()
except rospy.ROSInterruptException:
pass】
最新发布
当Angular项目运行ng serve时遇到版本不一致警告，可通过两种方案解决：1) 将本地版本更新至全局版本；2) 将全局版本更新至本地版本。涉及操作包括卸载、清除缓存、安装指定版本和更新依赖。
最低0.47元/天 解锁文章
扫一扫
2154
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
