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)