def getTwist(self):
        twist = super(MoveTo, self).getTwist()
        if twist == None:
            twist = Twist()
        targetPose = self._getTargetPose()
        c_pos = self.pose
        d_x = targetPose.position.x - c_pos.translation.x
        d_y = targetPose.position.y - c_pos.translation.y
        d_len = math.sqrt(d_x ** 2 + d_y ** 2)

        ##tft.euler_from_quaternion
        if d_len < self.target_tresh:
            print("close enough")
            return None
        t_q = targetPose.orientation
        c_q = c_pos.rotation
        t_euler = tft.euler_from_quaternion((t_q.x, t_q.y, t_q.z, t_q.w))
        c_euler = tft.euler_from_quaternion((c_q.x, c_q.y, c_q.z, c_q.w))
        d_rot = t_euler[2] - c_euler[2]
        # if d_rot > self.slow : #FIXME
        # print("small rotation")
        # return twist
        # if (abs(d_x)/abs(d_y)) < 1:
        #  print("small rotation")
        #  print(d_x)
        #  print(d_y)
        #  return twist
        twist.linear.x = self.speed  # TODO add acceleration
        # if twist == None && self.target:

        # if abs(twist.angular.z
        return twist

def odomCallback(data):
    global STATE
    if   STATE == 'waiting' or STATE == 'done':
        return
    elif STATE == 'faceUp':
        # Get current yaw in radians
        euler = euler_from_quaternion([data.pose.pose.orientation.x, data.pose.pose.orientation.y, data.pose.pose.orientation.z, data.pose.pose.orientation.w])
        yaw = euler[2]
        # Check if current yaw is up
        if ( abs(yaw-0.0) <= ANGULAR_THRESHOLD ):
            stop()
            STATE = 'turn'
    elif STATE == 'turn':
        # Get current yaw in radians
        euler = euler_from_quaternion([data.pose.pose.orientation.x, data.pose.pose.orientation.y, data.pose.pose.orientation.z, data.pose.pose.orientation.w])
        yaw = euler[2]
        # Check if in desired state
        if ( abs(yaw-desiredAngle) <= ANGULAR_THRESHOLD ):
            stop()
            STATE = 'goToX'
    elif STATE == 'goToX':
        xpos = #Get x pose.pose.position
        if ( abs(xpos-desiredXLocation) <= LINEAR_THRESHOLD ):
            stop()
            STATE = 'done'

    def receiveRealPose(self, data):
        
        try:
            (trans, rot) = self.listener.lookupTransform('world', 'minefield', rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
          return
	
	q = [rot[0], rot[1], rot[2], rot[3]]
	roll, pitch, yaw =  euler_from_quaternion(q)
        
        #frame = PyKDL.Frame(PyKDL.Rotation.RPY(roll, pitch, yaw), PyKDL.Vector(trans[0],trans[1],trans[2]))
        
	q = [data.pose.pose.orientation.x, data.pose.pose.orientation.y, data.pose.pose.orientation.z, data.pose.pose.orientation.w]
	roll, pitch, robot_yaw =  euler_from_quaternion(q)
  
	#robot = PyKDL.Frame(PyKDL.Rotation.RPY(roll, pitch, robot_yaw), PyKDL.Vector(data.pose.pose.position.x, data.pose.pose.position.y, data.pose.pose.position.z))

	#robot_transformed = frame * robot;
	
	#[roll, pitch, yaw] = robot_transformed.M.GetRPY()
	
	#print data.pose.pose.position.x - trans[0], data.pose.pose.position.y - trans[1], robot_yaw - yaw

	pose = [data.pose.pose.position.x - trans[0], data.pose.pose.position.y - trans[1], robot_yaw - yaw]
	self.updateRobotPose(pose)

def run1(data):
    global pointData, points, feedbackData
    if data.time == 0:
    	pointData = data
    elif abs(data.time) < 5:
        it = abs(data.time)
        data.x = feedbackData.feedback.base_position.pose.position.x
        data.y = feedbackData.feedback.base_position.pose.position.y
        data.z = data.time / abs(data.time)
        quaternion = (feedbackData.feedback.base_position.pose.orientation.x, feedbackData.feedback.base_position.pose.orientation.y, feedbackData.feedback.base_position.pose.orientation.z, feedbackData.feedback.base_position.pose.orientation.w)
        z = euler_from_quaternion(quaternion, axes='sxyz')
        data.z = z[2] + data.z * math.pi / 2
        points[it] = data
        rospy.loginfo("Add %d is : X:%f Y:%f Theta:%f" % (it, data.x, data.y, data.z))
    elif data.time == 5:
        data.x = feedbackData.feedback.base_position.pose.position.x
        data.y = feedbackData.feedback.base_position.pose.position.y
        data.z = data.time / abs(data.time)
        quaternion = (feedbackData.feedback.base_position.pose.orientation.x, feedbackData.feedback.base_position.pose.orientation.y, feedbackData.feedback.base_position.pose.orientation.z, feedbackData.feedback.base_position.pose.orientation.w)
        z = euler_from_quaternion(quaternion, axes='sxyz')
        data.z = z[2]
        points[0] = data
        rospy.loginfo("Add home is : X:%f Y:%f Theta:%f" % (data.x, data.y, data.z))
    elif points[data.time-5] is not None:
        pointData.x = points[data.time-5].x
        pointData.y = points[data.time-5].y
        pointData.z = points[data.time-5].z
        pointData.time = data.time
    else:
        rospy.loginfo("I can't find it.")

    def publish(self, data):
        q = data.getOrientation()
        roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])

        # print "Before ", "Yaw: " + str(yaw * 180 / pi), "Roll: " + str(roll * 180 / pi), "pitch: " + str(pitch * 180 / pi), "\n\n"

        array = quaternion_from_euler(roll, pitch, yaw + (self._angle * pi / 180))

        res = Quaternion()
        res.w = array[0]
        res.x = array[1]
        res.y = array[2]
        res.z = array[3]

        roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])

        # print "after ", "Yaw: " + str(yaw * 180 / pi), "Roll: " + str(roll * 180 / pi), "pitch: " + str(pitch * 180 / pi), "\n\n"

        msg = Imu()
        msg.header.frame_id = self._frameId
        msg.header.stamp = rospy.get_rostime()
        msg.orientation = res
        msg.linear_acceleration = data.getAcceleration()
        msg.angular_velocity = data.getVelocity()

        magMsg = MagneticField()
        magMsg.header.frame_id = self._frameId
        magMsg.header.stamp = rospy.get_rostime()
        magMsg.magnetic_field = data.getMagnetometer()

        self._pub.publish(msg)
        self._pubMag.publish(magMsg)

def callback(truth,odom):
    # Calculate error between truth and SLAM estimate
    x_ODOM_error = truth.pose.pose.position.x - odom.pose.pose.position.x
    y_ODOM_error = truth.pose.pose.position.y - odom.pose.pose.position.y
    
    xo = odom.pose.pose.orientation.x
    yo = odom.pose.pose.orientation.y
    zo = odom.pose.pose.orientation.z
    wo = odom.pose.pose.orientation.w
    
    xt = truth.pose.pose.orientation.x
    yt = truth.pose.pose.orientation.y
    zt = truth.pose.pose.orientation.z
    wt = truth.pose.pose.orientation.w

    # find orientation of robot (Euler coordinates)
    (ro, po, yo) = euler_from_quaternion([xo, yo, zo, wo])    
    (rt, pt, yt) = euler_from_quaternion([xt, yt, zt, wt])
    t_ODOM_error = yt-yo
    
    # query SLAM solution
    map_listener = tf.TransformListener()
    try:
      (trans,rot) = map_listener.lookupTransform('/map', '/odom', rospy.Time(0))
      x_SLAM_error = trans[0]-x_ODOM_error
      y_SLAM_error = trans[1]-y_ODOM_error
      t_SLAM_error = rot[2]-t_ODOM_error
    
      print "{0},{1},{2},{3}".format(rospy.get_time(), x_SLAM_error, y_SLAM_error, t_SLAM_error)

    except(tf.LookupException, tf.ConnectivityException):
      pass

def combinePoses(ps0, ps1, op=operator.add, listener=None):
    """
    Returns a PoseStamped of op(ps0,ps1)
    """
    # must be in same reference frame
    if listener != None:
        try:
            ps0, ps1 = convertToSameFrameAndTime(ps0, ps1, listener)
        except tf.Exception:
            return PoseStamped()

    if ps0 == None or ps1 == None:
        return False

    xtrans0, ytrans0, ztrans0 = ps0.pose.position.x, ps0.pose.position.y, ps0.pose.position.z
    xtrans1, ytrans1, ztrans1 = ps1.pose.position.x, ps1.pose.position.y, ps1.pose.position.z

    wrot0, xrot0, yrot0, zrot0 = ps0.pose.orientation.w, ps0.pose.orientation.x, ps0.pose.orientation.y, ps0.pose.orientation.z    
    wrot1, xrot1, yrot1, zrot1 = ps1.pose.orientation.w, ps1.pose.orientation.x, ps1.pose.orientation.y, ps1.pose.orientation.z

    ps0rot0, ps0rot1, ps0rot2 = tft.euler_from_quaternion([xrot0, yrot0, zrot0, wrot0])
    ps1rot0, ps1rot1, ps1rot2 = tft.euler_from_quaternion([xrot1, yrot1, zrot1, wrot1])

    addedPoint = Point(op(xtrans0,xtrans1), op(ytrans0,ytrans1), op(ztrans0,ztrans1))
    addedEuler = [op(ps0rot0,ps1rot0), op(ps0rot1,ps1rot1), op(ps0rot2,ps1rot2)]
    addedQuaternion = tft.quaternion_from_euler(addedEuler[0], addedEuler[1], addedEuler[2])
    addedOrientation = Quaternion(addedQuaternion[0], addedQuaternion[1], addedQuaternion[2], addedQuaternion[3])

    addedPose = PoseStamped()
    addedPose.header = ps0.header
    addedPose.pose.position = addedPoint
    addedPose.pose.orientation = addedOrientation

    return addedPose

    def init_plan(self, req):
        """
            add in vect_mark[0] the coordinates of the camera deduced from
            the position of the mark we choose to be the origin. This step
            enable the trackin in our plan.
        """
        try:

            marker = MARKER_NAME + str(req.marknumber)

            trans, rot = self.listener.lookupTransform(
                marker, '/map', rospy.Time(0))

            while euler_from_quaternion(rot)[0] > 1.57:
                time.sleep(0.01)
                print "SLEEEEEEEEEEp"
                trans, rot = self.listener.lookupTransform(
                    marker, '/map', rospy.Time(0))

            self.vect_tf[0] = [CAMERA_NAME, MAP, trans, rot]
            print " plan intialization...!"
            print "euler", euler_from_quaternion(self.vect_tf[0][3])
            if req.permanent == True:
                chdir(req.path)
                mon_fichier = open(FILE_SAVED_INIT_PLAN, "w")
                message = write_message([self.vect_tf[0]])
                # print "message", message

                mon_fichier.write(message)
                mon_fichier.close()
            print self.vect_tf
            return InitPlanResponse(True)
        except Exception, exc:
            print "init_plan"

    def ocall(self, data):
        twist = Twist()
        twist.angular.x = 0
        twist.angular.y = 0
        twist.angular.z = 0
        twist.linear.x = 0
        twist.linear.y = 0
        twist.linear.z = 0


        # print data
        if self.state == "Turn":
            self.odom = data
            self.old_euler = euler_from_quaternion([data.pose.pose.orientation.x, data.pose.pose.orientation.y, data.pose.pose.orientation.z,data.pose.pose.orientation.w])
            self.state = "Turning"

        # delta = data.pose.pose.orientation.z-self.odom.pose.pose.orientation.z
        euler = euler_from_quaternion([data.pose.pose.orientation.x, data.pose.pose.orientation.y, data.pose.pose.orientation.z,data.pose.pose.orientation.w])
        delta = euler[2]-self.old_euler[2]
        if abs(delta) > 3.14:
            delta = abs(delta)-6.28
        if self.state == "Turning":
            if abs(self.Z) >= .3:
                self.Z = self.Z*((1.785-abs(delta))/1.785)
                twist.angular.z = self.Z
                self.pub.publish(twist)
        # print euler[2], self.old_euler[2]

        print euler[2], delta
        if self.state == "Turning" and abs(delta) >= 1.5:
            self.X = self.Xo
            self.Z = self.Zo
            self.pub2.publish('Done')
            self.pub.publish(twist)
            self.callback("test none")

    def callback(self, msg):
        """
        Measuring the displacement based on the new position
        :param msg: Odometry msg with position and rotation information
        :type msg: nag_msgs.msg.Odometry
        :return: no return
        """
        if self.last_pose:
            new_pose = msg.pose.pose
            pos = new_pose.position
            pos_old = self.last_pose.position
            distance_delta = sqrt(pow(pos.x-pos_old.x, 2) + pow(pos.y-pos_old.y, 2))
            if distance_delta < 0.5:  # If delta is too big, it is incorrect. Not doing anything with this data
                self.total_distance += distance_delta

                new_orientation = new_pose.orientation
                old_orientation = self.last_pose.orientation
                new_rotation = euler_from_quaternion([new_orientation.x,
                                                      new_orientation.y,
                                                      new_orientation.z,
                                                      new_orientation.w])
                old_rotation = euler_from_quaternion([old_orientation.x,
                                                      old_orientation.y,
                                                      old_orientation.z,
                                                      old_orientation.w])
                rotation_delta = new_rotation[2] - old_rotation[2]
                if rotation_delta >= pi:
                    rotation_delta -= 2*pi
                elif rotation_delta <= -pi:
                    rotation_delta += 2*pi
                self.total_rotation += abs(rotation_delta)
            else:
                rospy.logerr("Distance delta too big (%f m), ignoring this step" % distance_delta)

        self.last_pose = msg.pose.pose

def turn_around():
    global turning
    global pos
    global currz
    command = Twist()
    send_command = rospy.ServiceProxy('constant_command', ConstantCommand)
    w = pos.pose.pose.orientation.w
    z = pos.pose.pose.orientation.z
    x_theta_new,y_theta_new,z_theta_new = euler_from_quaternion([0,0,z,w])

    target = get_new_orientation()
    currOrientation = z_theta_new

    if(currOrientation + target < 0):
	zgoal = 3
    else:
	zgoal = -3

    while(z_theta_new < (target - .1) or z_theta_new > (target + .1)):
        if(command.angular.z != zgoal):
	    command.angular.z = zgoal
	    send_command(command)
	w = pos.pose.pose.orientation.w
        z = pos.pose.pose.orientation.z
        x_theta_new,y_theta_new,z_theta_new = euler_from_quaternion([0,0,z,w])
    command.angular.z = 0
    send_command(command)
    rospy.sleep(.1)
    command.linear.x = .3
    send_command(command)
    currz = pos.pose.pose.position.x + pos.pose.pose.position.y

 def correct_orientation(self, target_quat):
     current_tf = None
     try:
         current_tf = self.__tf_listener.lookupTransform('/map', '/base_footprint', rospy.Time(0))
     except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
         rospy.logerr("rotate_in_place_recovery: Failed to look up transform")
         return 'failed'
 
     euler_current = euler_from_quaternion(current_tf[1])
     euler_target = euler_from_quaternion(target_quat)
     
     rospy.loginfo("Euler Current: %s", str(euler_current))
     rospy.loginfo("Euler Target: %s", str(euler_target))
     yaw_correction = euler_target[2] - (euler_current[2] + 2*math.pi)
     if yaw_correction > math.pi:
         yaw_correction -= 2*math.pi
     elif yaw_correction < -math.pi:
         yaw_correction += 2*math.pi
     rospy.loginfo("YAW Correction: %f", yaw_correction)
     
     correction = Twist()
     speed = max(abs(yaw_correction) * 0.2, 0.1)
     if yaw_correction < 0:
         correction.angular.z = -speed
     else:
         correction.angular.z = speed
     self.__cmd_vel.publish(correction)
     rospy.sleep(abs(yaw_correction)/speed)
     correction.angular.z = 0.0
     self.__cmd_vel.publish(correction)

def callback(data):
#     print "Received odom..."
    global mapOffset
    global odomPub
    
    #make odometry offset from the original odometry by the mapOffset
    Odom = Odometry()
    Odom.pose.pose.position.x = data.pose.pose.position.x + mapOffset.translation.x
    Odom.pose.pose.position.y = data.pose.pose.position.y + mapOffset.translation.y
    Odom.pose.pose.position.z = data.pose.pose.position.z + mapOffset.translation.z
    
#     print "O'("+str(Odom.pose.pose.position.x)+","+str(Odom.pose.pose.position.y)+")"
    
#     if (Odom.pose.pose.position.x <0 or Odom.pose.pose.position.y <0):
#         print Odom.pose.pose.position
#         raise Exception("NEGATIVE COORDINATES! ")
    
    #set the orientation based on the quaternion from the transformed position
    quat = data.pose.pose.orientation
    q = [quat.x,quat.y,quat.z,quat.w]
    roll, pitch, yaw = euler_from_quaternion(q)
    quat = mapOffset.rotation
    q = [quat.x,quat.y,quat.z,quat.w]
    rollOff, pitchOff, yawOff = euler_from_quaternion(q)
    roll += rollOff
    pitch += pitchOff
    yaw += yawOff
    quater = quaternion_from_euler(roll, pitch, yaw)
    Odom.pose.pose.orientation.w = quater[3]
    Odom.pose.pose.orientation.x = quater[0]
    Odom.pose.pose.orientation.y = quater[1]
    Odom.pose.pose.orientation.z = quater[2]
    
#     print "Publishing shifted odom...\n"
    odomPub.publish(Odom)

 def receive_marker_bundle(self, data):
     box_tag_seen = False
     belt_tag_seen = False
     for marker in data.markers:
         #just to be sure
         if marker.id == TagIds.MasterTag:
             box_tag_seen = True
             q = utils.msg_to_quaternion(marker.pose.pose.orientation)
             roll,pitch,yaw = euler_from_quaternion(q)
             self.tags_buffer_box.append(TagAR(marker.id,marker.pose.pose.position.x,
                                               marker.pose.pose.position.y,
                                               marker.pose.pose.position.z,
                                               roll, pitch, yaw))
         
         elif marker.id == TagIds.BeltFakeTag:
             belt_tag_seen = True
             q = utils.msg_to_quaternion(marker.pose.pose.orientation)
             roll,pitch,yaw = euler_from_quaternion(q)
             self.tags_buffer_belt.append(TagAR(marker.id,marker.pose.pose.position.x,
                                               marker.pose.pose.position.y,
                                               marker.pose.pose.position.z,
                                               roll, pitch, yaw))
                     
     if not box_tag_seen: self.tags_buffer_box.append(None)
     if not belt_tag_seen: self.tags_buffer_belt.append(None)     

 def updateImu(self, imu):
     self.imu_last_update = rospy.Time.now()
     self.imu_init = True
     
     if not self.imu_data :
         angle = euler_from_quaternion([imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w])
         imu_data =  PyKDL.Rotation.RPY(angle[0], angle[1], angle[2])
         imu_data = self.imu_tf.M * imu_data
         angle = imu_data.GetRPY()   
         self.last_imu_orientation = angle
         self.last_imu_update = imu.header.stamp
         self.imu_data = True
     else:
         angle = euler_from_quaternion([imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w])
         imu_data =  PyKDL.Rotation.RPY(angle[0], angle[1], angle[2])
         imu_data = self.imu_tf.M * imu_data
         angle = imu_data.GetRPY()   
         angle_quaternion = tf.transformations.quaternion_from_euler(angle[0], angle[1], angle[2])
         self.odom.pose.pose.orientation.x = angle_quaternion[0]
         self.odom.pose.pose.orientation.y = angle_quaternion[1]
         self.odom.pose.pose.orientation.z = angle_quaternion[2]
         self.odom.pose.pose.orientation.w = angle_quaternion[3]
         
         # Derive angular velocities from orientations ####################### 
         period = (imu.header.stamp - self.last_imu_update).to_sec()
         self.odom.twist.twist.angular.x = cola2_lib.normalizeAngle(angle[0] - self.last_imu_orientation[0]) / period 
         self.odom.twist.twist.angular.y = cola2_lib.normalizeAngle(angle[1] - self.last_imu_orientation[1]) / period 
         self.odom.twist.twist.angular.z = cola2_lib.normalizeAngle(angle[2] - self.last_imu_orientation[2]) / period 
         self.last_imu_orientation = angle
         self.last_imu_update = imu.header.stamp          
         #####################################################################
         
         if self.makePrediction():
             self.ekf.updatePrediction()
             self.publishData()

    def execute(self, userdata):
        rospy.loginfo("Creating goal to put robot in front of handle")
        pose_handle = Pose()
        if userdata.door_detection_data_in_base_link.handle_side == "left" or userdata.door_detection_data_in_base_link.handle_side == "right":  # closed door
            (r, p, theta) = euler_from_quaternion(( userdata.door_detection_data_in_base_link.door_handle.pose.orientation.x,
                                                    userdata.door_detection_data_in_base_link.door_handle.pose.orientation.y,
                                                    userdata.door_detection_data_in_base_link.door_handle.pose.orientation.z,
                                                    userdata.door_detection_data_in_base_link.door_handle.pose.orientation.w))  # gives back r, p, y
            theta += 3.1416  # the orientation of the door is looking towards the robot, we need the inverse
            pose_handle.orientation = Quaternion(*quaternion_from_euler(0, 0, theta))  # orientation to look parallel to the door
            pose_handle.position.x = userdata.door_detection_data_in_base_link.door_handle.pose.position.x - 0.4  # to align the shoulder with the handle
            pose_handle.position.y = userdata.door_detection_data_in_base_link.door_handle.pose.position.y + 0.2  # refer to upper comment
            pose_handle.position.z = 0.0  # we dont need the Z for moving
            userdata.door_handle_pose_goal = pose_handle
        else:  # open door
            # if it's open... just cross it?
            (r, p, theta) = euler_from_quaternion(( userdata.door_detection_data_in_base_link.door_position.pose.orientation.x,
                                                    userdata.door_detection_data_in_base_link.door_position.pose.orientation.y,
                                                    userdata.door_detection_data_in_base_link.door_position.pose.orientation.z,
                                                    userdata.door_detection_data_in_base_link.door_position.pose.orientation.w))  # gives back r, p, y
            theta += 3.1416  # the orientation of the door is looking towards the robot, we need the inverse
            pose_handle.orientation = Quaternion(*quaternion_from_euler(0, 0, theta))  # orientation to look parallel to the door
            pose_handle.position.x = userdata.door_detection_data_in_base_link.door_position.pose.position.x + 1.0 # enter the door
            pose_handle.position.y = userdata.door_detection_data_in_base_link.door_position.pose.position.y
            userdata.door_handle_pose_goal = pose_handle


        rospy.loginfo("Move base goal: \n" + str(pose_handle))

        return succeeded

 def tf_trigger(self, reference_frame, target_frame, tfs):
     #  this function is responsible for setting up the triggers for recording
     # on the bagfile.
     
     # sequence for calculating distance and yaw rotation for defining if a 
     # recording trigger is set according to the trigger value on the yaml file
     
     self.tfL.waitForTransform(reference_frame, target_frame, rospy.Time(0), rospy.Duration(3.0))
     trans, rot = self.tfL.lookupTransform(reference_frame, target_frame, rospy.Time(0))
     
     x = trans[0] - self.current_translation[target_frame][0]
     y = trans[1] - self.current_translation[target_frame][1]
     
     distance_trans = math.sqrt(x*x + y*y)
     distance_rot = abs(euler_from_quaternion(rot)[2] - euler_from_quaternion(self.current_rotation[target_frame])[2])
     
     if("trigger_record_translation" in tfs and distance_trans >= tfs["trigger_record_translation"]):
         rospy.loginfo("triggered for translation, trans = " + str(distance_trans))
         self.current_translation[target_frame] = trans
         self.current_rotation[target_frame] = rot
         return "triggered"
         
     if("trigger_record_rotation" in tfs and distance_rot >= tfs["trigger_record_rotation"]):
         rospy.loginfo("triggered for rotation, rot = " + str(distance_rot))
         self.current_translation[target_frame] = trans
         self.current_rotation[target_frame] = rot
         return "triggered"
     
     return "not_triggered"

    def listen_imu(self, imu):
        # node can sleep for 16 out of 20 ms without dropping any messages
        # rospy.sleep(16.0/1000.0)
        # self.NN += 1
        # if rospy.get_time() - self.t0 > 1:
        #     rospy.logfatal("Published {} messages in 1 s, {} Hz".format(self.NN, self.NN))
        #     self.NN, self.t0 = 0, rospy.get_time()
        current_orientation = np.array([imu.orientation.x,
                                        imu.orientation.y,
                                        imu.orientation.z,
                                        imu.orientation.w])
        current_angular_speed = np.array([imu.angular_velocity.x,
                                          imu.angular_velocity.y,
                                          0.0])
                                          
        current_rpy = list(transformations.euler_from_quaternion(current_orientation))
        # No care about yaw, but must be close to zero
        current_rpy[-1] = 0
        desired_rpy = list(transformations.euler_from_quaternion(self.desired_orientation))
        desired_rpy[-1] = 0
        corrected = self.rp_pid(desired_rpy, current_rpy, current_angular_speed)
        corrected_orientation = transformations.quaternion_multiply(
            transformations.quaternion_from_euler(*corrected),
            transformations.quaternion_from_euler(*current_rpy))

        setpoint_pose = Pose(self.desired_position, corrected_orientation)

        servo_angles = self.platform.ik(setpoint_pose)
        rospy.logdebug(
            "Servo angles (deg): {}".format(np.rad2deg(servo_angles)))
        self.publish_servo_angles(servo_angles)

    def publish(self, data):
        q = Quaternion()
        q.x = 0
        q.y = 0
        q.z = data[6]
        q.w = data[7]
        odomMsg = Odometry()
        odomMsg.header.frame_id = self._odom
        odomMsg.header.stamp = rospy.get_rostime()
        odomMsg.child_frame_id = self._baseLink
        odomMsg.pose.pose.position.x = data[0]
        odomMsg.pose.pose.position.y = data[1]
        odomMsg.pose.pose.position.z = 0
        odomMsg.pose.pose.orientation = q
        if self._firstTimePub:
            self._prevOdom = odomMsg
            self._firstTimePub = False
            return

        velocity = Twist()

        deltaTime = odomMsg.header.stamp.to_sec() - self._prevOdom.header.stamp.to_sec()
        yaw, pitch, roll = euler_from_quaternion(
            [odomMsg.pose.pose.orientation.w, odomMsg.pose.pose.orientation.x, odomMsg.pose.pose.orientation.y,
             odomMsg.pose.pose.orientation.z])
        prevYaw, prevPitch, prevRollprevYaw = euler_from_quaternion(
            [self._prevOdom.pose.pose.orientation.w, self._prevOdom.pose.pose.orientation.x,
             self._prevOdom.pose.pose.orientation.y, self._prevOdom.pose.pose.orientation.z])
        if deltaTime > 0:
            velocity.linear.x = (data[8] / deltaTime)

        deltaYaw = yaw - prevYaw

        # rospy.loginfo("yaw: %f\t\tpevYaw: %f\t\tdeltaYaw: %f" % (yaw,prevYaw,deltaYaw))

        if deltaYaw > math.pi: deltaYaw -= 2 * math.pi
        elif deltaYaw < -math.pi: deltaYaw += 2 * math.pi

        if deltaTime > 0:
            velocity.angular.z = -(deltaYaw / deltaTime)

        # rospy.loginfo("deltaYaw after check: %f\t\t angular: %f" % (deltaYaw, velocity.angular.z))

        odomMsg.twist.twist = velocity

        self._prevOdom = odomMsg

        traMsg = TransformStamped()
        traMsg.header.frame_id = self._odom
        traMsg.header.stamp = rospy.get_rostime()
        traMsg.child_frame_id = self._baseLink
        traMsg.transform.translation.x = data[0]
        traMsg.transform.translation.y = data[1]
        traMsg.transform.translation.z = 0
        traMsg.transform.rotation = q

        self._pub.publish(odomMsg)
        self._broadCase.sendTransformMessage(traMsg)

    def updateImu(self, imu):
        config = self.config
        config.imu_last_update = imu.header.stamp #rospy.Time.now()
        config.imu_init = True
        
        if not config.imu_data :
            angle = euler_from_quaternion([imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w])
            imu_data =  PyKDL.Rotation.RPY(angle[0], angle[1], angle[2])
            imu_data = config.imu_tf.M * imu_data
            angle = imu_data.GetRPY()   
            config.last_imu_orientation = angle
            config.last_imu_update = imu.header.stamp
            
            # Initialize heading buffer in order to apply a savitzky_golay derivation
            if len(config.heading_buffer) == 0:
                config.heading_buffer.append(angle[2])
                
            inc = cola2_lib.normalizeAngle(angle[2] - config.heading_buffer[-1])
            config.heading_buffer.append(config.heading_buffer[-1] + inc)
            
            if len(config.heading_buffer) == len(config.savitzky_golay_coeffs):
                config.imu_data = True
        else:
            angle = euler_from_quaternion([imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w])
            imu_data =  PyKDL.Rotation.RPY(angle[0], angle[1], angle[2])
            imu_data = config.imu_tf.M * imu_data
            angle = imu_data.GetRPY()   
            angle_quaternion = tf.transformations.quaternion_from_euler(angle[0], angle[1], angle[2])
            config.odom.pose.pose.orientation.x = angle_quaternion[0]
            config.odom.pose.pose.orientation.y = angle_quaternion[1]
            config.odom.pose.pose.orientation.z = angle_quaternion[2]
            config.odom.pose.pose.orientation.w = angle_quaternion[3]
            
            # Derive angular velocities from orientations ####################### 
            period = (imu.header.stamp - config.last_imu_update).to_sec()
            
            # For yaw rate we apply a savitzky_golay derivation
            inc = cola2_lib.normalizeAngle(angle[2] - config.heading_buffer[-1])
            config.heading_buffer.append(config.heading_buffer[-1] + inc)
            config.heading_buffer.pop(0)
            config.odom.twist.twist.angular.z = convolve(config.heading_buffer, config.savitzky_golay_coeffs, mode='valid') / period

            # TODO: Roll rate and Pitch rate should be also savitzky_golay derivations
            config.odom.twist.twist.angular.x = cola2_lib.normalizeAngle(angle[0] - config.last_imu_orientation[0]) / period 
            config.odom.twist.twist.angular.y = cola2_lib.normalizeAngle(angle[1] - config.last_imu_orientation[1]) / period 

            config.last_imu_orientation = angle
            config.last_imu_update = imu.header.stamp          
            #####################################################################
            
            try:
                self.LOCK.acquire()
                if self.makePrediction(imu.header.stamp):
                    self.filter.updatePrediction()
                    self.publishData()
            finally:
                self.LOCK.release()