Ask Your Question
0

rospy / SMACH vs. TF

asked 2013-07-31 04:09:09 -0500

ZdenekM gravatar image

updated 2013-08-06 00:37:41 -0500

Hello,

I'm having few SMACH states in different files and would like to use TF in some of them. There will be probably even some concurrent states so, I created helper class tf_utils (helper_methods.py) with TF listener protected by mutex. It's initialized like this:

_tfl = None

def get_tfl():

    return _tfl

def init_tfl():

    global _tfl

    if _tfl is None:

        _tfl = tf_utils()

Class tf_utils looks like this:

class tf_utils():

    def __init__(self,timeout=None):

        if timeout is None:

            timeout = rospy.Duration(5)

        self.timeout = timeout

        self.tfl = TransformListener()

        self.mutex = threading.Lock()

    def waitForTransform(self,f1,f2,time):

        ret = True

        self.mutex.acquire()

        try:

            self.tfl.waitForTransform(f1,f2,time,self.timeout)

        except tf.Exception:

            ret = False

        self.mutex.release()

        return ret 

etc...

In main file, I initialize node (rospy.init_node) and tf_utils (helper_methods.init_tfl()). Then for instance frameExists method works fine while with waitForTransform I'm getting this:

terminate called after throwing an instance of 'ros::TimeNotInitializedException'
  what():  Cannot use ros::Time::now() before the first NodeHandle has been created or ros::start() has been called.  If this is a standalone app or test that just uses ros::Time and does not communicate over ROS, you may also call ros::Time::init()

Situation is same for both wall time or simulated time.

What's wrong? Or is there any other approach how to use one transform listener in more python modules / SMACH states?

UPDATE:

This is how I initialize my stuff. First I initialize rospy and then (in main) my tf listener.

if __name__ == '__main__':

    rospy.init_node('rt_decision_making')

    try:

        main()

    except rospy.ROSInterruptException:
        print "program interrupted before completion"

and...

def main():

  helper_methods.init_tfl()

  tfl = helper_methods.get_tfl()

UPDATE2:

I ended up with the most simple python script:

#!/usr/bin/env python

import roslib; roslib.load_manifest('rt_decision_making')
import rospy
import tf
from tf import TransformListener

def main():

  rospy.init_node('rt_decision_making')
  rospy.sleep(2)

  tfl =  TransformListener()

  rospy.sleep(2)

  tfl.waitForTransform("base_link", "velodyne", rospy.Time.now(), rospy.Duration(1))

if __name__ == '__main__':

    try:

        main()

    except rospy.ROSInterruptException:
        print "program interrupted before completion"

...which works in Groovy but not in Hydro which I'm currently working with.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2013-08-05 12:05:56 -0500

tfoote gravatar image

You need to make sure that rospy is initialized in before you initialize your class. The easiest will probably be to just do that at the top of your main.

edit flag offensive delete link more

Comments

Thanks for answer. Well, I'm calling "rospy.init_node" as first thing. See updated question.

ZdenekM gravatar image ZdenekM  ( 2013-08-05 20:48:09 -0500 )edit

My guess is that it's happening at import time of some submodule. You can catch the backtrace of the exception to find out where it is coming from.

tfoote gravatar image tfoote  ( 2013-08-05 20:51:51 -0500 )edit

Thanks for hint. I tried to make the simplest possible script and it works under Groovy but not under Hydro. Looks like a bug...

ZdenekM gravatar image ZdenekM  ( 2013-08-06 00:38:45 -0500 )edit
ZdenekM gravatar image ZdenekM  ( 2013-08-06 00:45:54 -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

1 follower

Stats

Asked: 2013-07-31 04:09:09 -0500

Seen: 446 times

Last updated: Aug 06 '13