ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange

# 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:
available = True

#getting the start transform

#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:
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 ...
edit retag close merge delete

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)

Sort by » oldest newest most voted

The method you show is unable to rotate more than 2* pi. The simplest way to allow rotating more than 2* pi is to fetch the current yaw periodically, and calculate how far the robot has rotated since the last measurement. Add these small values up until you reach whatever total you want.

The above simple method may be a little inaccurate due to drift. You can improve the performance by breaking a big move into multiple smaller moves, each under 2* pi.

Regarding the actual question in the title: I hope we don't need to show you how to convert an angle from [-pi to pi] to [0 to 2*pi].

more