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: