2022 IB Diploma Extended Essays

global f roll=0 pitch=0 yaw=0 quaternion = (

dataMsg.pose.pose.orientation.x, dataMsg.pose.pose.orientation.y, dataMsg.pose.pose.orientation.z, dataMsg.pose.pose.orientation.w) (roll, pitch, yaw) = euler_from_quaternion(quaternion) (sec,nsec)=(dataMsg.header.stamp.secs,dataMsg.header.stamp.nsecs) time = 1./1000000000 * nsec + sec (roll,pitch,yaw) = euler_from_quaternion(quaternion) pose_position.write("{:.9f} {:.9f} {:.9f} {:.9f}\n".format(time, dataMsg.pose.pose.position.x, dataMsg.pose.pose.position.y, dataMsg.pose.pose.position.z)) pose_orientation.write("{:.9f} {:.9f} {:.9f} {:.9f}\n".format(time, yaw, roll, pitch)) pose_covariance.write("{}\n".format(dataMsg.pose.covariance))

rospy.init_node("poseupdateMsgToTxt") sub = rospy.Subscriber('poseupdate', PoseWithCovarianceStamped, processPoseWithCovarianceStamped_message) rospy.spin() Plot pose #!/usr/bin/env python import matplotlib as mpl import matplotlib.pyplot as plt import numpy as np import rospkg from math import *

def f_plot(*args, **kwargs): xlist = [] ylist = [] for i, arg in enumerate(args): if (i % 2 == 0): xlist.append(arg) else: ylist.append(arg) colors = kwargs.pop('colors', 'k') linewidth = kwargs.pop('linewidth', 1.) fig = plt.figure() ax = fig.add_subplot(111) i = 0 for x, y, color in zip(xlist, ylist, colors):