#! /usr/bin/env python
import rospy
# For the state machine
from StateMachine import Smach
# To graph the state machine diagram
# import pygraphviz
# ROS messages and services
from std_msgs.msg import String, Int32
# from eagle_one_test.msg import State
# 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)
#! /usr/bin/env python
import rospy
# For the state machine
from StateMachine import Smach
# To graph the state machine diagram
# import pygraphviz
# ROS messages and services
from std_msgs.msg import String, Int32
# from eagle_one_test.msg import State
# 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)
#! /usr/bin/env python
# So we can use ROS
import rospy
# ROS message to receive and send out messages
# Include our custom state machine
from StateMachine import Smach
if __name__ == '__main__':
rospy.init_node('state_machine')
smach = Smach()
takepicture = TakePicture(takepicture_time)
# Reacquisition Mode Instantiation
reac_velocities = (0.3, 0.3, 0.3) # m/s
reac_max_alt = 3000 # mm
reac_tag_timeout = 15 # seconds
reac_prev_state_timeout = 5 # seconds
reac = Reacquisition(reac_velocities, reac_max_alt, reac_tag_timeout, reac_prev_state_timeout)
# follow = Follow()
# Create the timers to transition to Reacquisition mode
# takeoff_to_reac_timer = rospy.Timer(rospy.Duration(takeoff_to_reac_timeout), goto_reacquisition)
# land_to_reac_timer = rospy.Timer(rospy.Duration(land_to_reac_timeout), goto_reacquisition)
# follow_to_reac_timer = rospy.Timer(rospy.Duration(follow_to_reac_timeout), goto_reacquisition)
smach = Smach()
# TODO: Test all of this
# The start of it all
# def main():
takeoff.turn_off_timer(takeoff.timer, 'Takeoff')
land.turn_off_timer(land.timer, 'Land')
reac.turn_off_timer(reac.prev_state_timer, 'Reac prev state')
reac.turn_off_timer(reac.land_timer, 'Reac land')
while not rospy.is_shutdown():
print state
# NOTE: There are some subtleties here. We want to be able to use more than
# one mode for certain modes which is why follow mode is on it's own
# Check if we're in takeoff mode
# Reacquisition Mode Instantiation
reac_velocities = (0.3, 0.3, 0.3) # m/s
reac_max_alt = 3000 # mm
reac_tag_timeout = 15 # seconds
reac_prev_state_timeout = 5 # seconds
reac = Reacquisition(reac_velocities, reac_max_alt, reac_tag_timeout,
reac_prev_state_timeout)
# follow = Follow()
# Create the timers to transition to Reacquisition mode
# takeoff_to_reac_timer = rospy.Timer(rospy.Duration(takeoff_to_reac_timeout), goto_reacquisition)
# land_to_reac_timer = rospy.Timer(rospy.Duration(land_to_reac_timeout), goto_reacquisition)
# follow_to_reac_timer = rospy.Timer(rospy.Duration(follow_to_reac_timeout), goto_reacquisition)
smach = Smach()
# TODO: Test all of this
# The start of it all
# def main():
takeoff.turn_off_timer(takeoff.timer, 'Takeoff')
land.turn_off_timer(land.timer, 'Land')
reac.turn_off_timer(reac.prev_state_timer, 'Reac prev state')
reac.turn_off_timer(reac.land_timer, 'Reac land')
while not rospy.is_shutdown():
print state
# NOTE: There are some subtleties here. We want to be able to use more than
# one mode for certain modes which is why follow mode is on it's own
# Check if we're in takeoff mode
#! /usr/bin/env python
import rospy
# For the state machine
# from transitions import Machine
from StateMachine import Smach
# To graph the state machine diagram
# import pygraphviz
# Import the service
from eagle_one_test.srv import *
# Initialize the state
smach = Smach()
if __name__=='__main__':
smach.state_change_server()
# Define the different states of the machine
# states = ['secure', 'takeoff', 'follow', 'take_picture', \
# 'land', 'emergency', 'reacquisition']
# Define the transitions between states
# FORMAT: ['trigger_event', 'source_state', 'destination_state']
# transitions = [
# ['takeoff_command', 'secure', 'takeoff'],
# ['takeoff_alt_reached', 'takeoff', 'follow'],
# ['picture_command', 'follow', 'take_picture'],
# ['picture_taken', 'take_picture', 'land'],
