def process_control_mode_output(bag, topic, output_var):
output_var.t = get_header_time(bag, topic)
output_var.control_mode = bag._data[topic+'/control_mode']
(output_var.alt_cmd,
output_var.pitch_cmd,
output_var.roll_cmd,
output_var.yaw_cmd,
output_var.yaw_rate_cmd) = \
(bag._data[topic+'/%s_cmd' % ax] for ax in ('alt', 'pitch', 'roll', 'yaw', 'yaw_rate'))
def process_asctec_ctrl_input(bag, topic, output_var):
"""
"""
output_var.t = get_header_time(bag, topic)
output_var.roll_cmd, output_var.pitch_cmd, output_var.yaw_rate_cmd = \
np.array(tuple(bag._data[topic + '/%s' % ax]/scale
for ax, scale in (('roll', ROLL_SCALE),
('pitch', PITCH_SCALE),
('yaw', YAW_SCALE))
))
def process_state(bag, topic, output_var):
"""
Process the state data in a given (nav_msgs/Odom) topic and place results in the
north, east, up, {north,east,up}_vel, ori_quat and ori_ypr (yaw/pitch/roll) attributes
of the provided output_var argument
"""
output_var.t = get_header_time(bag, topic)
output_var.north, output_var.east, output_var.up = \
(mult*bag._data[topic+'/pose/pose/position/%s' % ax] for (ax, mult) in zip('xyz',(1.,1.,-1.)))
output_var.north_vel, output_var.east_vel, output_var.up_vel = (bag._data[topic+'/twist/twist/linear/%s' % ax] for ax in 'xyz')
output_var.ori_quat = np.array(tuple(bag._data[topic+'/pose/pose/orientation/%s' % ax] for ax in 'wxyz')).T
output_var.ori_ypr = np.degrees(np.array(quaternion_to_eulerypr(output_var.ori_quat.T)).T)
def process_imu(bag, topic, output_var):
"""
"""
output_var.t = get_header_time(bag, topic)
output_var.ori_quat = np.array(tuple(bag._data[topic+'/orientation/%s' % ax] for ax in 'wxyz')).T
output_var.ori_ypr = np.degrees(np.array(quaternion_to_eulerypr(output_var.ori_quat.T)).T)
def process_controller_status(bag, topic, output_var):
output_var.t = get_header_time(bag, topic)
output_var.active_mode = bag._data[topic+'/active_mode']
output_var.autoseq_idxs = np.where(np.array([{'autosequence':True}.get(x,False) for x in output_var.active_mode]))[0]