Calculating angles bigger than 180 degrees using tflistener

edit

Hi,

I'm trying to make my differential drive robot rotate a specified angle, but I'm having trouble calculating the angle when the robot has rotated more than 180 degrees, because when the angle gets bigger than 180 degrees it loops back to -180 degrees. Here you can see what I mean (the angles are in radians):

angle_turned: 3.10394590162
angle_turned: 3.13691517119
angle_turned: -3.11200932277
angle_turned: -3.07439747739
angle_turned: -3.04209143293
angle_turned: -3.0088080041
angle_turned: -2.97583873453

I found this tutorial on the ros wiki which I'm using as a reference. I think that tutorial checks when the angle loops back around to -180 degrees by fist getting the rotation axis tf::Vector3 actual_turn_axis relative_transform.getRotation().getAxis(); and then doing this check: if ( actual_turn_axis.dot( desired_turn_axis ) < 0 ) angle_turned = 2 * M_PI - angle_turned;

(lines 49 to 55 in the '0.1 Turning' paragraph of that tutorial)

My implementation in Python (of just the tflistener and calculation part) does not work however. The direction never changes sign:

angle_turned: 3.10394590162 direction: [0. 0. 1.]
angle_turned: 3.13691517119 direction: [0. 0. 1.]
angle_turned: -3.11200932277 direction: [0. 0. 1.]
angle_turned: -3.07439747739 direction: [0. 0. 1.]
angle_turned: -3.04209143293 direction: [0. 0. 1.]
angle_turned: -3.0088080041 direction: [0. 0. 1.]
angle_turned: -2.97583873453 direction: [0. 0. 1.]

My Python code (I'm currently manually rotating the robot using the teleop_twist_keyboard package):

import rospy
import tf2_ros
import geometry_msgs.msg
import numpy as np
import tf
from tf import transformations as ts

if __name__ == "__main__":
    rospy.init_node('tf2listener')

    tfBuffer = tf2_ros.Buffer()
    listener = tf2_ros.TransformListener(tfBuffer)

   rate = rospy.Rate(10.0)

   #checking if transform is available
   available = False
   while not available:
       if tfBuffer.can_transform('odom', 'base_link', rospy.Time()):
           available = True

   #getting the start transform
   start_transform = tfBuffer.lookup_transform('odom', 'base_link', rospy.Time())

   #converting to a homogeneous transformation matrix
   start_transform_translation = [start_transform.transform.translation.x, start_transform.transform.translation.y, start_transform.transform.translation.z]
   start_transform_rotation = [start_transform.transform.rotation.x, start_transform.transform.rotation.y, start_transform.transform.rotation.z, start_transform.transform.rotation.w]
   start_transform = ts.concatenate_matrices(ts.translation_matrix(start_transform_translation), ts.quaternion_matrix(start_transform_rotation))

   #taking the inverse
   start_transform_inverse = ts.inverse_matrix(start_transform)

   #setting the desired turn axis, assume we'll only rotate the robot counterclockwise for now
   desired_turn_axis = np.array([0, 0, 1]) #counterclockwise rotation

   while not rospy.is_shutdown():
       #getting the current transform
       try:
           current_transform = tfBuffer.lookup_transform('odom', 'base_link', rospy.Time())
       except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
           rate.sleep()
           continue

       #converting to a homogeneous transformation matrix
       current_transform_translation = [current_transform.transform.translation.x, current_transform.transform.translation.y, current_transform.transform.translation.z]
       current_transform_rotation = [current_transform.transform.rotation.x, current_transform.transform.rotation.y, current_transform.transform.rotation.z, current_transform.transform.rotation.w]
       current_transform = ts.concatenate_matrices(ts.translation_matrix(current_transform_translation), ts.quaternion_matrix(current_transform_rotation))

       #calculating the relative transform
       relative_transform = np.dot(start_transform_inverse, current_transform)

       #getting the angle and direction of the relative transform
       angle_turned, direction, point = ts.rotation_from_matrix(relative_transform)

       #checking if direction of rotation of the relative transform is opposite of desired direction of rotation
       if np.dot(desired_turn_axis, np.transpose(direction)) < 0:
           angle_turned = (2 * np.pi) - angle_turned

       print('angle_turned ...