def ekf_callback(msg):
print "ekf callback"
x_ekf = pose_point.pose.position.x
y_ekf = pose_point.pose.position.y
rot_q = msg.pose.orientation
(roll, pitch, theta_ekf) = euler_fq([rot_q.x, rot_q.y, rot_q.z, rot_q.w])
def newOdom(msg):
global x_odom
global y_odom
global theta_odom
x_odom = msg.pose.pose.position.x
y_odom = msg.pose.pose.position.y
rot_q = msg.pose.pose.orientation
(roll, pitch, theta_odom) = euler_fq([rot_q.x, rot_q.y, rot_q.z, rot_q.w])
def newPos(msg):
print("newPos Callback")
global x
global y
global theta
x = msg.pose.position.x
y = msg.pose.position.y
rot_q = msg.pose.orientation
(roll, pitch, theta) = euler_fq([rot_q.x, rot_q.y, rot_q.z, rot_q.w])
def path_cb(msg):
global xValue
global yValue
global thetaValue
xValue = []
yValue = []
thetaValue = []
for i in msg.poses:
xValue.append(i.pose.position.x)
yValue.append(i.pose.position.y)
(roll, pitch, theta_path) = euler_fq([
i.pose.orientation.x, i.pose.orientation.y, i.pose.orientation.z,
i.pose.orientation.w
])
thetaValue.append(theta_path)
def path_cb2(msg):
global xValue
global yValue
global thetaValue
xValue = []
yValue = []
thetaValue = []
print("path_cb2 Callback")
poses = msg.poses
# self.xValue = poses[0].position.x
# self.yValue = poses[0].position.y
# rot_q = poses[0].orientation
# (roll, pitch, theta_pos) = euler_from_quaternion([rot_q.x, rot_q.y, rot_q.z, rot_q.w])
# self.thetaValue = theta_pos
for i in range(1, len(poses)):
xValue.append(poses[i].position.x)
yValue.append(poses[i].position.y)
rot_q = poses[i].orientation
(roll, pitch,
theta_pos) = euler_fq([rot_q.x, rot_q.y, rot_q.z, rot_q.w])
thetaValue.append(theta_pos)
