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.
