def plot_axis_control(v, axis):
axismap = {'roll': (v.control.roll_cmd, 2, +1, v.asctec_ctrl_input.roll_cmd, -1),
'pitch': (v.control.pitch_cmd, 1, -1, v.asctec_ctrl_input.pitch_cmd, -1),
'yaw': (v.control.yaw_cmd, 0, -1, v.asctec_ctrl_input.yaw_rate_cmd, +1) # not sure about the multiplier here
}
control_mode_cmd, state_axis, imu_mult, asctec_cmd, asctec_cmd_mult = axismap[axis]
newfig("%s Axis Control" % axis.capitalize(), "time [s]", "%s [deg]" % axis.capitalize())
# np.clip() and the [1:] stuff in the following to attempt deal with bogus initial data points in IMU data:
plt.plot(v.control.t, control_mode_cmd, label='cmd (from mux)')
plt.plot(v.state.t[1:], v.state.ori_ypr[1:,state_axis], label='meas (Vicon)')
plt.plot(np.clip(v.imu.t[1:], 0, np.inf), imu_mult*v.imu.ori_ypr[1:,state_axis], label='meas (IMU)')
if axis is not 'yaw':
plt.plot(v.asctec_ctrl_input.t, asctec_cmd_mult*asctec_cmd, label='cmd (AscTec)')
# Plot difference between vicon and imu:
tout, data_out = uniform_resample((('linear', v.imu.t[1:], v.imu.ori_ypr[1:,state_axis]),
('linear', v.state.t[1:], v.state.ori_ypr[1:,state_axis])),
0.02)
plt.plot(tout, imu_mult*data_out[0][0] - data_out[1][0], label='IMU - Vicon')
plt.legend()
def process_data(self):
v = Dummy()
v.state = Dummy()
v.control = Dummy()
v.imu = Dummy()
v.asctec_ctrl_input = Dummy()
# kludge:
if 'odom' in self.input_bag.get_topics():
odom_topic = 'odom'
elif 'estimator/output' in self.input_bag.get_topics():
odom_topic = 'estimator/output'
# following should not be here!!!
elif 'dual_ekf_node/output' in self.input_bag.get_topics():
odom_topic = 'dual_ekf_node/output'
else:
raise "Can't find an odometry topic"
process_state(self.input_bag, odom_topic, v.state)
process_control_mode_output(self.input_bag, 'controller_mux/output', v.control)
process_imu(self.input_bag, 'asctec/imu', v.imu)
process_asctec_ctrl_input(self.input_bag, 'asctec/CTRL_INPUT', v.asctec_ctrl_input)
tout, data_out = uniform_resample((('linear', v.imu.t[1:], v.imu.ori_ypr[1:,0]),
('linear', v.state.t[1:], v.state.ori_ypr[1:,0])), 0.02)
self.v = v
# setup istart and iend for each group of data:
vars = (self.v.control, self.v.imu, self.v.asctec_ctrl_input, self.v.state)
if self.t_start is not None:
for w in vars:
w.istart = np.where(w.t >= self.t_start)[0][0]
else:
for w in vars:
w.istart = 1
if self.t_end is not None:
for w in vars:
w.iend = np.where(w.t >= self.t_end)[0][0]
else:
for w in vars:
w.iend = None
in_bag_fname = sys.argv[1]
input_bag = BagLoader(in_bag_fname)
v = Dummy()
v.state = Dummy()
v.control = Dummy()
v.imu = Dummy()
v.asctec_ctrl_input = Dummy()
process_state(input_bag, 'odom', v.state)
process_control_mode_output(input_bag, 'controller_mux/output', v.control)
process_imu(input_bag, 'asctec/imu', v.imu)
process_asctec_ctrl_input(input_bag, 'asctec/CTRL_INPUT', v.asctec_ctrl_input)
tout, data_out = uniform_resample((('linear', v.imu.t[1:], v.imu.ori_ypr[1:,0]),
('linear', v.state.t[1:], v.state.ori_ypr[1:,0])), 0.02)
def plot_axis_control(v, axis):
axismap = {'roll': (v.control.roll_cmd, 2, +1, v.asctec_ctrl_input.roll_cmd, -1),
'pitch': (v.control.pitch_cmd, 1, -1, v.asctec_ctrl_input.pitch_cmd, -1),
'yaw': (v.control.yaw_cmd, 0, -1, v.asctec_ctrl_input.yaw_rate_cmd, +1) # not sure about the multiplier here
}
control_mode_cmd, state_axis, imu_mult, asctec_cmd, asctec_cmd_mult = axismap[axis]
newfig("%s Axis Control" % axis.capitalize(), "time [s]", "%s [deg]" % axis.capitalize())
# np.clip() and the [1:] stuff in the following to attempt deal with bogus initial data points in IMU data:
plt.plot(v.control.t, control_mode_cmd, label='cmd (from mux)')
plt.plot(v.state.t[1:], v.state.ori_ypr[1:,state_axis], label='meas (Vicon)')
plt.plot(np.clip(v.imu.t[1:], 0, np.inf), imu_mult*v.imu.ori_ypr[1:,state_axis], label='meas (IMU)')
if axis is not 'yaw':
plt.plot(v.asctec_ctrl_input.t, asctec_cmd_mult*asctec_cmd, label='cmd (AscTec)')
# Plot difference between vicon and imu:
