ROS Answers: Open Source Q&A Forum - RSS feedhttps://answers.ros.org/questions/Open source question and answer forum written in Python and DjangoenROS Answers is licensed under Creative Commons Attribution 3.0Sat, 10 Jul 2021 14:24:22 -0500Calculating angles bigger than 180 degrees using tflistenerhttps://answers.ros.org/question/381743/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](https://wiki.ros.org/pr2_controllers/Tutorials/Using%20the%20base%20controller%20with%20odometry%20and%20transform%20information) 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: {} direction: {}'.format(angle_turned, direction))
#checking if angle turned is higher than desired angle (here desired angle is set to 1.5*pi)
if angle_turned > 1.5 * np.pi:
break
rate.sleep()
I don't know if this is appropriate or not, but you can find my whole robot setup on [my github](https://github.com/ongaroleandro/autonomous-mobile-robot)Sun, 04 Jul 2021 09:19:24 -0500https://answers.ros.org/question/381743/calculating-angles-bigger-than-180-degrees-using-tflistener/Comment by Mike Scheutzow for <div class="snippet"><p>Hi, </p>
<p>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):</p>
<blockquote>
<p>angle_turned: 3.10394590162 <br>
angle_turned: 3.13691517119 <br>
angle_turned: -3.11200932277 <br>
angle_turned: -3.07439747739 <br>
angle_turned: -3.04209143293 <br>
angle_turned: -3.0088080041 <br>
angle_turned: -2.97583873453 </p>
</blockquote>
<p>I found <a href="https://wiki.ros.org/pr2_controllers/Tutorials/Using%20the%20base%20controller%20with%20odometry%20and%20transform%20information">this</a> 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 <code>tf::Vector3 actual_turn_axis relative_transform.getRotation().getAxis();</code> and then doing this check: <code>if ( actual_turn_axis.dot( desired_turn_axis ) < 0 )
angle_turned = 2 * M_PI - angle_turned;</code> </p>
<p>(lines 49 to 55 in the '0.1 Turning' paragraph of that tutorial)</p>
<p>My implementation in Python (of just the tflistener and calculation part) does not work however. The direction never changes sign:</p>
<blockquote>
<p>angle_turned: 3.10394590162 direction: [0. 0. 1.] <br>
angle_turned: 3.13691517119 direction: [0. 0. 1.] <br>
angle_turned: -3.11200932277 direction: [0. 0. 1.] <br>
angle_turned: -3.07439747739 direction: [0. 0. 1.] <br>
angle_turned: -3.04209143293 direction: [0. 0. 1.] <br>
angle_turned: -3.0088080041 direction: [0. 0. 1.] <br>
angle_turned: -2.97583873453 direction: [0. 0. 1.] </p>
</blockquote>
<p>My Python code (I'm currently manually rotating the robot using the teleop_twist_keyboard package):</p>
<pre><code>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 ...</code></pre><span class="expander"> <a>(more)</a></span></div>https://answers.ros.org/question/381743/calculating-angles-bigger-than-180-degrees-using-tflistener/?comment=382187#post-id-382187You 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?Sat, 10 Jul 2021 11:24:10 -0500https://answers.ros.org/question/381743/calculating-angles-bigger-than-180-degrees-using-tflistener/?comment=382187#post-id-382187Comment by leandro for <div class="snippet"><p>Hi, </p>
<p>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):</p>
<blockquote>
<p>angle_turned: 3.10394590162 <br>
angle_turned: 3.13691517119 <br>
angle_turned: -3.11200932277 <br>
angle_turned: -3.07439747739 <br>
angle_turned: -3.04209143293 <br>
angle_turned: -3.0088080041 <br>
angle_turned: -2.97583873453 </p>
</blockquote>
<p>I found <a href="https://wiki.ros.org/pr2_controllers/Tutorials/Using%20the%20base%20controller%20with%20odometry%20and%20transform%20information">this</a> 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 <code>tf::Vector3 actual_turn_axis relative_transform.getRotation().getAxis();</code> and then doing this check: <code>if ( actual_turn_axis.dot( desired_turn_axis ) < 0 )
angle_turned = 2 * M_PI - angle_turned;</code> </p>
<p>(lines 49 to 55 in the '0.1 Turning' paragraph of that tutorial)</p>
<p>My implementation in Python (of just the tflistener and calculation part) does not work however. The direction never changes sign:</p>
<blockquote>
<p>angle_turned: 3.10394590162 direction: [0. 0. 1.] <br>
angle_turned: 3.13691517119 direction: [0. 0. 1.] <br>
angle_turned: -3.11200932277 direction: [0. 0. 1.] <br>
angle_turned: -3.07439747739 direction: [0. 0. 1.] <br>
angle_turned: -3.04209143293 direction: [0. 0. 1.] <br>
angle_turned: -3.0088080041 direction: [0. 0. 1.] <br>
angle_turned: -2.97583873453 direction: [0. 0. 1.] </p>
</blockquote>
<p>My Python code (I'm currently manually rotating the robot using the teleop_twist_keyboard package):</p>
<pre><code>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 ...</code></pre><span class="expander"> <a>(more)</a></span></div>https://answers.ros.org/question/381743/calculating-angles-bigger-than-180-degrees-using-tflistener/?comment=382191#post-id-382191The 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](https://github.com/ongaroleandro/autonomous-mobile-robot/blob/main/code%20used%20for%20testing/send_twist_msgs.py) 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 be implemented once it works.Sat, 10 Jul 2021 12:31:36 -0500https://answers.ros.org/question/381743/calculating-angles-bigger-than-180-degrees-using-tflistener/?comment=382191#post-id-382191Answer by Mike Scheutzow for <div class="snippet"><p>Hi, </p>
<p>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):</p>
<blockquote>
<p>angle_turned: 3.10394590162 <br>
angle_turned: 3.13691517119 <br>
angle_turned: -3.11200932277 <br>
angle_turned: -3.07439747739 <br>
angle_turned: -3.04209143293 <br>
angle_turned: -3.0088080041 <br>
angle_turned: -2.97583873453 </p>
</blockquote>
<p>I found <a href="https://wiki.ros.org/pr2_controllers/Tutorials/Using%20the%20base%20controller%20with%20odometry%20and%20transform%20information">this</a> 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 <code>tf::Vector3 actual_turn_axis relative_transform.getRotation().getAxis();</code> and then doing this check: <code>if ( actual_turn_axis.dot( desired_turn_axis ) < 0 )
angle_turned = 2 * M_PI - angle_turned;</code> </p>
<p>(lines 49 to 55 in the '0.1 Turning' paragraph of that tutorial)</p>
<p>My implementation in Python (of just the tflistener and calculation part) does not work however. The direction never changes sign:</p>
<blockquote>
<p>angle_turned: 3.10394590162 direction: [0. 0. 1.] <br>
angle_turned: 3.13691517119 direction: [0. 0. 1.] <br>
angle_turned: -3.11200932277 direction: [0. 0. 1.] <br>
angle_turned: -3.07439747739 direction: [0. 0. 1.] <br>
angle_turned: -3.04209143293 direction: [0. 0. 1.] <br>
angle_turned: -3.0088080041 direction: [0. 0. 1.] <br>
angle_turned: -2.97583873453 direction: [0. 0. 1.] </p>
</blockquote>
<p>My Python code (I'm currently manually rotating the robot using the teleop_twist_keyboard package):</p>
<pre><code>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 ...</code></pre><span class="expander"> <a>(more)</a></span></div> https://answers.ros.org/question/381743/calculating-angles-bigger-than-180-degrees-using-tflistener/?answer=382195#post-id-382195The 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]**.Sat, 10 Jul 2021 13:10:03 -0500https://answers.ros.org/question/381743/calculating-angles-bigger-than-180-degrees-using-tflistener/?answer=382195#post-id-382195Comment by leandro for <p>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.</p>
<p>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.</p>
<p>Regarding the actual question in the title: I hope we don't need to show you how to convert an angle from <strong>[-pi to pi]</strong> to <strong>[0 to 2*pi]</strong>.</p>
https://answers.ros.org/question/381743/calculating-angles-bigger-than-180-degrees-using-tflistener/?comment=382199#post-id-382199Thank you, I will try the method you described.Sat, 10 Jul 2021 14:24:22 -0500https://answers.ros.org/question/381743/calculating-angles-bigger-than-180-degrees-using-tflistener/?comment=382199#post-id-382199