def trajectory_cb(self, msg):
'''
Message structure:
- header: frame, timestamp
- waypoint[]:
- pose
- twist
'''
print msg
self.state_history = {
'position': deque(),
'linear_vel': deque(),
'orientation_q': deque(),
'angular_vel': deque(),
}
rospy.logwarn("Recieving trajectory!")
self.target_trajectory = {
'position': deque(),
'linear_vel': deque(),
'orientation_q': deque(),
'angular_vel': deque(),
}
for struct_waypoint in msg.trajectory:
# Deserialize to numpy
pose, twist = sub8_utils.posetwist_to_numpy(struct_waypoint)
(position, orientation_q), (linear_vel, angular_vel) = pose, twist
# TODO: Less repeated code
self.target_trajectory['position'].append(position)
self.target_trajectory['linear_vel'].append(linear_vel)
self.target_trajectory['orientation_q'].append(orientation_q)
self.target_trajectory['angular_vel'].append(angular_vel)
def trajectory_cb(self, msg):
'''
Message structure:
- header: frame, timestamp
- waypoint[]:
- pose
- twist
'''
print msg
self.state_history = {
'position': deque(),
'linear_vel': deque(),
'orientation_q': deque(),
'angular_vel': deque(),
}
rospy.logwarn("Recieving trajectory!")
self.target_trajectory = {
'position': deque(),
'linear_vel': deque(),
'orientation_q': deque(),
'angular_vel': deque(),
}
for struct_waypoint in msg.trajectory:
# Deserialize to numpy
pose, twist = sub8_utils.posetwist_to_numpy(struct_waypoint)
(position, orientation_q), (linear_vel, angular_vel) = pose, twist
# TODO: Less repeated code
self.target_trajectory['position'].append(position)
self.target_trajectory['linear_vel'].append(linear_vel)
self.target_trajectory['orientation_q'].append(orientation_q)
self.target_trajectory['angular_vel'].append(angular_vel)
def traj_cb(self, msg):
pose, twist = sub8_utils.posetwist_to_numpy(msg.trajectory[0])
self.target_state = pose[0]
