Get tf transform AFTER publishing node has died
see original version below
Hey there,
I still struggle to get tf transforms out of ros for successive runs of a pygmooptimization problem. At least it does not crash anymore but I have not found a working way to reset/restart the tf listener and buffer yet.
This is what I do for starting the algorithm:
#!/usr/bin/env python
import pygmo as pg
import rospy
import roslaunch
import tf2_ros
import [algo] as co
import sys
from argparse import ArgumentParser
parser = ArgumentParser()
parser.add_argument("-f", "--bagfile", dest="bagfile",
help="write report to FILE", metavar="FILE")
args = parser.parse_args()
print(args)
# start roscore
uuid_roscore = roslaunch.rlutil.get_or_generate_uuid(None, False)
roslaunch.configure_logging(uuid_roscore)
roscore = roslaunch.parent.ROSLaunchParent(uuid_roscore, [],is_core=True)
roscore.start()
# start tf listener
node_name = '[node_name]'
rospy.init_node(node_name)
rate = rospy.Rate(10.0)
algo = pg.algorithm(pg.nsga2(gen=100))
algo.set_verbosity(100)
problem = pg.problem(co.optimize(args.bagfile))
pop = pg.population(problem, 20)
pop = algo.evolve(pop)
The important part of the class for the evaluation is:
class ProcessListener(roslaunch.pmon.ProcessListener):
process_finished = False
def process_died(self, name, exit_code):
self.process_finished = True
rospy.logwarn("%s died with code %s", name, exit_code)
# global process_finished
class optimize(object):
def __init__(self, bagfile):
self.bagfile = bagfile
def evaluate(self):
# start tf listener
tf_buffer = tf2_ros.Buffer()
tf2_listener = tf2_ros.TransformListener(tf_buffer)
# start launchfile
uuid_launchfile = roslaunch.rlutil.get_or_generate_uuid(None, False)
roslaunch.configure_logging(uuid_launchfile)
cli_args = ['[package_name]', 'launchfile.launch', 'configuration_directory:=$(find [package_name])/config','configuration_basenames:=config.lua', 'bag_filenames:='+self.bagfile]
roslaunch_args = cli_args[2:]
roslaunch_file = [(roslaunch.rlutil.resolve_launch_arguments(cli_args)[0], roslaunch_args)]
process_listener = ProcessListener()
launchfile = roslaunch.parent.ROSLaunchParent(uuid_launchfile, roslaunch_file, process_listeners=[process_listener],is_core=False)
launchfile.start()
#get transform
transform_received = False
while not process_listener.process_finished:
# continue
time.sleep(.01)
print("Process had finished")
for i in range(3000):
if not transform_received:
try:
trans = tf_buffer.lookup_transform('map',
'base_link',
rospy.Time(0))
transform_received = True
print("Transform received")
except:
time.sleep(.01)
# continue
if(not transform_received):
print("NO transform received")
pos_x = 1337.0
pos_y = 1337.0
pos_z = 1337.0
else:
pos_x = trans.transform.translation.x
pos_y = trans.transform.translation.y
pos_z = trans.transform.translation.z
print(pos_x)
print(pos_y)
print(pos_z)
del tf_buffer
del tf2_listener
return [pos_x, pos_y, pos_z]
The error message I get is (at the moment, different other tries in the comments e.g. restarting the master...)
Warning: TF_OLD_DATA ignoring data from the past for frame odom at time 1.56268e+09 according to authority /[node]
Possible reasons are listed at http://wiki.ros.org/tf/Errors%20explained
at line 277 in /tmp/binarydeb/ros-melodic-tf2-0.6.5/src/buffer_core.cpp
How can I manage that each loop iteration starts with fresh (=empty) tf data? As you probably noticed, I am quite new to python.
Hey there,
in order to evaluate the performance of some algorithms, I want to compare the position of my robot after a rosbag with sensor data has finished with another run with different parameters.
The straightforward approach with a TransformListenerdoes not work as the nodes are not sending anymore.
Is there a more convenient solution ...
