def write_quad_state_msg(self, pos, rot, vel):
"""Converts the local state of the quad into a 'QuadPositionDerived'
message from the mocap package.
Warning. 'QuadPositionDerived' was maybe a poorly chosen name, since
that type of message contains:
position as x,y,z
attitude as euler angles in degrees
velocity
angular velocity
acceleration
angular acceleration
However, I decided to stick with the original mocap settings inherited
by Matteo, so I did not change the name or format of that message.
Some field of the message will be left blank, since our simulators only
use position, attitude and velocity. It does not matter what will be put
in those fileds, since we will not use them.
"""
msg = mocap.msg.QuadPositionDerived()
msg.found_body = True
msg.x, msg.y, msg.z = pos
msg.roll, msg.pitch, msg.yaw = uf.rot_to_ea_deg(rot)
msg.x_vel, msg.y_vel, msg.z_vel = vel
return msg
def print_time_state(self):
print 'time: ' + str(self._time)
print 'pos: ' + str(tuple(self._pos))
print 'vel: ' + str(tuple(self._vel))
print 'roll, pitch, yaw: ' + str(tuple(uf.rot_to_ea_deg(self._rot)))
