Ask Your Question
1

Calculating angles bigger than 180 degrees using tflistener

asked 2021-07-04 11:33:22 -0500

leandro gravatar image

updated 2021-07-10 05:09:52 -0500

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 ...
(more)
edit retag flag offensive close merge delete

Comments

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?

Mike Scheutzow gravatar image Mike Scheutzow  ( 2021-07-10 11:24:10 -0500 )edit

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)

leandro gravatar image leandro  ( 2021-07-10 12:31:36 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2021-07-10 13:10:03 -0500

Mike Scheutzow gravatar image

updated 2021-07-10 13:11:06 -0500

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

edit flag offensive delete link more

Comments

Thank you, I will try the method you described.

leandro gravatar image leandro  ( 2021-07-10 14:24:22 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2021-07-04 09:19:24 -0500

Seen: 100 times

Last updated: Jul 10