# Load Odometry Data
td.addLabelingIncremental("pos", 3)
td.addLabelingIncremental("att", 4)
td.reInit()
RosDataAcquisition.rosBagLoadOdometry(odometryBag, odometryTopic, td, "pos", "att")
# Load Timestamps into new td
td_aligned.addLabelingIncremental("pos", 3)
td_aligned.addLabelingIncremental("att", 4)
td_aligned.reInit()
RosDataAcquisition.rosBagLoadTimestampsOnly(timestampBag, timestampTopic, td_aligned)
# Interpolate Data
td.interpolateColumns(td_aligned, "pos", "pos")
td.interpolateQuaternion(td_aligned, "att", "att")
# Plotting
plotterPos = Plotter(-1, [3, 1], "Position", ["", "", "time[s]"], ["x[m]", "y[m]", "z[m]"], 10000)
plotterAtt = Plotter(-1, [4, 1], "Attitude", ["", "", "", "time[s]"], ["w", "x", "y", "z"], 10000)
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"], ["", "", "", ""])
# 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"], ["", "", ""])
td.addLabelingIncremental('pos', 3)
td.addLabelingIncremental('att', 4)
td.reInit()
RosDataAcquisition.rosBagLoadOdometry(odometryBag, odometryTopic, td, 'pos',
'att')
# Load Timestamps into new td
td_aligned.addLabelingIncremental('pos', 3)
td_aligned.addLabelingIncremental('att', 4)
td_aligned.reInit()
RosDataAcquisition.rosBagLoadTimestampsOnly(timestampBag, timestampTopic,
td_aligned)
# Interpolate Data
td.interpolateColumns(td_aligned, 'pos', 'pos')
td.interpolateQuaternion(td_aligned, 'att', 'att')
# Plotting
plotterPos = Plotter(-1, [3, 1], 'Position', ['', '', 'time[s]'],
['x[m]', 'y[m]', 'z[m]'], 10000)
plotterAtt = Plotter(-1, [4, 1], 'Attitude', ['', '', '', 'time[s]'],
['w', 'x', 'y', 'z'], 10000)
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'], ['', '', '', ''])
