Пример #1
0
smach = Smach()
state = String()
transition = String()


# Callback functions to handle the data along the /smach/transition topic
def transition_cb(msg):
    transition.data = msg.data


# ROS initializations
rospy.init_node('qc_smach_server')
rospy.loginfo("Hulk Smach!")  # Let's us know that this has loaded
rate = rospy.Rate(100)  # 100Hz

# Setup the publishers and subscribers
state_pub = rospy.Publisher('/smach/state', String, queue_size=100000)
transition_sub = rospy.Subscriber('/smach/transition', String, transition_cb)
counter_pub = rospy.Publisher('/smach/counter', Int32, queue_size=1000)
counter = 0

if __name__ == '__main__':
    while not rospy.is_shutdown():
        smach.change_state(transition.data)
        counter_pub.publish(counter)
        print("New state: %s" % smach.state)
        state.data = smach.state
        state_pub.publish(state)
        counter += 1
        rate.sleep()
Пример #2
0
    # # Check if we're in reacquisition mode
    # elif (state == 'reacquisition'):
    #     # TODO: Turn on reacquisition mode's timer and turn off every other mode's
    #     # timers EXCEPT take picture
        # takeoff.turn_off_timer(takeoff.timer)
        # land.turn_off_timer(land.timer)
        # reac.turn_on_timer(reac.prev_state_timer)
        # reac.turn_on_timer(reac.land_timer)
    #
    #     print "Reacquisition Mode"
    #     reac.move()
    #
    # Check if we're in secure mode and do nothing
# elif (state == 'secure'):
#         takeoff.turn_off_timer(takeoff.timer)
#         land.turn_off_timer(land.timer)
#         reac.turn_off_timer(reac.prev_state_timer)
#         reac.turn_off_timer(reac.land_timer)
#         print "Secure Mode"
    #
    # # Check if we're in follow mode
    # # Follow mode is used in every mode except for secure and emergency
    # if (not ((smach.state == 'secure')) and (smach.state == 'emergency')):
    #     print "Follow Mode"

    smach.change_state(transition)
    rate.sleep()
#
# if __name__ == '__main__':
#     main()
# Initialize the state machine and variables
smach      = Smach()
state      = String()
transition = String()

# Callback functions to handle the data along the /smach/transition topic
def transition_cb(msg):
    transition.data = msg.data

# ROS initializations
rospy.init_node('qc_smach_server')
rospy.loginfo("Hulk Smach!") # Let's us know that this has loaded
rate = rospy.Rate(100) # 100Hz

# Setup the publishers and subscribers
state_pub = rospy.Publisher('/smach/state', String, queue_size=100000)
transition_sub = rospy.Subscriber('/smach/transition', String, transition_cb)
counter_pub = rospy.Publisher('/smach/counter', Int32, queue_size=1000)
counter = 0

if __name__=='__main__':
    while not rospy.is_shutdown():
        smach.change_state(transition.data)
        counter_pub.publish(counter)
        print("New state: %s" % smach.state)
        state.data = smach.state
        state_pub.publish(state)
        counter += 1
        rate.sleep()
Пример #4
0
    # # Check if we're in reacquisition mode
    # elif (state == 'reacquisition'):
    #     # TODO: Turn on reacquisition mode's timer and turn off every other mode's
    #     # timers EXCEPT take picture
    # takeoff.turn_off_timer(takeoff.timer)
    # land.turn_off_timer(land.timer)
    # reac.turn_on_timer(reac.prev_state_timer)
    # reac.turn_on_timer(reac.land_timer)
    #
    #     print "Reacquisition Mode"
    #     reac.move()
    #
    # Check if we're in secure mode and do nothing
# elif (state == 'secure'):
#         takeoff.turn_off_timer(takeoff.timer)
#         land.turn_off_timer(land.timer)
#         reac.turn_off_timer(reac.prev_state_timer)
#         reac.turn_off_timer(reac.land_timer)
#         print "Secure Mode"
#
# # Check if we're in follow mode
# # Follow mode is used in every mode except for secure and emergency
# if (not ((smach.state == 'secure')) and (smach.state == 'emergency')):
#     print "Follow Mode"

    smach.change_state(transition)
    rate.sleep()
#
# if __name__ == '__main__':
#     main()