ROSinenbroetchen's profile - activity

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)