def imu_callback(self,msg): (roll,pitch,yaw) = euler_from_quaternion([msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w]) #yaw += pi/2.0 # why???????? yaw *= -1.0 # apply correction for magnetic declination yaw += self.declination #if yaw >= 2*pi: # yaw -= 2*pi #elif yaw < 0: # yaw += 2*pi #print str(yaw/(2*pi)*360) if len(self.heading) > 0 and len(self.heading) >= self.buff_len: self.heading.pop(0) self.heading.append(yaw) mmsg = HeadingMsg() if self.buff_len > 1: param = norm.fit(self.heading) mmsg.mean = param[0] mmsg.stddev = param[1] else: mmsg.mean = yaw mmsg.stddev = 0.0 self.pub.publish(mmsg) if self.published is False: rospy.loginfo("Publishing robot heading.") self.published = True
def imu_callback(self,msg): (roll,pitch,yaw) = euler_from_quaternion([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]) #yaw += pi # why???????? # apply correction for magnetic declination yaw += self.declination if yaw >= 2*pi: yaw -= 2*pi elif yaw < 0: yaw += 2*pi if len(self.heading) > 0 and len(self.heading) >= self.buff_len: self.heading.pop(0) self.heading.append(yaw) param = norm.fit(self.heading) mmsg = HeadingMsg() mmsg.mean = param[0] mmsg.stddev = param[1] self.pub.publish(mmsg) if self.published is False: rospy.loginfo("Publishing robot heading.") self.published = True