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

sendTransform and lookup_transform in same node

asked 2023-01-17 09:32:54 -0500

SW gravatar image

Hi i am using ros2 humble with tf2 and python. I encounter a problem with sending a transform and looking up the transform in the same node. I get the error: "tf2.LookupException: "weed_0" passed to lookupTransform argument source_frame does not exist.". I am using tf2 to transform detection from camera to tool frame.

I get that i am getting this error because it cant find the frame weed_0. One solution that i found was to try and look up the transformation before new points get sent. This would means that when the node gets launched for the first time that it would not publish the coordinates of the points. The second time the node would get launched the node would lookup_transform, publish the position and sndTransform of the new detections. The current solution works but i am wondering if it is also possible to first sendTransform and shortly after that lookupTransform. If more info is needed please ask. Thanks in advanced.

The current program is shown below:

main function is :

    try:
        pub_detections_tool_frame(self)     # here the placed points get read using lookup_transform and then coordinates get published to topic
    except:
        print("Failed to find a transform to ")     
    transform_frame(self)

pub_detections_tool_frame()

msg = Detections()
msg.header.stamp = self.Detections.header.stamp

if self.i_weed != 0:
    range_weed = range(self.i_weed)
    for a in range_weed: 
        t_old = get_transform(self, 'weed_removal_tool', "weed_" + str(a))

        # print('detect header stamp: ', self.Detections.header.stamp)
        # print('tf header stamp: ', Time.from_msg(t_old.header.stamp).nanoseconds)
        # print('current time stamp: ', self.get_clock().now().nanoseconds )
    #print(t_old)
        x = t_old.transform.translation.x * (-1) + traveled_distance(self, Time.from_msg(t_old.header.stamp).nanoseconds, self.get_clock().now().nanoseconds )
        y = t_old.transform.translation.y * (-1)
        z = t_old.transform.translation.z * (-1)

        msg.detected.append(detection_to_msg('weed', x ,y ,z))


if self.i_crop != 0:
    range_crop = range(self.i_crop)
    for b in range_crop:
        t_old = get_transform(self, 'weed_removal_tool', "crop_" + str(b))
        x = t_old.transform.translation.x * (-1) + traveled_distance(self, Time.from_msg(t_old.header.stamp).nanoseconds, self.get_clock().now().nanoseconds )
        y = t_old.transform.translation.y * (-1)
        z = t_old.transform.translation.z * (-1)

        msg.detected.append(detection_to_msg('crop', x ,y ,z))

    self.publisher.publish(msg)

transform_frame()

    self.i = 0
    self.i_weed = 0
    self.i_crop = 0

    tt = get_transform(self, 'weed_removal_tool', 'camera')
    t_new = TransformStamped()

    size = range(len(self.detected_list)) 

    for i in size:
        object = self.detected_list[i]
        t_new = TransformStamped()
        #t_new.header.stamp = self.get_clock().now().to_msg()  
        t_new.header.stamp = self.Detections.header.stamp 
        t_new.header.frame_id = 'camera'
        if object[0] == 'weed':
            t_new.child_frame_id = object[0] + "_" + str(self.i_weed)
            self.i_weed = self.i_weed + 1
        else:
           t_new.child_frame_id = object[0] + "_" + str(self.i_crop) 
           self.i_crop = self.i_crop + 1

        t_new.transform.translation.x = object[1] 
        t_new.transform.translation.y = object[2] 
        t_new.transform.translation.z = object[3] 
        t_new.transform.rotation.x = tt.transform.rotation.x
        t_new.transform.rotation.y = tt.transform.rotation.y
        t_new.transform.rotation.z = tt.transform.rotation.z
        t_new.transform.rotation.w = tt.transform.rotation.w
        self.i = self.i +1
        self.tf_broadcaster.sendTransform(t_new)

get_transform()

try:
    when = self.get_clock ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2023-01-21 15:51:53 -0500

Mike Scheutzow gravatar image

updated 2023-01-21 15:57:01 -0500

It does seem odd that 50 milliseconds is not enough time if you've actually published the transform.

Anyway, I'd say you are not using the API in the intended way. If there's a chance the transform might not exist yet, you should first test the content of the tf_buffer using method canTransform() or call waitForTransform(). See:

https://docs.ros2.org/foxy/api/tf2_ro...

edit flag offensive delete link more

Comments

Hi Mike, thanks for your answer. I think something is going wrong with buffer. Did not yet find the solution. But the current code is fine for now.

SW gravatar image SW  ( 2023-01-26 08:21:35 -0500 )edit

Question Tools

2 followers

Stats

Asked: 2023-01-17 09:32:54 -0500

Seen: 121 times

Last updated: Jan 21 '23