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