turtlebot_follower_rfn.py

Go to the documentation of this file.

#!/usr/bin/env python
import roslib; roslib.load_manifest('turtlebot_follower_rfn')
import rospy
import roslaunch.parent
import roslaunch.pmon
from rfnserver import RFNServer

class TFLauncher:
    def __init__(self, launchfile):
        self.launch = None
        self.launchfile = launchfile
        roslaunch.pmon._init_signal_handlers()
        self.server = RFNServer("follower", lambda fsf: self.launch_launchfile())
        # For error checking, each of these calls will return True upon success
        #   and False upon failure.
        self.server.register_with_frame("following_operator")
        self.server.register_preempt_callback(self.kill_launchfile)
        self.server.start()


    def launch_launchfile(self):
        if not self.launchfile:
            rospy.logerr("Param launchfile not found!")
            self.server.set_aborted()
            return False
        # WARNING: This is a roslaunch-caller-like abomination.
        #   (Plus, it's borrowed code from app_manager.)  We
        #   should leverage a true roslaunch API when it exists.
        rospy.loginfo("Lauching file: " + str(self.launchfile))
        rospy.loginfo("(Note: roslaunch API is unstable.  Cross " +
                      "your fingers this still works!")
        try:
            self.launch = roslaunch.parent.ROSLaunchParent(rospy.get_param("/run_id"),
                                                           [self.launchfile],
                                                           is_core=False,
                                                           process_listeners=())
            self.launch._load_config()
            self.launch.start()
        except Exception as e:
            try:
                # attempt to kill any launched resources
                self.kill_launchfile()
            except:
                pass
            rospy.logerr("launch failed! " + str(e))
            self.server.set_aborted()
            return False
        # Wait until we're told to stop, ie by getting kicked off.
        rospy.spin()
        self.server.set_succeeded()
        return True


    def kill_launchfile(self):
        try:
            if self.launch:
                self.launch.shutdown()
        finally:
            self.launch = None
            self.server.set_succeeded()





def main():
    rospy.init_node('turtlebot_follower_rfn')
    launchfile = rospy.get_param('turtlebot_follower_rfn/launchfile')
    t = TFLauncher(launchfile)
    rospy.spin()


if __name__ == '__main__':
    main()