Exemplo n.º 1
0
 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
Exemplo n.º 2
0
 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