# td_rovio.d[:, attIds[1:]] *= -1 # print(td_rovio.d.shape) rovioEvaluator.getAllDerivatives() rovioEvaluator.alignTime() rovioEvaluator.alignBodyFrame() rovioEvaluator.alignInertialFrame() rovioEvaluator.getYpr() rovioEvaluator.evaluateSigmaBounds() td_rovio.computeRMS('pos', 'att', td_vicon, 'pos', 'att', 0.0) if plotPos and draw_fig: # Position plotting plotterPos = Plotter(-1, [3, 1], 'Position', ['', '', 'time[s]'], ['x[m]', 'y[m]', 'z[m]'], 10000) if rovioEvaluator.doCov: plotterPos.addDataToSubplotMultiple(td_rovio, 'posSm', [1, 2, 3], ['r--', 'r--', 'r--'], ['', '', '']) plotterPos.addDataToSubplotMultiple(td_rovio, 'posSp', [1, 2, 3], ['r--', 'r--', 'r--'], ['', '', '']) plotterPos.addDataToSubplotMultiple(td_rovio, 'pos', [1, 2, 3], ['r', 'r', 'r'], ['', '', '']) plotterPos.addDataToSubplotMultiple(td_vicon, 'pos', [1, 2, 3], ['b', 'b', 'b'], ['', '', '']) if plotVel and draw_fig: # Velocity plotting plotterVel = Plotter(-1, [3, 1], 'Robocentric Velocity', ['', '', 'time[s]'], ['$v_x$[m/s]', '$v_y$[m/s]', '$v_z$[m/s]'], 10000) plotterVel.addDataToSubplotMultiple(td_rovio, 'vel', [1, 2, 3], ['r', 'r', 'r'], ['', '', ''])
td_aligned.reInit() RosDataAcquisition.rosBagLoadTimestampsOnly(timestampBag,timestampTopic,td_aligned) # Interpolate Data td.interpolateColumns(td_aligned, 'pos', 'pos') td.interpolateQuaternion(td_aligned, 'att', 'att') ind, = np.nonzero(np.fabs(np.diff(td.col(1))) < 1e-10) print(ind) ind, = np.nonzero(np.fabs(np.diff(td_aligned.col(1))) < 1e-10) print(ind) # Plotting plotterPos = Plotter(-1, [3,1],'Position',['','','time[s]'],['x[m]','y[m]','z[m]'],150000) plotterAtt = Plotter(-1, [4,1],'Attitude',['','','','time[s]'],['w','x','y','z'],150000) plotterPos.addDataToSubplotMultiple(td, 'pos', [1,2,3], ['b','b','b'], ['','','']) plotterAtt.addDataToSubplotMultiple(td, 'att', [1,2,3,4], ['b','b','b','b'], ['','','','']) plotterPos.addDataToSubplotMultiple(td_aligned, 'pos', [1,2,3], ['g','g','g'], ['','','']) plotterAtt.addDataToSubplotMultiple(td_aligned, 'att', [1,2,3,4], ['g','g','g','g'], ['','','','']) plotterVel = Plotter(-1, [3,1],'Velocity',['','','time[s]'],['x[m]','y[m]','z[m]'],150000) plotterVel.addDataToSubplotMultiple(td, 'vel', [1,2,3], ['b','b','b'], ['','','']) # Apply body and inertial if bodyTranslation != None and bodyRotation != None: td_aligned.applyBodyTransform('pos', 'att', bodyTranslation, bodyRotation) if inertialTranslation != None and inertialRotation != None: td_aligned.applyBodyTransform('pos', 'att', inertialTranslation, inertialRotation) plotterPos.addDataToSubplotMultiple(td_aligned, 'pos', [1,2,3], ['r','r','r'], ['','','']) plotterAtt.addDataToSubplotMultiple(td_aligned, 'att', [1,2,3,4], ['r','r','r','r'], ['','','',''])
rovioEvaluator.initTimedData(td_rovio) rovioEvaluator.initTimedDataGT(td_vicon) rovioEvaluator.acquireData() rovioEvaluator.acquireDataGT() rovioEvaluator.getAllDerivatives() rovioEvaluator.alignTime() rovioEvaluator.alignBodyFrame() rovioEvaluator.alignInertialFrame() rovioEvaluator.getYpr() rovioEvaluator.evaluateSigmaBounds() if plotPos: # Position plotting plotterPos = Plotter(-1, [3,1],'Position',['','','time[s]'],['x[m]','y[m]','z[m]'],10000) if rovioEvaluator.doCov: plotterPos.addDataToSubplotMultiple(td_rovio, 'posSm', [1,2,3], ['r--','r--','r--'], ['','','']) plotterPos.addDataToSubplotMultiple(td_rovio, 'posSp', [1,2,3], ['r--','r--','r--'], ['','','']) plotterPos.addDataToSubplotMultiple(td_rovio, 'pos', [1,2,3], ['r','r','r'], ['','','']) plotterPos.addDataToSubplotMultiple(td_vicon, 'pos', [1,2,3], ['b','b','b'], ['','','']) if plotVel: # Velocity plotting plotterVel = Plotter(-1, [3,1],'Robocentric Velocity',['','','time[s]'],['$v_x$[m/s]','$v_y$[m/s]','$v_z$[m/s]'],10000) plotterVel.addDataToSubplotMultiple(td_rovio, 'vel', [1,2,3], ['r','r','r'], ['','','']) plotterVel.addDataToSubplotMultiple(td_vicon, 'vel', [1,2,3], ['b','b','b'], ['','','']) if plotAtt: # Attitude plotting plotterAtt = Plotter(-1, [4,1],'Attitude Quaternion',['','','','time[s]'],['w[1]','x[1]','y[1]','z[1]'],10000) plotterAtt.addDataToSubplotMultiple(td_rovio, 'att', [1,2,3,4], ['r','r','r','r'], ['','','','']) plotterAtt.addDataToSubplotMultiple(td_vicon, 'att', [1,2,3,4], ['b','b','b','b'], ['','','','']) if plotYpr: # Yaw-pitch-roll plotting