Ask Your Question
0

Get tf transform AFTER publishing node has died

asked 2019-07-10 10:19:47 -0500

updated 2019-07-16 08:24:59 -0500

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 ... (more)

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2019-07-10 11:35:37 -0500

gvdhoorn gravatar image

updated 2019-07-11 02:14:16 -0500

Just a question at this point: if your Listener has been listening while everything was running, it should still have all the transforms. If you then ask for "the latest" transform (ie: ros::Time(0)) it should still be able to answer your query.

Does that not work for you? Are you using ros::Time::now()?


Edit: you might also want to look into the length of the cache for the Listener that you've created. Default is 1 minute I believe. Older TFs will be pruned, and in that case even asking for ros::Time(0) wouldn't work (as the cache would be empty).

edit flag offensive delete link more

Comments

Thank you for your response. Actually, that could work for me. I'll try it.

ROSinenbroetchen gravatar imageROSinenbroetchen ( 2019-07-11 02:05:48 -0500 )edit

Still did not manage to fix it. I am using a python script to iterate through the paremeters given by a genetic algorithm. Each time, the master is shut down and a new one is started. In the second loop, I alwasy get an error message saying: rospy.service.ServiceException: service [/[node_name]/tf2_frames] already registered, although I started a new master and tried to unsubscribe before shutting the old one down:

m = rosgraph.masterapi.Master(rospy.get_name())
node_uri = m.lookupNode(node_name)
print("Unregister: " + node_uri + node_name + '/tf2_frames')
m.unregisterService('~tf2_frames',node_uri + node_name + '/tf2_frames')

What am I missing?

ROSinenbroetchen gravatar imageROSinenbroetchen ( 2019-07-15 06:01:21 -0500 )edit

rospy.signal_shutdown('Quit') seemed to help. I keep watching.

ROSinenbroetchen gravatar imageROSinenbroetchen ( 2019-07-15 06:16:30 -0500 )edit

Each time, the master is shut down and a new one is started

but your nodes aren't?

I'm not entirely sure what you're doing, or whether it's actually connected to your issues, but interacting with the master on a side channel to (de)register services/topics/nodes is probably not entirely sanitary and could introduce some weird effects.

Still did not manage to fix it.

you haven't actually described what doesn't work for you.

If a TfListener has a sufficiently long buffer and it has seen the TFs that you're interested in it should be able to return "the latest" transform to you. Regardless of whether other nodes are still running. It's decoupled from that.

But your last few comments seem to indicate that you're doing some unconventional meddling with the master/your nodes, which could be affecting normal operations.

gvdhoorn gravatar imagegvdhoorn ( 2019-07-15 06:18:01 -0500 )edit

Actually they did. It could be similar to this bug: https://answers.ros.org/question/1077...

ROSinenbroetchen gravatar imageROSinenbroetchen ( 2019-07-15 06:19:40 -0500 )edit

It did NOT help. The second iteration, no new ros node is started. Next guess was to start the whole ROS stuff before this iteration loop and to pass the tf2 buffer as parameter. Did not work either.

ROSinenbroetchen gravatar imageROSinenbroetchen ( 2019-07-16 07:02:39 -0500 )edit

I'm not sure where the need for capitals comes from.

You still haven't shown me any errors or code of something that doesn't work. Please show actual errors copy-pasted and/or code snippets.

I would still expect lookups to work for frames that were last broadcast N seconds ago if:

  1. the listener has a long enough buffer (at least as long as N, but probably more)
  2. you ask for either a point t that falls before the last broadcast of the frame or ask for "the latest"

Whether or not there still are broadcasters for those frames should not matter.

gvdhoorn gravatar imagegvdhoorn ( 2019-07-16 07:39:56 -0500 )edit

That was just meant as emphasis. I will rewrite the original question with code snippets. The problem is rather that I cannot get rid of the listener and transforms of the preceeding loop run. Even when I shut everything down, including the code. That does not make any sense at all for me.

ROSinenbroetchen gravatar imageROSinenbroetchen ( 2019-07-16 07:45:27 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2019-07-10 10:19:47 -0500

Seen: 18 times

Last updated: yesterday