loop.py

Go to the documentation of this file.

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

class LoopServer:
    def __init__(self):
        self.server = RFNServer("loop_topic", self.loop_function)
        # For error checking, each of these calls will return True upon success
        #   and False upon failure.
        self.server.add_frame("../frames/looping.yaml")
        self.server.add_lexical_unit("../lu/loop_looping.yaml")
        self.server.register_with_frame("looping")
        self.server.start()

    def loop_function(self, filled_semantic_frame):
        r = rospy.Rate(2) #hz
        i = 0
        p = self.server.is_preempt_requested()
        while ((not p) and
               (not rospy.is_shutdown())):
            rospy.loginfo("Loop " + str(i) + "!  Preempt request?: " + str(p))
            i = i + 1
            r.sleep()
            p = self.server.is_preempt_requested()
        if p:
            rospy.loginfo("Preempted!")
            self.server.set_preempted()
        else:
            rospy.loginfo("Killed!")
            self.server.set_aborted()
        rospy.sleep(0.5)
        return



def main():
    rospy.init_node('loop_node')
    server = LoopServer()
    rospy.spin()

if __name__ == '__main__':
    main()