sendTransform and lookup_transform in same node
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 ...