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()
# # 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()
# # 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()
