# Calculating angles bigger than 180 degrees using tflistener

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 ...
```

You need to tell us more precisely what behavior you want from the code. For example, what should the robot do if asked to rotate 4*pi ? Does it move or not move? And if it moves, which way should it move?

The robot should make two full rotations if it is asked to rotate 4*pi radians; The direction will be determined by the angular velocity you set; a positive angular velocity makes it rotate CCW and negative CW.

The code I posted is used to calculate the angle after you've published a message on the /cmd_vel topic; e.g. where the angular velocity is 0.6 rad/s and the linear velocity is 0 m/s. When the robot has rotated say 4*pi radians it should publish another message on the /cmd_vel topic to make the robot stop rotating.

I thought it would be clearer to only post the code I'm having problems with, but if it helps here's the code of the whole node. On line 80 the method starts where the angle calculation will be done and where the code I posted here will ...(more)