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

tf2_ros lookup_transform giving wrong transformation

asked 2022-10-20 20:48:58 -0500

DiegoC gravatar image

updated 2022-10-23 16:01:10 -0500

Hello everyone,

I want to get the transform from tool0 frame to base_link frame of the UR3 robot but I'm having problems.

Below is the code I'm using to get the transform:

#!/usr/bin/env python

import rospy
import tf2_ros

class UR3TfListener(object):

    def tfListenerCallback(self, event):
        if self.tf_buffer.can_transform('tool0','base_link',rospy.Time(0)):
                trans = self.tf_buffer.lookup_transform('tool0','base_link',rospy.Time(0))
        else:
            rospy.logdebug("Can't transform")

        tx = trans.transform.translation.x
        ty = trans.transform.translation.y
        tz = trans.transform.translation.z
        rx = trans.transform.rotation.x
        ry = trans.transform.rotation.y
        rz = trans.transform.rotation.z

        rospy.logdebug("tx: %d, ty: %d, tz: %d, rx: %d, ry: %d, rz: %d", tx, ty, tz, rx, ry, rz)

    def __init__(self):

        self.tf_buffer = tf2_ros.Buffer()
        self.listener = tf2_ros.TransformListener(self.tf_buffer)

        rospy.Timer(rospy.Duration(0.1),self.tfListenerCallback)

if __name__ == '__main__':
    rospy.init_node('tf_listener', anonymous=False, log_level=rospy.DEBUG)
    tf_listener = UR3TfListener()
    rospy.spin()

The problem that I'm having is that the transform that I'm getting is something like this

[DEBUG] [1666494690.406768, 199.024000]: tx: 0, ty: 0, tz: 0, rx: 0, ry: 0, rz:0

And the transform shouldn't be 0 as the frames look like following:

image description

Edit 1: /tf and /tf_static echo result

When I run rostopic echo /tf -n 1 I get:

   transforms: 
     - 
       header: 
         seq: 0
         stamp: 
           secs: 11
           nsecs: 296000000
         frame_id: "upper_arm_link"
       child_frame_id: "forearm_link"
       transform: 
         translation: 
           x: -0.24365
           y: 0.0
           z: 0.0
         rotation: 
           x: 0.0
           y: 0.0
           z: 9.38252996394e-06
           w: 0.999999999956
     - 
       header: 
         seq: 0
         stamp: 
           secs: 11
           nsecs: 296000000
         frame_id: "robotiq_85_left_inner_knuckle_link"
       child_frame_id: "robotiq_85_left_finger_tip_link"
       transform: 
         translation: 
           x: 0.04303959807
           y: -0.03759940821
           z: 0.0
         rotation: 
           x: 0.0
           y: 0.0
           z: -1.31530571e-05
           w: 0.999999999913
        - 
       header: 
         seq: 0
         stamp: 
           secs: 11
           nsecs: 296000000
         frame_id: "robotiq_85_base_link"
       child_frame_id: "robotiq_85_left_inner_knuckle_link"
       transform: 
         translation: 
           x: 0.06142
           y: 0.0127
           z: 0.0
         rotation: 
           x: 0.999999999913
           y: -1.31530571e-05
           z: -1.36017806871e-18
           w: -1.03411553546e-13
     - 
       header: 
         seq: 0
         stamp: 
           secs: 11
           nsecs: 296000000
         frame_id: "robotiq_85_base_link"
       child_frame_id: "robotiq_85_left_knuckle_link"
       transform: 
         translation: 
           x: 0.05490451627
           y: 0.03060114443
           z: 0.0
         rotation: 
           x: 0.999999999913
           y: -1.31530571e-05
           z: -1.36017806871e-18
           w: -1.03411553546e-13
     - 
       header: 
         seq: 0
         stamp: 
           secs: 11
           nsecs: 296000000
         frame_id: "robotiq_85_right_inner_knuckle_link"
       child_frame_id: "robotiq_85_right_finger_tip_link"
       transform: 
         translation: 
           x: 0.04303959807
           y: -0.03759940821
           z: 0.0
         rotation: 
           x: 0.0
           y: 0.0
           z: -1.31530571e-05
           w: 0.999999999913
     - 
       header: 
         seq: 0
         stamp: 
           secs: 11
           nsecs: 296000000
         frame_id: "robotiq_85_base_link"
       child_frame_id: "robotiq_85_right_inner_knuckle_link"
       transform: 
         translation: 
           x: 0.06142
           y: -0.0127
           z: 0.0
         rotation: 
           x: 0.0
           y: 0.0
           z: 1.31530571e-05
           w: 0.999999999913
     - 
       header: 
         seq: 0
         stamp: 
           secs: 11
           nsecs: 296000000
         frame_id: "robotiq_85_base_link"
       child_frame_id: "robotiq_85_right_knuckle_link"
       transform: 
         translation: 
           x: 0.05490451627
           y: -0.03060114443
           z: 0.0
         rotation: 
           x: 0.0
           y: 0.0
           z: 1.31530571e-05
           w: 0.999999999913
     - 
       header: 
         seq: 0
         stamp: 
           secs: 11
           nsecs: 296000000
         frame_id: "shoulder_link"
       child_frame_id: "upper_arm_link"
       transform: 
         translation: 
           x: 0.0
           y: 0.0
           z: 0.0 ...
(more)
edit retag flag offensive close merge delete

Comments

How did you print or note that transformation? Unfortunately, I don't see any print statement in your code snippet. On the other hand, the code is incomplete. Thus, I can not reproduce it here. Nevertheless, you may try the following sample code: http://wiki.ros.org/tf2/Tutorials/Wri...

ravijoshi gravatar image ravijoshi  ( 2022-10-21 23:10:58 -0500 )edit

I based my code on the tf2 tutorials, also I edited my question and put the entire code.

DiegoC gravatar image DiegoC  ( 2022-10-22 13:50:54 -0500 )edit

Please show us the message (including the header) that is being published on /tf.

Mike Scheutzow gravatar image Mike Scheutzow  ( 2022-10-23 10:00:38 -0500 )edit

I updated the question with the messages from /tf and /tf_static and the tf tree

DiegoC gravatar image DiegoC  ( 2022-10-23 12:53:13 -0500 )edit
add a comment

2 Answers

Sort by ยป oldest newest most voted
2

answered 2022-10-24 03:37:32 -0500

ravijoshi gravatar image

The problem is not with the tf or ROS. You are using %d as a format string, which will round float values to integers. Coincidentally, our values are less than 1. Thus, they are rounded to 0.

I am surprised and confused at the same time after seeing the following statement:

    rospy.logdebug(
        "tx: %f, ty: %f, tz: %f, rx: %f, ry: %f, rz: %f", tx, ty, tz, rx, ry, rz
    )

I recommend, using the following statement instead:

    rospy.logdebug(trans)

Below is the code snippet:

#!/usr/bin/env python

import rospy
import tf2_ros


class UR3TfListener(object):
    def __init__(self):
        self.tf_buffer = tf2_ros.Buffer()
        self.listener = tf2_ros.TransformListener(self.tf_buffer)
        self.timer = rospy.Timer(rospy.Duration(1), self.tfListenerCallback)

    def tfListenerCallback(self, event):
        try:
            trans = self.tf_buffer.lookup_transform("tool0", "base_link", rospy.Time())
        except (
            tf2_ros.LookupException,
            tf2_ros.ConnectivityException,
            tf2_ros.ExtrapolationException,
        ):
            rospy.logdebug("Can't transform")
            return

        tx = trans.transform.translation.x
        ty = trans.transform.translation.y
        tz = trans.transform.translation.z
        rx = trans.transform.rotation.x
        ry = trans.transform.rotation.y
        rz = trans.transform.rotation.z

        rospy.logdebug(trans)
        rospy.logdebug(
            "tx: %f, ty: %f, tz: %f, rx: %f, ry: %f, rz: %f", tx, ty, tz, rx, ry, rz
        )


if __name__ == "__main__":
    rospy.init_node("tf_listener", anonymous=False, log_level=rospy.DEBUG)
    tf_listener = UR3TfListener()
    rospy.spin()

I used random values to publish a transformation as shown below:

ravi@dell:~/ros_ws$ rosrun tf2_ros static_transform_publisher 0.4 0.2 0.3 0.1 0.2 0.3 base_link tool0
[ INFO] [1666599679.602630460]: Spinning until killed publishing base_link to tool0

Then, I run your code to print the transformation. Please see below:

ravi@dell:~/ros_ws$ rosrun beginner_tutorials tf.py 
[DEBUG] [1666600470.164958]: init_node, name[/tf_listener], pid[71067]
[DEBUG] [1666600470.167868]: binding to 0.0.0.0 0
[DEBUG] [1666600470.170540]: bound to 0.0.0.0 38651
[DEBUG] [1666600470.173766]: ... service URL is rosrpc://dell:38651
[DEBUG] [1666600470.176383]: [/tf_listener/get_loggers]: new Service instance
[DEBUG] [1666600470.181235]: ... service URL is rosrpc://dell:38651
[DEBUG] [1666600470.183920]: [/tf_listener/set_logger_level]: new Service instance
[DEBUG] [1666600470.191242]: ... service URL is rosrpc://dell:38651
[DEBUG] [1666600470.195970]: [/tf_listener/tf2_frames]: new Service instance
[DEBUG] [1666600470.211223]: node[/tf_listener, http://dell:39269/] entering spin(), pid[71067]
[DEBUG] [1666600470.220975]: connecting to tm 58745
[DEBUG] [1666600470.312265]: header: 
  seq: 0
  stamp: 
    secs: 0
    nsecs:         0
  frame_id: "tool0"
child_frame_id: "base_link"
transform: 
  translation: 
    x: -0.3500360106436593
    y: -0.2633910550476512
    z: -0.31320910487014486
  rotation: 
    x: -0.14357217502739192
    y: -0.10602051106179562
    z: -0.03427079855048211
    w: 0.9833474432563559
[DEBUG] [1666600470.316059]: tx: -0.350036, ty: -0.263391, tz: -0.313209, rx: -0.143572, ry: -0.106021, rz: -0.034271
edit flag offensive delete link more

Comments

Thank you so much for your answer, it solved my problem. And thanks to everyone who helped.

DiegoC gravatar image DiegoC  ( 2022-10-24 09:53:14 -0500 )edit

Glad you made it work. May I ask you to upvote the answer by clicking on ^ icon in the left side, please?

ravijoshi gravatar image ravijoshi  ( 2022-10-24 09:55:09 -0500 )edit
add a comment
1

answered 2022-10-22 06:40:51 -0500

Mike Scheutzow gravatar image

If there are no errors, you are never calling rate.sleep(). rospy requires that you call rospy.spin() or rate.sleep().

edit flag offensive delete link more

Comments

I am calling rate.sleep(). To avoid confusions I edited the question and added the entire code

DiegoC gravatar image DiegoC  ( 2022-10-22 13:52:13 -0500 )edit

The code you originally provided was not calling rate.sleep(). Are you periodically re-publishing the transform? If it is not a static transform, the value will time out after some amount of time (typically about 10 seconds.)

Mike Scheutzow gravatar image Mike Scheutzow  ( 2022-10-23 07:20:58 -0500 )edit

Sorry for not providing the full code before. The transform is being published by the simulation, if I run rostopic echo /tf -n 1 I can see the transform between the different frames of the robot, is just from code that I get 0

DiegoC gravatar image DiegoC  ( 2022-10-23 09:13:16 -0500 )edit

I doubt an (almost) infinite while loop in the ctor of a Python object is supposed to work this way.

I'd suggest using a Timer or similar to periodically call lookup_transform(..).

gvdhoorn gravatar image gvdhoorn  ( 2022-10-23 12:08:16 -0500 )edit

It still doesn't work using a Timer instead of a while loop

DiegoC gravatar image DiegoC  ( 2022-10-23 13:00:49 -0500 )edit

I would suggest you update your OP with the code you're using. As others have already stated, it's difficult to help anyone without seeing the code.

gvdhoorn gravatar image gvdhoorn  ( 2022-10-23 14:39:10 -0500 )edit

I updated the OP with the code using rospy.Timer()

DiegoC gravatar image DiegoC  ( 2022-10-23 16:02:15 -0500 )edit
add a comment

Question Tools

2 followers

subscribe to rss feed

Stats

Asked: 2022-10-20 20:48:58 -0500

Seen: 317 times

Last updated: Oct 24 '22

Related questions

AttributeError's when running Python scripts

ROS Melodic OpenCV xfeatures2d

ROS TCP Connector C# Lost Publishers & Freezing

Controlling a quadcopter in Gazebo

error getMD5Sum in customized message type

Navigation without lidar

URDF model using the a variable parameter

ros::Rate::sleep() equivalent in rosserial

convert imu to odom

How to correctly link opencl with ros?

ROS Answers is licensed under Creative Commons Attribution 3.0 Content on this site is licensed under a Creative Commons Attribution Share Alike 3.0 license.
about | faq | help | privacy policy | terms of service
Powered by Askbot version 0.10.2