rospy / SMACH vs. TF

edit

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.