CODEWITHSUNDEEP
pythonrossubscriberslamtf2-ros
Hongyan Wu
Hongyan Wu

Reputation: 11

TF2 transform can't find an actuall existing frame

In a global planner node that I wrote, I have the following init code

#!/usr/bin/env python
import rospy
import copy
import tf2_ros
import time
import numpy as np
import math
import tf
from math import sqrt, pow
from geometry_msgs.msg import Vector3, Point
from std_msgs.msg import Int32MultiArray
from std_msgs.msg import Bool
from nav_msgs.msg import OccupancyGrid, Path
from geometry_msgs.msg import PoseStamped, PointStamped
from tf2_geometry_msgs import do_transform_point
from Queue import PriorityQueue

class GlobalPlanner():

    def __init__(self):
        print("init global planner")
        self.tfBuffer = tf2_ros.Buffer()
        self.listener = tf2_ros.TransformListener(self.tfBuffer)
        
        
        self.drone_position_sub = rospy.Subscriber('uav/sensors/gps', PoseStamped, self.get_drone_position)
        self.drone_position = []
        self.drone_map_position = []
        self.map_sub = rospy.Subscriber("/map", OccupancyGrid, self.get_map) 
        self.goal_sub = rospy.Subscriber("/cell_tower/position", Point, self.getTransformedGoal)
        self.goal_position = []
        self.goal = Point()
        self.goal_map_position = []
        self.occupancy_grid = OccupancyGrid()
        self.map = []
        self.p_path = Int32MultiArray()

        self.position_pub = rospy.Publisher("/uav/input/position", Vector3, queue_size = 1)
        #next_movement in
        self.next_movement = Vector3
        self.next_movement.z = 3
        self.path_pub = rospy.Publisher('/uav/path', Int32MultiArray, queue_size=1)

        self.width = rospy.get_param('global_planner_node/map_width')
        self.height = rospy.get_param('global_planner_node/map_height')

        #Check whether there is a path plan
        self.have_plan = False
        self.path = []

        self.euc_distance_drone_goal = 100
        self.twod_distance_drone_goal = []
        self.map_distance_drone_goal = []
        
        self.mainLoop()

And there is a call-back function call getTransformed goal, which will take the goal position in the "cell_tower" frame to the "world" frame. Which looks like this

def getTransformedGoal(self, msg):
        self.goal = msg
        try: 
            #Lookup the tower to world transform
            transform = self.tfBuffer.lookup_transform('cell_tower', 'world', rospy.Time())
            #transform = self.tfBuffer.lookup_transform('world','cell-tower' rospy.Time())
            #Convert the goal to a PointStamped
            goal_pointStamped = PointStamped()
            goal_pointStamped.point.x = self.goal.x
            goal_pointStamped.point.y = self.goal.y
            goal_pointStamped.point.z = self.goal.z
            #Use the do_transform_point function to convert the point using the transform
            new_point = do_transform_point(goal_pointStamped, transform)
            #Convert the point back into a vector message containing integers
            transform_point = [new_point.point.x, new_point.point.y]
            
            #Publish the vector
            self.goal_position = transform_point
        except (tf2_ros.LookupException, tf2_ros.ConnectivityException,  tf2_ros.ExtrapolationException) as e:
            print(e)
            print('global_planner tf2 exception, continuing')

The error message said that

"cell_tower" passed to lookupTransform argument target_frame does not exist.

I check the RQT plot for both active and all, which shows that when active, the topic /tf is not being subscribe by the node global planner. Check the following image, which is for active enter image description here

and this image is for all the node (include non-active) enter image description here

But I have actually set up the listner, I have another node call local planner that use the same strategy and it works for that node, but not for the global planner I'm not sure why this is.

Upvotes: 1

Views: 1180

Answers (1)

ignacio
ignacio

Reputation: 1197

Try adding a timeout to your lookup_transform() function call, as your transformation may not be available when you need it:

transform = self.tfBuffer.lookup_transform('cell_tower', 'world',rospy.Time.now(), rospy.Duration(1.0))

Upvotes: 1

Related Questions