pr2_props_rfn.py

Go to the documentation of this file.

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

class PropsLauncher:
    def __init__(self, launchfile):
        self.launch = None
        self.launchfile = launchfile
        roslaunch.pmon._init_signal_handlers()
        self.server = RFNServer("high_five", 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("high_fiving")
        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 terminate.
        self.launch.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_preempted()





def main():
    rospy.init_node('pr2_props_rfn')
    launchfile = rospy.get_param('pr2_props_rfn/launchfile')
    t = PropsLauncher(launchfile)
    rospy.spin()


if __name__ == '__main__':
    main()



  # <node name="turtlebot_follower_rfn" pkg="turtlebot_follower_rfn" type="turtlebot_follower_rfn.py">
  #   <param name="launchfile"
  #          value="$(find turtlebot_follower_rfn)/turtlebot_follower_nodelet.launch" />
  # </node>