def __init__(self):
rospy.init_node('patrol_smach', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Initialize a number of parameters and variables
setup_task_environment(self)
# Track success rate of getting to the goal locations
self.n_succeeded = 0
self.n_aborted = 0
self.n_preempted = 0
# A list to hold then navigation waypoints
nav_states = list()
# Turn the waypoints into SMACH states
for waypoint in self.waypoints:
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'base_footprint'
nav_goal.target_pose.pose = waypoint
move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(10.0),
server_wait_timeout=rospy.Duration(10.0))
nav_states.append(move_base_state)
# Initialize the patrol state machine
self.sm_patrol = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Add the states to the state machine with the appropriate transitions
with self.sm_patrol:
StateMachine.add('NAV_STATE_0', nav_states[0], transitions={'succeeded':'NAV_STATE_1','aborted':'NAV_STATE_1','preempted':'NAV_STATE_1'})
StateMachine.add('NAV_STATE_1', nav_states[1], transitions={'succeeded':'NAV_STATE_2','aborted':'NAV_STATE_2','preempted':'NAV_STATE_2'})
StateMachine.add('NAV_STATE_2', nav_states[2], transitions={'succeeded':'NAV_STATE_3','aborted':'NAV_STATE_3','preempted':'NAV_STATE_3'})
StateMachine.add('NAV_STATE_3', nav_states[3], transitions={'succeeded':'NAV_STATE_4','aborted':'NAV_STATE_4','preempted':'NAV_STATE_4'})
StateMachine.add('NAV_STATE_4', nav_states[0], transitions={'succeeded':'','aborted':'','preempted':''})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('patrol', self.sm_patrol, '/SM_ROOT')
intro_server.start()
# Execute the state machine for the specified number of patrols
while (self.n_patrols == -1 or self.patrol_count < self.n_patrols) and not rospy.is_shutdown():
sm_outcome = self.sm_patrol.execute()
self.patrol_count += 1
rospy.loginfo("FINISHED PATROL LOOP: " + str(self.patrol_count))
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def main():
rospy.init_node('smach_example_hierarchical_state_machine')
# Create the top level SMACH state machine
sm_top = StateMachine(outcomes=['outcome5'])
# Open the container
with sm_top:
StateMachine.add('BAS', Bas(), transitions={'outcome3': 'SUB'})
# Create the sub SMACH state machine
sm_sub = StateMachine(outcomes=['outcome4'])
# Open the container
with sm_sub:
# Add state to the container
StateMachine.add('FOO',
Foo(),
transitions={
'outcome1': 'BAR',
'outcome2': 'outcome4'
})
StateMachine.add('BAR', Bar(), transitions={'outcome1': 'FOO'})
StateMachine.add('SUB', sm_sub, transitions={'outcome4': 'outcome5'})
sis = IntrospectionServer('example', sm_top, '/SM_STATEMACHINE')
sis.start()
# Execute SMACH plan
outcome = sm_top.execute()
rospy.spin()
sis.stop()
def main():
rospy.init_node('smach_example_state_machine')
init_robot_pose()
move_base_override.init()
reset_robot_actuators()
odom_sub = rospy.Subscriber('/odom', Odometry, odometry_cb)
sq = Sequence(outcomes=[Transitions.SUCCESS, Transitions.FAILURE],
connector_outcome=Transitions.SUCCESS)
with sq:
Sequence.add('waiting', WaitStartState())
for i in range(3):
Sequence.add('fishing {}'.format(i), create_fish_sequence())
# Sequence.add('inner_door', create_door_state_machine(0.3))
# Sequence.add('outer_door', create_door_state_machine(0.6))
# Create and start the introspection server
sis = IntrospectionServer('strat', sq, '/strat')
sis.start()
# Execute the state machine
outcome = sq.execute()
# Wait for ctrl-c to stop the application
rospy.spin()
sis.stop()
def main():
rospy.init_node('bbauv_suavc_smach')
sauvc = smach.StateMachine(outcomes=['succeeded','preempted'])
with sauvc:
smach.StateMachine.add('COUNTDOWN_START',Countdown(1.0),transitions={'succeeded':'SMACH_TRUE'})
smach.StateMachine.add('SMACH_TRUE',SmachSwitch(True),transitions={'succeeded':'VISION_TRACK_TRUE'})
smach.StateMachine.add('VISION_TRACK_TRUE',TrackerSwitch(False),transitions={'succeeded':'NAV_TRUE'})
smach.StateMachine.add('NAV_TRUE',NavigationSwitch(True),transitions={'succeeded':'TOPSIDE_FALSE'})
smach.StateMachine.add('TOPSIDE_FALSE',TopsideSwitch(False),transitions={'succeeded':'COUNTDOWN_DIVE'})
smach.StateMachine.add('COUNTDOWN_DIVE',Countdown(1),transitions={'succeeded':'VISION_TRACK_TRUE1'})
smach.StateMachine.add('VISION_TRACK_TRUE1',TrackerSwitch(True),transitions={'succeeded':'NAV_TRUE1'})
smach.StateMachine.add('NAV_TRUE1',NavigationSwitch(False),transitions={'succeeded':'COUNTDOWN_MISSION2'})
smach.StateMachine.add('COUNTDOWN_MISSION2',Countdown(180),transitions={'succeeded':'TOPSIDE_TRUE'})
smach.StateMachine.add('TOPSIDE_TRUE',TopsideSwitch(True),transitions={'succeeded':'COUNTDOWN_END'})
smach.StateMachine.add('COUNTDOWN_END',Countdown(0.1),transitions={'succeeded':'VISION_TRACK_FALSE'})
smach.StateMachine.add('VISION_TRACK_FALSE',TrackerSwitch(False),transitions={'succeeded':'NAV_FALSE'})
smach.StateMachine.add('NAV_FALSE',NavigationSwitch(False),transitions={'succeeded':'SMACH_FALSE'})
smach.StateMachine.add('SMACH_FALSE',SmachSwitch(False),transitions={'succeeded':'succeeded'})
sis = IntrospectionServer('my_introspserver', sauvc,'/SM_ROOT')
sis.start()
outcome = sauvc.execute()
rospy.on_shutdown(shutdown)
rospy.spin()
sis.stop()
def main():
from bender_skills import robot_factory
rospy.init_node("crowd_information")
robot = robot_factory.build(
[
"facial_features",
# "AI",
"person_detector",
# "knowledge",
"tts"
],
core=False)
robot.check()
sm = getInstance(robot, True)
ud = smach.UserData()
ud.features = ['gender', 'age']
# introspection server
sis = IntrospectionServer('crowd_information', sm, '/CROWD_SM')
sis.start()
outcome = sm.execute(ud)
sis.stop()
def main():
rospy.init_node('tinker_mission_follow')
rospy.loginfo(colored('starting follow and guide task ...', 'green'))
# Main StateMachine
state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with state:
StateMachine.add('Start_Button', MonitorStartButtonState(),
transitions={'valid': 'Start_Button', 'invalid': '1_Start'})
StateMachine.add('1_Start', MonitorKinectBodyState(),
transitions={'valid':'1_Start', 'invalid':'Sequence'})
sequence = Sequence(outcomes=['succeeded', 'preempted', 'aborted'], connector_outcome='succeeded')
with sequence:
follow_concurrence = Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='succeeded', child_termination_cb=lambda x: True, input_keys=[])
with follow_concurrence:
Concurrence.add('FollowMe', FollowMeState())
Concurrence.add('KeyWordsRecognition', KeywordsRecognizeState('stop'))
Sequence.add('Follow_concurrence', follow_concurrence)
StateMachine.add('Sequence', sequence, {'succeeded': 'succeeded', 'aborted': 'aborted'})
# Run state machine introspection server for smach viewer
intro_server = IntrospectionServer('tinker_mission_navigation', state, '/tinker_mission_navigation')
intro_server.start()
outcome = state.execute()
rospy.spin()
intro_server.stop()
def main():
rospy.init_node('smach_example_state_machine')
init_robot_pose()
move_base_override.init()
reset_robot_actuators()
odom_sub = rospy.Subscriber('/odom', Odometry, odometry_cb)
sq = Sequence(outcomes=[Transitions.SUCCESS, Transitions.FAILURE],
connector_outcome=Transitions.SUCCESS)
with sq:
Sequence.add('waiting', WaitStartState())
for i in range(3):
Sequence.add('fishing {}'.format(i), create_fish_sequence())
# Sequence.add('inner_door', create_door_state_machine(0.3))
# Sequence.add('outer_door', create_door_state_machine(0.6))
# Create and start the introspection server
sis = IntrospectionServer('strat', sq, '/strat')
sis.start()
# Execute the state machine
outcome = sq.execute()
# Wait for ctrl-c to stop the application
rospy.spin()
sis.stop()
def __init__(self):
rospy.init_node('TestFollow')
rospy.on_shutdown(self.shutdown)
rospy.logerr('Follow start')
self.smach_bool = False
self.newFollow = StateMachine(outcomes = ['succeeded' , 'aborted','error'])
with self.newFollow:
StateMachine.add('RUNNODE' , RunNode() , transitions = {'succeeded':'FOLLOW' ,
'aborted':'RUNNODE',
})
StateMachine.add('FOLLOW' ,FollowTest() , transitions = {'succeeded':'succeeded' ,
'error':'error',
'aborted':'aborted'} )
intro_server = IntrospectionServer('sm_gpsr',self.newFollow,'/SM_ROOT')
intro_server.start()
out = self.newFollow.execute()
rospy.logerr(out)
intro_server.stop()
self.smach_bool = True
def main():
rospy.init_node('smach_usecase_step_02')
# Create a SMACH state machine
sm0 = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
# Open the container
with sm0:
# Reset turtlesim
StateMachine.add('RESET', ServiceState('reset', std_srvs.srv.Empty),
{'succeeded': 'SPAWN'})
# Create a second turtle
StateMachine.add(
'SPAWN',
ServiceState('spawn',
turtlesim.srv.Spawn,
request=turtlesim.srv.SpawnRequest(
0.0, 0.0, 0.0, 'turtle2')))
# Attach a SMACH introspection server
sis = IntrospectionServer('smach_usecase_01', sm0, '/USE_CASE')
sis.start()
# Execute SMACH tree
outcome = sm0.execute()
# Signal ROS shutdown (kill threads in background)
rospy.spin()
sis.stop()
def __init__(self):
rospy.init_node('monitor_fake_battery', anonymous=False)
rospy.on_shutdown(self.shutdown)
# Set the low battery threshold (between 0 and 100)
self.low_battery_threshold = rospy.get_param('~low_battery_threshold', 50)
# Initialize the state machine
sm_battery_monitor = StateMachine(outcomes=[])
with sm_battery_monitor:
# Add a MonitorState to subscribe to the battery level topic
StateMachine.add('MONITOR_BATTERY',
MonitorState('battery_level', Float32, self.battery_cb),
transitions={'invalid':'RECHARGE_BATTERY',
'valid':'MONITOR_BATTERY',
'preempted':'MONITOR_BATTERY'},)
# Add a ServiceState to simulate a recharge using the set_battery_level service
StateMachine.add('RECHARGE_BATTERY',
ServiceState('battery_simulator/set_battery_level', SetBatteryLevel, request=100),
transitions={'succeeded':'MONITOR_BATTERY',
'aborted':'MONITOR_BATTERY',
'preempted':'MONITOR_BATTERY'})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('monitor_battery', sm_battery_monitor, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = sm_battery_monitor.execute()
intro_server.stop()
def __init__(self):
rospy.init_node('move_to_and_inspect_point_sm', anonymous=False)
hsm = StateMachine(outcomes=['finished statemachine'])
with hsm:
StateMachine.add( 'GO_TO_POINT',
SimpleActionState( 'pid_global',
MoveAction,
makeMoveGoal("pid_global_plan", -3.0, 0, -0.5, radius_of_acceptance = 2.0)),
transitions = { "succeeded": 'INSPECT_POINT',
"preempted": 'INSPECT_POINT',
"aborted": 'INSPECT_POINT' })
StateMachine.add( 'INSPECT_POINT',
SimpleActionState( 'inspect_point',
MoveAction,
makeMoveGoal("inspect_point", -3.0, 0.0, -0.5, radius_of_acceptance=2.0)),
transitions = { 'succeeded': 'INSPECT_POINT',
"preempted": 'INSPECT_POINT',
"aborted": 'INSPECT_POINT' })
intro_server = IntrospectionServer(str(rospy.get_name()), hsm,'/SM_ROOT')
intro_server.start()
hsm.execute()
#patrol.execute()
print("State machine execute finished")
intro_server.stop()
def main():
rospy.init_node('mission_planner')
sm = smach.StateMachine(
outcomes=['mission_complete', 'mission_failed', 'aborted'])
with sm:
Sink(sm, 5, 'DEPTH')
#state detectGate()
depthTask = Depth(100, 'HEADING')
depthTask.addDepthAction(sm)
headingTask = Heading(200, 'DEPTH+HEADING')
headingTask.addHeadingAction(sm)
depthHeadingTask = DepthHeading(350, 350, 'mission_complete')
depthHeadingTask.addDepthHeading(sm)
sis = IntrospectionServer('ALPHEUS_MISSION_PLANNER', sm,
'/START_ALPHEUS')
sis.start()
outcome = sm.execute()
rospy.spin()
sis.stop()
def main():
rospy.init_node('smach_example_state_machine')
sm = StateMachine(['exit'])
with sm:
for key, (x, y, next_point) in WAYPOINTS.items():
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.pose.position.x = x
goal.target_pose.pose.position.y = y
goal.target_pose.pose.orientation.w = 1.
StateMachine.add(key, SimpleActionState('move_base',
MoveBaseAction,
goal=goal),
transitions={'succeeded': next_point, 'aborted': 'exit', 'preempted': 'exit'})
# Create and start the introspection server
sis = IntrospectionServer('strat', sm, '/SM_ROOT')
sis.start()
# Execute the state machine
outcome = sm.execute()
# Wait for ctrl-c to stop the application
rospy.spin()
sis.stop()
def __init__(self):
rospy.init_node('patrol_smach', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Initialize a number of parameters and variables
setup_task_environment(self)
# Track success rate of getting to the goal locations
self.n_succeeded = 0
self.n_aborted = 0
self.n_preempted = 0
self.patrol_count = 0
self.n_patrols = 1
# A random number generator for use in the transition callback
self.rand = Random()
# A list to hold then nav_states
nav_states = list()
# Turn the waypoints into SMACH states
for waypoint in self.waypoints:
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = waypoint
move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(10.0),
server_wait_timeout=rospy.Duration(10.0))
nav_states.append(move_base_state)
# Initialize the patrol state machine
self.sm_patrol = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Register a callback function to fire on state transitions within the sm_patrol state machine
self.sm_patrol.register_transition_cb(self.transition_cb, cb_args=[])
# Add the states to the state machine with the appropriate transitions
with self.sm_patrol:
StateMachine.add('NAV_STATE_0', nav_states[0], transitions={'succeeded':'NAV_STATE_1','aborted':'NAV_STATE_1'})
StateMachine.add('NAV_STATE_1', nav_states[1], transitions={'succeeded':'NAV_STATE_2','aborted':'NAV_STATE_2'})
StateMachine.add('NAV_STATE_2', nav_states[2], transitions={'succeeded':'NAV_STATE_3','aborted':'NAV_STATE_3'})
StateMachine.add('NAV_STATE_3', nav_states[3], transitions={'succeeded':'NAV_STATE_4','aborted':'NAV_STATE_4'})
StateMachine.add('NAV_STATE_4', nav_states[0], transitions={'succeeded':'NAV_STATE_0','aborted':'NAV_STATE_0'})
StateMachine.add('CHECK_COUNT', CheckCount(self.n_patrols), transitions={'succeeded':'NAV_STATE_0','aborted':'','preempted':''})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('patrol', self.sm_patrol, '/SM_ROOT')
intro_server.start()
# Execute the state machine
self.sm_outcome = self.sm_patrol.execute()
rospy.loginfo('State Machine Outcome: ' + str(self.sm_outcome))
intro_server.stop()
os._exit(0)
def main():
sm_iterator = construct_sm()
sis = IntrospectionServer('smach_viwer', sm_iterator, '/SM_ROOT')
sis.start()
outcome = sm_iterator.execute()
rospy.spin()
sis.stop()
def main():
smach_states = rospy.get_param("smach_states")
# The autonomy_sm handles the full autonomy sequence for the competition run
autonomy_sm = StateMachine(outcomes=['succeeded','aborted','preempted'])
with autonomy_sm:
# TODO: create localization state. might just be a MonitorState
# Sequentially add all the states from the config file to the state machine,
# where state i transitions to state i+1
names = []
for i in range(len(smach_states)):
state = smach_states[i]
name, action_state = create_action_state(state)
# If the state name is already in the state machine, add the new
# one with increasing numbers
if name in names:
name = name+"2"
while name in names:
name = name[:-1]+str(int(name[-1])+1) # increase num
names.append(name)
StateMachine.add_auto(name, action_state, connector_outcomes=['succeeded'])
# StateMachine.add()
# Create the concurrence contained for the fully autonomy sequence. This
# runs the state machine for the competition run. It also concurrently runs
# a state with a timer counting down from 10 minutes and a state that listens
# to the /click/start_button topic. If either of these are triggered, it will
# end autonomy and place us into the teleop state.
# TODO: add 10 minute competition timer state
autonomy_concurrence = Concurrence(outcomes=['enter_teleop', 'stay', 'aborted'],
default_outcome='enter_teleop',
child_termination_cb=autonomy_child_term_cb,
outcome_cb=autonomy_out_cb)
with autonomy_concurrence:
# state that runs full autonomy state machine
Concurrence.add('AUTONOMY', autonomy_sm)
# state that listens for toggle message
Concurrence.add('TOGGLE_LISTEN', MonitorState('/click/start_button', Empty, monitor_cb))
# Top level state machine, containing the autonomy and teleop machines.
top_sm = StateMachine(outcomes=['DONE'])
with top_sm:
StateMachine.add('TELEOP_MODE', MonitorState('/click/start_button', Empty, monitor_cb), transitions={'invalid':'AUTONOMY_MODE', 'valid':'TELEOP_MODE', 'preempted':'AUTONOMY_MODE'})
StateMachine.add('AUTONOMY_MODE', autonomy_concurrence,
transitions={'enter_teleop':'TELEOP_MODE', 'stay':'AUTONOMY_MODE', 'aborted':'DONE'})
#StateMachine.add('TELEOP_MODE', MonitorState('/click/start_button', Empty, monitor_cb),
# transitions={'invalid':'DONE', 'valid':'TELEOP_MODE', 'preempted':'DONE'})
sis = IntrospectionServer('smach_introspection_server', top_sm, '/COMPETITION_SMACH')
sis.start()
top_sm.execute()
rospy.spin()
sis.stop()
def main():
rospy.init_node("iterator_tutorial")
sm_iterator = construct_sm()
# Run state machine introspection server for smach viewer
intro_server = IntrospectionServer('iterator_tutorial',sm_iterator,'/ITERATOR_TUTORIAL')
intro_server.start()
outcome = sm_iterator.execute()
rospy.spin()
intro_server.stop()
def execute_sm(self):
'''
运行状态即机
'''
intro_server = IntrospectionServer('sm_fetchtea', self.sm_fetchtea, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm_fetchtea.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def main():
rospy.init_node("iterator_tutorial")
sm_iterator = construct_sm()
# Run state machine introspection server for smach viewer
intro_server = IntrospectionServer('iterator_tutorial', sm_iterator,
'/ITERATOR_TUTORIAL')
intro_server.start()
outcome = sm_iterator.execute()
rospy.spin()
intro_server.stop()
def main():
rospy.init_node('pr2_touch_simple')
smts = SMTouchSimple()
sm = smts.get_sm_basic()
rospy.sleep(1)
sis = IntrospectionServer('touch_simple', sm, '/INPUT_START_CLICK')
sis.start()
outcome = sm.execute()
sis.stop()
def main():
rospy.init_node('pr2_touch_simple')
smts = SMTouchSimple()
sm = smts.get_sm_basic()
rospy.sleep(1)
sis = IntrospectionServer('touch_simple', sm, '/INPUT_START_CLICK')
sis.start()
outcome = sm.execute()
sis.stop()
def main():
rospy.init_node('register_head_ellipse')
smer = SMEllipsoidRegistration()
sm = smer.get_sm()
rospy.sleep(1)
sis = IntrospectionServer('register_head', sm, '/UNFREEZE_PC')
sis.start()
outcome = sm.execute()
sis.stop()
def main():
rospy.init_node('register_head_ellipse')
smer = SMEllipsoidRegistration()
sm = smer.get_sm()
rospy.sleep(1)
sis = IntrospectionServer('register_head', sm, '/UNFREEZE_PC')
sis.start()
outcome = sm.execute()
sis.stop()
def main():
rospy.init_node('smach_sm_touch_face')
smna = SMNavApproach()
sm = smna.get_sm()
rospy.sleep(1)
sis = IntrospectionServer('nav_prep', sm, '/SM_NAV_PREP')
sis.start()
outcome = sm.execute()
sis.stop()
def main():
rospy.init_node('smach_sm_touch_face')
smna = SMNavApproach()
sm = smna.get_sm()
rospy.sleep(1)
sis = IntrospectionServer('nav_prep', sm, '/SM_NAV_PREP')
sis.start()
outcome = sm.execute()
sis.stop()
def main():
rospy.init_node('tinker_mission_navigation')
rospy.loginfo(colored('starting navigation task ...', 'green'))
# load waypoints from xml
pose_list = TKpos.PoseList.parse(open(rospy.get_param('~waypoint_xml'), 'r'))
for pose in pose_list.pose:
WayPointGoalState.waypoint_dict[pose.name] = TKpos.Pose.X(pose)
# Main StateMachine
state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with state:
StateMachine.add('Start_Button', MonitorStartButtonState(),
transitions={'valid': 'Start_Button', 'invalid': 'Sequence'})
sequence = Sequence(outcomes=['succeeded', 'preempted', 'aborted'],
connector_outcome='succeeded')
with sequence:
# Sequence.add('GoToWaypoin1', WayPointGoalState('waypoint1'), transitions={'aborted': 'GoToWaypoin1'})
# Sequence.add('ArriveWaypoint1', SpeakState('I have arrived at way point one'))
# Sequence.add('GoToWaypoin2', WayPointGoalState('waypoint2'),
# transitions={'succeeded': 'ArriveWaypoint2', 'aborted': 'Obstacle'})
# Sequence.add('Obstacle', SpeakState('Obstacle in front of me'))
# Sequence.add('ObstacleDelay', DelayState(10),
# transitions={'succeeded': 'GoToWaypoin2'})
# Sequence.add('ArriveWaypoint2', SpeakState('I have arrived at way point two'))
Sequence.add('GoToWaypoin3', WayPointGoalState('waypoint3'), transitions={'aborted': 'GoToWaypoin3'})
Sequence.add('ArriveWaypoint3', SpeakState('I have arrived at way point three'))
Sequence.add('StopCommandAndGo', SpeakState('Please GO. If you want to stop, say stop tinker'))
Sequence.add('Train_human', FollowTrainState())
follow_concurrence = Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='succeeded', child_termination_cb=lambda x: True, input_keys=[])
with follow_concurrence:
Concurrence.add('FollowMe', FollowMeState())
# Concurrence.add('KeyWordsRecognition', KeywordsRecognizeState('stop'))
Sequence.add('Follow_concurrence', follow_concurrence)
Sequence.add('GoToWaypoin3Again', WayPointGoalState('waypoint3'), transitions={'aborted': 'GoToWaypoin3Again'})
Sequence.add('ArriveWaypoint3Again', SpeakState('I am back at way point three'))
Sequence.add('GoOut', WayPointGoalState('out'), transitions={'aborted': 'GoOut'})
StateMachine.add('Sequence', sequence, {'succeeded': 'succeeded', 'aborted': 'aborted'})
# Run state machine introspection server for smach viewer
intro_server = IntrospectionServer('tinker_mission_navigation', state, '/tinker_mission_navigation')
intro_server.start()
outcome = state.execute()
rospy.spin()
intro_server.stop()
def main():
rospy.init_node('bbauv_suavc_smach')
sauvc = smach.StateMachine(outcomes=['succeeded', 'preempted'])
with sauvc:
smach.StateMachine.add('COUNTDOWN_START',
Countdown(1.0),
transitions={'succeeded': 'SMACH_TRUE'})
smach.StateMachine.add('SMACH_TRUE',
SmachSwitch(True),
transitions={'succeeded': 'VISION_TRACK_TRUE'})
smach.StateMachine.add('VISION_TRACK_TRUE',
TrackerSwitch(False),
transitions={'succeeded': 'NAV_TRUE'})
smach.StateMachine.add('NAV_TRUE',
NavigationSwitch(True),
transitions={'succeeded': 'TOPSIDE_FALSE'})
smach.StateMachine.add('TOPSIDE_FALSE',
TopsideSwitch(False),
transitions={'succeeded': 'COUNTDOWN_MISSION'})
smach.StateMachine.add('COUNTDOWN_MISSION',
Countdown(180),
transitions={'succeeded': 'TOPSIDE_TRUE'})
smach.StateMachine.add('TOPSIDE_TRUE',
TopsideSwitch(True),
transitions={'succeeded': 'COUNTDOWN_END'})
smach.StateMachine.add('COUNTDOWN_END',
Countdown(0.1),
transitions={'succeeded': 'VISION_TRACK_FALSE'})
smach.StateMachine.add('VISION_TRACK_FALSE',
TrackerSwitch(False),
transitions={'succeeded': 'NAV_FALSE'})
smach.StateMachine.add('NAV_FALSE',
NavigationSwitch(False),
transitions={'succeeded': 'SMACH_FALSE'})
smach.StateMachine.add('SMACH_FALSE',
SmachSwitch(False),
transitions={'succeeded': 'succeeded'})
sis = IntrospectionServer('my_introspserver', sauvc, '/SM_ROOT')
sis.start()
outcome = sauvc.execute()
rospy.on_shutdown(shutdown)
rospy.spin()
sis.stop()
def __init__(self):
rospy.init_node('patrol_smach', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Initialize a number of parameters and variables
setup_task_environment(self)
# Track success rate of getting to the goal locations
self.n_succeeded = 0
self.n_aborted = 0
self.n_preempted = 0
# A list to hold then navigation waypoints
nav_states = list()
# Turn the waypoints into SMACH states
for waypoint in self.waypoints:
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = waypoint
move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(10.0),
server_wait_timeout=rospy.Duration(10.0))
nav_states.append(move_base_state)
# Initialize the patrol state machine
self.sm_patrol = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Add the states to the state machine with the appropriate transitions
with self.sm_patrol:
StateMachine.add('NAV_STATE_0', nav_states[0], transitions={'succeeded':'NAV_STATE_1','aborted':'NAV_STATE_1','preempted':'NAV_STATE_1'})
StateMachine.add('NAV_STATE_1', nav_states[1], transitions={'succeeded':'NAV_STATE_2','aborted':'NAV_STATE_2','preempted':'NAV_STATE_2'})
StateMachine.add('NAV_STATE_2', nav_states[2], transitions={'succeeded':'NAV_STATE_3','aborted':'NAV_STATE_3','preempted':'NAV_STATE_3'})
StateMachine.add('NAV_STATE_3', nav_states[3], transitions={'succeeded':'NAV_STATE_4','aborted':'NAV_STATE_4','preempted':'NAV_STATE_4'})
StateMachine.add('NAV_STATE_4', nav_states[0], transitions={'succeeded':'','aborted':'','preempted':''})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('patrol', self.sm_patrol, '/SM_ROOT')
intro_server.start()
# Execute the state machine for the specified number of patrols
while (self.n_patrols == -1 or self.patrol_count < self.n_patrols) and not rospy.is_shutdown():
sm_outcome = self.sm_patrol.execute()
self.patrol_count += 1
rospy.loginfo("FINISHED PATROL LOOP: " + str(self.patrol_count))
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def main():
rospy.init_node("sm_registration_setup")
smrs = SMRegistrationSetup()
sm = smrs.get_sm()
if False:
sm.set_initial_state(['SETUP_TASK_CONTROLLER'])
rospy.sleep(1)
sis = IntrospectionServer('registration_setup', sm, '/NAV_APPROACH')
sis.start()
outcome = sm.execute()
sis.stop()
def __init__(self):
rospy.init_node('final_project_kl', anonymous=False)
rospy.on_shutdown(self.shutdown)
self.keyPointManager = KeyPointManager()
# Create the nav_patrol state machine using a Concurrence container
self.nav_patrol = Concurrence(
outcomes=['succeeded', 'key_points', 'stop'],
default_outcome='succeeded',
outcome_map={'key_points': {
'MONITOR_AR': 'invalid'
}},
child_termination_cb=self.concurrence_child_termination_cb,
outcome_cb=self.concurrence_outcome_cb)
# Add the sm_nav machine and a AR Tag MonitorState to the nav_patrol machine
with self.nav_patrol:
Concurrence.add('SM_NAV', Patrol().getSM())
Concurrence.add(
'MONITOR_AR',
MonitorState('/ar_pose_marker', AlvarMarkers, self.ar_cb))
# Create the top level state machine
self.sm_top = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
self.sm_top.userdata.sm_ar_tag = None
with self.sm_top:
StateMachine.add('PATROL',
self.nav_patrol,
transitions={
'succeeded': 'PATROL',
'key_points': 'PATROL_KEYPOINTS',
'stop': 'STOP'
})
StateMachine.add('PATROL_KEYPOINTS',
PatrolThroughKeyPoints(
self.keyPointManager).getSM(),
transitions={'succeeded': 'STOP'})
StateMachine.add('STOP', Stop(), transitions={'succeeded': ''})
intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm_top.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def main():
rospy.init_node("sm_registration_setup")
smrs = SMRegistrationSetup()
sm = smrs.get_sm()
if False:
sm.set_initial_state(['SETUP_TASK_CONTROLLER'])
rospy.sleep(1)
sis = IntrospectionServer('registration_setup', sm, '/NAV_APPROACH')
sis.start()
outcome = sm.execute()
sis.stop()
def main():
rospy.init_node('tinker_mission_follow')
rospy.loginfo(colored('starting follow and guide task ...', 'green'))
# Main StateMachine
state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with state:
StateMachine.add('Start_Button',
MonitorStartButtonState(),
transitions={
'valid': 'Start_Button',
'invalid': '1_Start'
})
StateMachine.add('1_Start',
MonitorKinectBodyState(),
transitions={
'valid': '1_Start',
'invalid': 'Sequence'
})
sequence = Sequence(outcomes=['succeeded', 'preempted', 'aborted'],
connector_outcome='succeeded')
with sequence:
follow_concurrence = Concurrence(
outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='succeeded',
child_termination_cb=lambda x: True,
input_keys=[])
with follow_concurrence:
Concurrence.add('FollowMe', FollowMeState())
Concurrence.add('KeyWordsRecognition',
KeywordsRecognizeState('stop'))
Sequence.add('Follow_concurrence', follow_concurrence)
StateMachine.add('Sequence', sequence, {
'succeeded': 'succeeded',
'aborted': 'aborted'
})
# Run state machine introspection server for smach viewer
intro_server = IntrospectionServer('tinker_mission_navigation', state,
'/tinker_mission_navigation')
intro_server.start()
outcome = state.execute()
rospy.spin()
intro_server.stop()
def main():
rospy.init_node('mission_planner')
sm = smach.StateMachine(
outcomes=['mission_complete', 'mission_failed', 'aborted'])
theta = 0
with sm:
Sink(sm, 'DEPTH1', 545, 'mission_complete')
sis = IntrospectionServer('ZARNA_MISSION_PLANNER', sm, '/START_ZARNA')
sis.start()
outcome = sm.execute()
rospy.spin()
sis.stop()
def execute_smach_container(smach_container, enable_introspection=False,
name='/SM_ROOT', userdata=UserData()):
if not rospy.core.is_initialized():
rospy.init_node('smach')
if enable_introspection:
# Create and start the introspection server
sis = IntrospectionServer('smach_executor', smach_container, name)
sis.start()
outcome = smach_container.execute(userdata)
_logger.info("smach outcome: %s", outcome)
if enable_introspection:
sis.stop()
return outcome
def __init__(self):
rospy.init_node('monitor_fake_battery', anonymous=False)
rospy.on_shutdown(self.shutdown)
# Set the low battery threshold (between 0 and 100)
self.low_battery_threshold = rospy.get_param('~low_battery_threshold',
50)
# Initialize the state machine
sm_battery_monitor = StateMachine(outcomes=[])
with sm_battery_monitor:
# Add a MonitorState to subscribe to the battery level topic
StateMachine.add(
'MONITOR_BATTERY',
MonitorState('battery_level', Float32, self.battery_cb),
transitions={
'invalid': 'RECHARGE_BATTERY',
'valid': 'MONITOR_BATTERY',
'preempted': 'MONITOR_BATTERY'
},
)
# Add a ServiceState to simulate a recharge using the set_battery_level service
StateMachine.add('RECHARGE_BATTERY',
ServiceState(
'battery_simulator/set_battery_level',
SetBatteryLevel,
request=100),
transitions={
'succeeded': 'MONITOR_BATTERY',
'aborted': 'MONITOR_BATTERY',
'preempted': 'MONITOR_BATTERY'
})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('monitor_battery',
sm_battery_monitor, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = sm_battery_monitor.execute()
intro_server.stop()
def main():
rospy.init_node('mission_planner')
sm = smach.StateMachine(
outcomes=['mission_complete', 'mission_failed', 'aborted'])
theta = 0
with sm:
Sink(sm, 'SINK1', 525, 'HEADING1')
Heading(sm, 'HEADING1', 90, 'mission_complete')
#Forward(sm, 'FORWARD1', 12, 'mission_complete')
sis = IntrospectionServer('ZARNA_MISSION_PLANNER', sm, '/START_ZARNA')
sis.start()
outcome = sm.execute()
rospy.spin()
sis.stop()
def main():
rospy.init_node('tinker_mission_person_recognition')
rospy.loginfo(colored('starting person recognition task ...', 'green'))
# Main StateMachine
state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with state:
StateMachine.add('0_wait_for_start', MonitorStartButtonState(), transitions={'valid': '0_wait_for_start','invalid': '0_arm_mode'})
StateMachine.add('0_arm_mode', ArmModeState(ArmModeState.Arm_Mode_Kinect), transitions={'succeeded':'0_wait_for_human'})
StateMachine.add('0_wait_for_human', SpeakState('I am tinker , I am waiting for operator'),
transitions={'succeeded': '1_Start'})
StateMachine.add('1_Start', MonitorKinectBodyState(), transitions={'valid':'1_Start', 'invalid':'Sequence'})
sequence = Sequence(outcomes=['succeeded', 'preempted', 'aborted'],
connector_outcome='succeeded')
with sequence:
Sequence.add('2_1_train_speak', SpeakState('please look at the camera'))
Sequence.add('2_2_train', TrainPersonState())
Sequence.add('2_3_train_finish', SpeakState('I have memorized you, thanks'))
Sequence.add('3_wait', DelayState(10))
Sequence.add('4_turn_back', ChassisSimpleMoveState(theta=3.23))
Sequence.add('4_1_turn_back', ChassisSimpleMoveState(x=-1.0))
Sequence.add('5_1_find_operator', FindPersonState())
Sequence.add('5_2_point_operator', MoveArmState(offset=Point(0, 0, 0)), remapping={'target':'person_pose'})
Sequence.add('5_3_say', SpeakState('I have found you'))
Sequence.add('5_4_wait', DelayState(5))
StateMachine.add('Sequence', sequence, {'succeeded': 'Sequence_reset', 'aborted': 'Sequence_reset'})
sequence_reset = Sequence(outcomes=['succeeded', 'preempted', 'aborted'],
connector_outcome='succeeded')
with sequence_reset:
Sequence.add('6_1_arm_init', ArmModeState(ArmModeState.Arm_Mode_Init))
Sequence.add('6_2_report', GenerateReportState(image='human_result.png', text='human_result.txt'))
Sequence.add('6_3_finish', SpeakState('task finished'))
StateMachine.add('Sequence_reset', sequence_reset, {'succeeded': 'succeeded', 'aborted': 'aborted'})
# Run state machine introspection server for smach viewer
intro_server = IntrospectionServer('tinker_mission_person_recognition', state, '/tinker_mission_person_recognition')
intro_server.start()
outcome = state.execute()
rospy.spin()
intro_server.stop()
def execute_smach_container(smach_container,
enable_introspection=False,
name='/SM_ROOT',
userdata=UserData()):
if not rospy.core.is_initialized():
rospy.init_node('smach')
if enable_introspection:
# Create and start the introspection server
sis = IntrospectionServer('smach_executor', smach_container, name)
sis.start()
outcome = smach_container.execute(userdata)
_logger.info("smach outcome: %s", outcome)
if enable_introspection:
sis.stop()
return outcome
def __init__(self):
rospy.init_node('random_patrol', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Initialize a number of parameters and variables
setup_task_environment(self)
# Initialize the patrol state machine
self.sm_patrol = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# Set the userdata.waypoints variable to the pre-defined waypoints
self.sm_patrol.userdata.waypoints = self.waypoints
# Add the states to the state machine with the appropriate transitions
with self.sm_patrol:
StateMachine.add('PICK_WAYPOINT',
PickWaypoint(),
transitions={'succeeded': 'NAV_WAYPOINT'},
remapping={'waypoint_out': 'patrol_waypoint'})
StateMachine.add('NAV_WAYPOINT',
Nav2Waypoint(),
transitions={
'succeeded': 'PICK_WAYPOINT',
'aborted': 'PICK_WAYPOINT',
'preempted': 'PICK_WAYPOINT'
},
remapping={'waypoint_in': 'patrol_waypoint'})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('patrol', self.sm_patrol,
'/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm_patrol.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def __init__(self):
rospy.init_node('speech_test')
rospy.on_shutdown(self.shutdown)
self.test = StateMachine(outcomes=['succeeded', 'aborted', 'error'])
with self.test:
self.test.userdata.target = list()
self.test.userdata.action = list()
self.test.userdata.task_num = 0
self.test.userdata.sentences = 'i am shall mon'
#得到命令
StateMachine.add('GETTASK',
GetTask(),
transitions={
'succeeded': 'SPEAK',
'aborted': 'aborted'
},
remapping={
'target': 'target',
'action': 'action',
'task_num': 'task_num'
})
StateMachine.add('SPEAK',
Speak(),
transitions={
'succeeded': 'ANSWER',
'aborted': 'aborted',
'error': 'error'
},
remapping={'sentences': 'sentences'})
StateMachine.add('ANSWER',
Anwser(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
rospy.loginfo(self.test.userdata.target)
rospy.loginfo(self.test.userdata.action)
rospy.loginfo(self.test.userdata.task_num)
intro_server = IntrospectionServer('test', self.test, '/SM_ROOT')
intro_server.start()
out = self.test.execute()
intro_server.stop()
def main():
rospy.init_node('smach_example_state_machine')
# Create the top level SMACH state machine
sm_top = StateMachine(outcomes=['outcome6'])
# Open the countainer
with sm_top:
StateMachine.add('BAS', Bas(), transitions={'outcome3': 'CON'})
# Create the sub SMACH state machine
sm_con = Concurrence(
outcomes=['outcome4', 'outcome5'],
default_outcome='outcome4',
outcome_map={'outcome5': {
'FOO': 'outcome2',
'BAR': 'outcome1'
}})
# Open the countainer
with sm_con:
# Add states to the container
Concurrence.add('FOO', Foo())
Concurrence.add('BAR', Bar())
StateMachine.add('CON',
sm_con,
transitions={
'outcome4': 'CON',
'outcome5': 'outcome6'
})
sis = IntrospectionServer('example', sm_top, '/SM_PATH')
sis.start()
# Execute SMACH plan
outcome = sm_top.execute()
rospy.spin()
sis.stop()
def main():
rospy.init_node('mission_planner')
sm = smach.StateMachine(outcomes=['mission_complete', 'mission_failed', 'aborted'])
theta = 0
with sm:
Sink (sm, 'SINK1', 547, 'HEADING1')
Heading(sm, 'HEADING1', 90, 'mission_complete')
#it = ImageTask() # Image Task should return User data which should be
# further mapped to Heading etc states
#it.init(sm)
sis = IntrospectionServer('ZARNA_MISSION_PLANNER', sm, '/START_ZARNA')
# start introspection server by - rosrun smach_viewer smach_viewer.py
sis.start()
outcome = sm.execute()
rospy.spin()
sis.stop()
def main():
rospy.init_node('tinker_RIPS')
rospy.loginfo(colored('starting RIPS task ...', 'green'))
# load waypoints from xml
pose_list = TKpos.PoseList.parse(open(rospy.get_param('~waypoint_xml'), 'r'))
for pose in pose_list.pose:
WayPointGoalState.waypoint_dict[pose.name] = TKpos.Pose.X(pose)
# Main StateMachine
state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with state:
StateMachine.add('Start_Button', MonitorStartButtonState(),
transitions={'valid': 'Start_Button', 'invalid': 'Sequence'})
sequence_1 = Sequence(outcomes=['succeeded', 'preempted', 'aborted'],
connector_outcome='succeeded')
with sequence_1:
Sequence.add('GoToWaypoin1', WayPointGoalState('waypoint1'), transitions={'aborted': 'GoToWaypoin1'})
Sequence.add('ArriveWaypoint1', SpeakState('Wait for continue'))
StateMachine.add('Sequence', sequence_1, {'succeeded': 'Continue', 'aborted': 'aborted'})
StateMachine.add('Continue', MonitorStartButtonState(),
transitions={'valid': 'Continue', 'invalid': 'Sequence_2'})
sequence_2 = Sequence(outcomes=['succeeded', 'preempted', 'aborted'],
connector_outcome='succeeded')
with sequence_2:
Sequence.add('GoToWaypoin2', WayPointGoalState('waypoint2'), transitions={'aborted': 'GoToWaypoin2'})
Sequence.add('ArriveWaypoint2', SpeakState('I have arrived at way point two'))
Sequence.add('GoToWaypoin3', WayPointGoalState('waypoint3'), transitions={'aborted': 'GoToWaypoin3'})
Sequence.add('ArriveWaypoint3', SpeakState('I have arrived at way point three'))
StateMachine.add('Sequence_2', sequence_2, {'succeeded': 'succeeded', 'aborted': 'aborted'})
# Run state machine introspection server for smach viewer
intro_server = IntrospectionServer('tinker_mission_navigation', state, '/tinker_mission_navigation')
intro_server.start()
outcome = state.execute()
rospy.spin()
intro_server.stop()
def main():
sm = smach.StateMachine(
outcomes=['mission_complete', 'mission_failed', 'aborted'])
theta = 0
with sm:
# smach.StateMachine.add('TORPEDO', Torpedo(), transitions={'torpedo_success':'mission_complete'})
# smach.StateMachine.add('DETECTBUOY', DetectBuoy(), transitions={'buoy_success':'mission_complete', 'buoy_retry': 'DETECTBUOY'})
Sink(sm, 'SINK1', 515, 'HEADING1')
Heading(sm, 'HEADING1', 75, 'FORWARD1')
Forward(sm, 'FORWARD1', 6, 'FORWARD2')
Forward(sm, 'FORWARD2', 12, 'mission_complete')
#Heading(sm, 'HEADING2', 90, 'FORWARD2')
#Forward(sm, 'FORWARD2', 10, 'HEADING3')
#Heading(sm, 'HEADING3', 90, 'FORWARD3')
#Forward(sm, 'FORWARD3', 15, 'mission_complete')
#DetectBuoy(sm, 'DETECTBUOY', 'FORWARD2')
#Forward(sm, 'FORWARD2', 14, 'HEADING2')
#Heading(sm, 'HEADING2', theta + 45, 'FORWARD3')
#Forward(sm, 'FORWARD3', 14, 'SWAY1')
#torpedo fire
#Sway(sm, 'SWAY1', -5, 'FORWARD4')
#Forward(sm, 'FORWARD4', 10, 'SINK2')
#Sink (sm, 'SINK2', 525, 'mission_complete') #resurface
#it = ImageTask() # Image Task should return User data which should be
# further mapped to Heading etc states
#it.init(sm)
sis = IntrospectionServer('ZARNA_MISSION_PLANNER', sm, '/START_ZARNA')
# start introspection server by - rosrun smach_viewer smach_viewer.py
sis.start()
outcome = sm.execute()
boolean = 1
sis.stop()
rospy.loginfo("Mission Complete")
def __init__(self):
rospy.init_node('final_project_kl', anonymous=False)
rospy.on_shutdown(self.shutdown)
self.keyPointManager = KeyPointManager()
# Create the nav_patrol state machine using a Concurrence container
self.nav_patrol = Concurrence(outcomes=['succeeded', 'key_points', 'stop'],
default_outcome='succeeded',
outcome_map = {'key_points' : {'MONITOR_AR':'invalid'}},
child_termination_cb=self.concurrence_child_termination_cb,
outcome_cb=self.concurrence_outcome_cb)
# Add the sm_nav machine and a AR Tag MonitorState to the nav_patrol machine
with self.nav_patrol:
Concurrence.add('SM_NAV', Patrol().getSM())
Concurrence.add('MONITOR_AR',MonitorState('/ar_pose_marker', AlvarMarkers, self.ar_cb))
# Create the top level state machine
self.sm_top = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
self.sm_top.userdata.sm_ar_tag = None
with self.sm_top:
StateMachine.add('PATROL', self.nav_patrol, transitions={'succeeded':'PATROL', 'key_points':'PATROL_KEYPOINTS', 'stop':'STOP'})
StateMachine.add('PATROL_KEYPOINTS', PatrolThroughKeyPoints(self.keyPointManager).getSM(), transitions={'succeeded':'STOP'})
StateMachine.add('STOP', Stop(), transitions={'succeeded':''})
intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm_top.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def __init__(self):
rospy.init_node('random_patrol', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Initialize a number of parameters and variables
setup_task_environment(self)
# Initialize the patrol state machine
self.sm_patrol = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Set the userdata.waypoints variable to the pre-defined waypoints
self.sm_patrol.userdata.waypoints = self.waypoints
# Add the states to the state machine with the appropriate transitions
with self.sm_patrol:
StateMachine.add('PICK_WAYPOINT', PickWaypoint(),
transitions={'succeeded':'NAV_WAYPOINT'},
remapping={'waypoint_out':'patrol_waypoint'})
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'PICK_WAYPOINT',
'aborted':'PICK_WAYPOINT',
'preempted':'PICK_WAYPOINT'},
remapping={'waypoint_in':'patrol_waypoint'})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('patrol', self.sm_patrol, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm_patrol.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def run_smach(self):
sm = smach.StateMachine(outcomes=["preempted", "aborted", "succeeded"])
with sm:
# @smach.cb_interface(outcomes=['map_build_successeded', 'map_build_failed'])
# def build_map_cb(userdata,status,result):
# rosprint("Result of build_map:{}".format(result))
# if result.message == "build_map_failed":
# return "map_build_failed"
# elif result.message == "map_build_successeded":
# return "map_build_successeded"
#define move goal
move_goal = MoveGoal()
move_goal.direction = "ccw"
move_goal.duration = 1
move_goal.speed = 0.1
#define states
smach.StateMachine.add("BUILD_MAP",
SimpleActionState("build_map",
BuildMapAction),
transitions={"aborted": "MOVE"})
#"succeeded": "BUILD_MAP"})
smach.StateMachine.add("MOVE",
SimpleActionState("move",
MoveAction,
goal=move_goal),
transitions={"succeeded": "BUILD_MAP"})
sis = IntrospectionServer("player_node", sm, "/SM_ROOT")
sis.start()
outcome = sm.execute()
rospy.spin()
sis.stop()
def main():
rospy.init_node('smach_sm_touch_face')
if sys.argv[1] == 'nav_only':
smtf = SMTouchFace()
sm = smtf.get_nav_prep_sm()
rospy.sleep(1)
sis = IntrospectionServer('nav_prep', sm, '/SM_NAV_PREP')
sis.start()
outcome = sm.execute()
sis.stop()
elif sys.argv[1] == 'full_demo':
smtf = SMTouchFace()
sm_nav = smtf.get_nav_prep_sm()
sm_touch = smtf.get_touch_sm()
sm_full = smach.StateMachine(outcomes=['succeeded','preempted','shutdown', 'aborted'])
with sm_full:
smach.StateMachine.add(
'SM_NAV_PREP',
sm_nav,
transitions={'succeeded' : 'SM_TOUCH_FACE'})
smach.StateMachine.add(
'SM_TOUCH_FACE',
sm_touch)
rospy.sleep(1)
sis = IntrospectionServer('nav_prep', sm_full, '/SM_TOUCH_FACE_FULL')
sis.start()
outcome = sm_full.execute()
sis.stop()
elif sys.argv[1] == 'touch_only':
smtf = SMTouchFace()
sm = smtf.get_touch_sm()
rospy.sleep(1)
sis = IntrospectionServer('touch_face', sm, '/SM_TOUCH_FACE')
sis.start()
outcome = sm.execute()
sis.stop()
def __init__(self):
rospy.init_node('patrol_smach', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Initialize a number of parameters and variables
self.init()
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait up to 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move_base action server")
# Track success rate of getting to the goal locations
self.n_succeeded = 0
self.n_aborted = 0
self.n_preempted = 0
self.n_patrols = 2
# Turn the waypoints into SMACH states
nav_states = list()
for waypoint in self.waypoints:
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = waypoint
move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(10.0),
server_wait_timeout=rospy.Duration(10.0))
nav_states.append(move_base_state)
move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(10.0))
# Initialize the top level state machine
self.sm = StateMachine(outcomes=['succeeded','aborted','preempted'])
with self.sm:
# Initialize the iterator
self.sm_patrol_iterator = Iterator(outcomes = ['succeeded','preempted','aborted'],
input_keys = [],
it = lambda: range(0, self.n_patrols),
output_keys = [],
it_label = 'index',
exhausted_outcome = 'succeeded')
with self.sm_patrol_iterator:
# Initialize the patrol state machine
self.sm_patrol = StateMachine(outcomes=['succeeded','aborted','preempted','continue'])
# Add the states to the state machine with the appropriate transitions
with self.sm_patrol:
StateMachine.add('NAV_STATE_0', nav_states[0], transitions={'succeeded':'NAV_STATE_1','aborted':'NAV_STATE_1','preempted':'NAV_STATE_1'})
StateMachine.add('NAV_STATE_1', nav_states[1], transitions={'succeeded':'NAV_STATE_2','aborted':'NAV_STATE_2','preempted':'NAV_STATE_2'})
StateMachine.add('NAV_STATE_2', nav_states[2], transitions={'succeeded':'NAV_STATE_3','aborted':'NAV_STATE_3','preempted':'NAV_STATE_3'})
StateMachine.add('NAV_STATE_3', nav_states[3], transitions={'succeeded':'NAV_STATE_4','aborted':'NAV_STATE_4','preempted':'NAV_STATE_4'})
StateMachine.add('NAV_STATE_4', nav_states[0], transitions={'succeeded':'continue','aborted':'continue','preempted':'continue'})
# Close the sm_patrol machine and add it to the iterator
Iterator.set_contained_state('PATROL_STATE', self.sm_patrol, loop_outcomes=['continue'])
# Close the top level state machine
StateMachine.add('PATROL_ITERATOR', self.sm_patrol_iterator, {'succeeded':'succeeded', 'aborted':'aborted'})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('patrol', self.sm, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def __init__(self):
rospy.init_node('find_treasure', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# How long do we have to get to each waypoint?
self.move_base_timeout = rospy.get_param("~move_base_timeout", 10) #seconds
# Initialize the patrol counter
self.patrol_count = 0
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
# Wait up to 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move_base action server")
# Create a list to hold the target quaternions (orientations)
quaternions = list()
# First define the corner orientations as Euler angles
euler_angles = (pi/2, pi, 3*pi/2, 0)
# Then convert the angles to quaternions
for angle in euler_angles:
q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz')
q = Quaternion(*q_angle)
quaternions.append(q)
# Create a list to hold the waypoint poses
self.waypoints = list()
# Append each of the four waypoints to the list. Each waypoint
# is a pose consisting of a position and orientation in the map frame.
# -0.163200, 0.044660, 0.193186
self.waypoints.append(Pose(Point(0.9579, 1.8710, 0.0), quaternions[3]))
self.waypoints.append(Pose(Point(0.7555, 0.0692, 0.0), quaternions[1]))
self.waypoints.append(Pose(Point(-0.72511, 0.4952, 0.0), quaternions[0]))
self.waypoints.append(Pose(Point(0.167730, 2.18168, 0.0), quaternions[2]))
position_locations = list()
position_locations.append(('position1', self.waypoints[0]))
position_locations.append(('position2', self.waypoints[1]))
position_locations.append(('position3', self.waypoints[2]))
position_locations.append(('position4', self.waypoints[3]))
self.position_locations = OrderedDict(position_locations)
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
rospy.loginfo("Starting Tasks")
# Initialize a number of parameters and variables
# setup_task_environment(self)
RVizUtils.get_instance().init_primary_waypoint_markers(self.waypoints)
''' Create individual state machines for assigning tasks to each position '''
# Initialize the overall state machine
self.sm_find_treasure = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Build the find treasure state machine from the nav states and treasure finding states
with self.sm_find_treasure:
# First State Machine
sm_nav = StateMachine(outcomes=['succeeded','aborted','preempted'])
sm_nav.userdata.waypoints = self.waypoints
sm_nav.userdata.waypoints_primary_count = len(self.waypoints)
rospy.loginfo(sm_nav.userdata.waypoints)
with sm_nav:
# StateMachine.add('PICK_WAYPOINT', PickWaypoint(),
# transitions={'succeeded':'NAV_WAYPOINT','aborted':'','preempted':''},
# remapping={'waypoint_out':'patrol_waypoint',
# 'waypoints_primary_count':'waypoints_primary_count'})
StateMachine.add('NAV_WAYPOINT', NavState(),
transitions={'succeeded':'NAV_WAYPOINT',
'aborted':'',
'preempted':''},
remapping={'waypoint_in':'patrol_waypoint',
'waypoints_primary_count':'waypoints_primary_count'})
# Second State Machine
sm_read_tags = StateMachine(outcomes=['valid','invalid','preempted'])
sm_read_tags.userdata.waypoints = self.waypoints
with sm_read_tags:
StateMachine.add('read_tag', ReadTagState('ar_pose_marker', AlvarMarkers),
transitions={'invalid':'read_tag', 'valid':'read_tag', 'preempted':''})
# Third State Machine
sm_detect_faces = StateMachine(outcomes=['valid','invalid','preempted'])
with sm_detect_faces:
StateMachine.add('detect_face', FaceDetectState("/camera/rgb/image_color", Image),
transitions={'invalid':'detect_face', 'valid':'detect_face', 'preempted':''})
# Forth State Machine
sm_recognize_faces = StateMachine(outcomes=['succeeded','aborted','preempted'])
with sm_recognize_faces:
StateMachine.add('recognize_face', FaceRecognitionState(), transitions={'succeeded':'', 'aborted':'', 'preempted':''})
# goal = FaceRecognitionGoal()
# goal.order_id = 1
# goal.order_argument = ''
# StateMachine.add('recognize_face', SimpleActionState('face_recognition',
# FaceRecognitionAction, goal=goal), transitions={'succeeded':'', 'aborted':'', 'preempted':''})
sm_con = Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],default_outcome='succeeded',
child_termination_cb=self.child_term_cb, outcome_cb=self.out_cb)
with sm_con:
Concurrence.add('SM_NAV', sm_nav)
Concurrence.add('SM_READ_TAGS', sm_read_tags)
# Concurrence.add('SM_DETECT_FACES', sm_detect_faces)
Concurrence.add('SM_RECOGNIZE_FACES', sm_recognize_faces)
sm_estimate_position = StateMachine(outcomes=['succeeded','aborted','preempted'])
with sm_estimate_position:
StateMachine.add('estimate_pose', PoseEstimation('ar_pose_marker', AlvarMarkers),
transitions={'succeeded':'', 'aborted':'estimate_pose', 'preempted':''})
StateMachine.add('POSE_ESTIMATE', sm_estimate_position, transitions={'succeeded':'CON','aborted':'CON','preempted':'CON'})
StateMachine.add('CON',sm_con, transitions={'succeeded':'','aborted':'','preempted':''})
# Create and start the SMACH introspection server sm_find_treasure
intro_server = IntrospectionServer('find_treasure', self.sm_find_treasure, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm_find_treasure.execute()
rospy.loginfo('the length now is')
rospy.loginfo(self.waypoints)
intro_server.stop()
def __init__(self):
rospy.init_node('smach_home_status', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
#####################################
# JO IS AWAKE
#####################################
# State machine for Jo-awake-go-sleep
self.sm_jo_awake_sleep = StateMachine(outcomes=['succeeded','aborted','preempted'])
with self.sm_jo_awake_sleep:
StateMachine.add('LOOK_WAKE', MonitorState("/JO/sleep", Empty, self.empty_cb), transitions={'valid':'GOING_SLEEP', 'preempted':'preempted', 'invalid':'GOING_SLEEP'})
StateMachine.add('GOING_SLEEP', JoGoingSleep(),
transitions={'succeeded':'LOOK_IN_BED'})
StateMachine.add('LOOK_IN_BED', MonitorState("/myo_disconnected", Empty, self.empty_cb), transitions={'valid':'IN_BED', 'preempted':'preempted', 'invalid':'IN_BED'})
StateMachine.add('IN_BED', JoInBed(),
transitions={'succeeded':'succeeded'})
# State machine for Jo-awake-bothgo-sleep
self.sm_jo_awake_bothsleep = StateMachine(outcomes=['succeeded','aborted','preempted'])
with self.sm_jo_awake_bothsleep:
StateMachine.add('LOOK_WAKE', MonitorState("/BOTH/sleep", Empty, self.empty_cb), transitions={'valid':'GOING_SLEEP', 'preempted':'preempted', 'invalid':'GOING_SLEEP'})
StateMachine.add('GOING_SLEEP', BothGoingSleep(),
transitions={'succeeded':'LOOK_IN_BED'})
StateMachine.add('LOOK_IN_BED', MonitorState("/myo_disconnected", Empty, self.empty_cb), transitions={'valid':'IN_BED', 'preempted':'preempted', 'invalid':'IN_BED'})
StateMachine.add('IN_BED', BothInBed(),
transitions={'succeeded':'succeeded'})
# State machine for Jo-awake-go-out
self.sm_jo_awake_out = StateMachine(outcomes=['succeeded','aborted','preempted'])
with self.sm_jo_awake_out:
StateMachine.add('LOOK_OUT', MonitorState("/JO/go_out", Empty, self.empty_cb), transitions={'valid':'PAUSE', 'preempted':'preempted', 'invalid':'PAUSE'})
StateMachine.add('PAUSE', Pause(),
transitions={'succeeded':'succeeded'})
# State machine for Jo-awake
self.sm_jo_awake = Concurrence(outcomes=['succeeded', 'stop', 'go_sleep', 'go_out'],
default_outcome='succeeded',
child_termination_cb=self.jo_awake_child_termination_cb,
outcome_cb=self.jo_awake_outcome_cb)
with self.sm_jo_awake:
Concurrence.add('SM_GO_TO_SLEEP', self.sm_jo_awake_sleep)
Concurrence.add('SM_BOTH_GO_TO_SLEEP', self.sm_jo_awake_bothsleep)
Concurrence.add('SM_GO_OUT', self.sm_jo_awake_out)
#####################################
# JO IS SLEEPING
#####################################
# State machine for Jo-sleep-waking
self.sm_jo_sleep_waking = StateMachine(outcomes=['succeeded','aborted','preempted'])
with self.sm_jo_sleep_waking:
StateMachine.add('WAIT_WAKE_UP', MonitorState("/JO/wake_up", Empty, self.empty_cb), transitions={'valid':'WAKING_UP', 'preempted':'preempted', 'invalid':'WAKING_UP'})
StateMachine.add('WAKING_UP', JoWakingUp(),
transitions={'succeeded':'succeeded'})
# State machine for Jo-sleep-bothwaking
self.sm_jo_sleep_bothwaking = StateMachine(outcomes=['succeeded','aborted','preempted'])
with self.sm_jo_sleep_bothwaking:
StateMachine.add('WAIT_WAKE_UP', MonitorState("/BOTH/wake_up", Empty, self.empty_cb), transitions={'valid':'WAKING_UP', 'preempted':'preempted', 'invalid':'WAKING_UP'})
StateMachine.add('WAKING_UP', BothWakingUp(),
transitions={'succeeded':'succeeded'})
# State machine for Jo-awake
self.sm_jo_sleep = Concurrence(outcomes=['succeeded','aborted','preempted', 'wake_up'],
default_outcome='succeeded',
child_termination_cb=self.jo_sleep_child_termination_cb,
outcome_cb=self.jo_sleep_outcome_cb)
with self.sm_jo_sleep:
Concurrence.add('SM_WAKE_UP', self.sm_jo_sleep_waking)
Concurrence.add('SM_BOTH_WAKE_UP', self.sm_jo_sleep_bothwaking)
#####################################
# JO IS OUT TODO
#####################################
# State machine for Jo-out-back
self.sm_jo_out_back = StateMachine(outcomes=['succeeded','aborted','preempted'])
with self.sm_jo_out_back:
StateMachine.add('WAIT_BACK_HOME', MonitorState("/JO/back_home", Empty, self.empty_cb), transitions={'valid':'WAIT_MYO', 'preempted':'preempted', 'invalid':'WAIT_MYO'})
StateMachine.add('WAIT_MYO', MonitorState("/myo_connected", Empty, self.empty_cb), transitions={'valid':'COMING_BACK', 'preempted':'preempted', 'invalid':'COMING_BACK'})
StateMachine.add('COMING_BACK', JoBackHome(),
transitions={'succeeded':'succeeded'})
# State machine for Jo-out-bothback
self.sm_jo_out_bothback = StateMachine(outcomes=['succeeded','aborted','preempted'])
with self.sm_jo_out_bothback:
StateMachine.add('WAIT_BACK_HOME', MonitorState("/BOTH/back_home", Empty, self.empty_cb), transitions={'valid':'WAIT_MYO', 'preempted':'preempted', 'invalid':'WAIT_MYO'})
StateMachine.add('WAIT_MYO', MonitorState("/myo_connected", Empty, self.empty_cb), transitions={'valid':'COMING_BACK', 'preempted':'preempted', 'invalid':'COMING_BACK'})
StateMachine.add('COMING_BACK', BothBackHome(),
transitions={'succeeded':'succeeded'})
# State machine for Jo-out
self.sm_jo_out = Concurrence(outcomes=['succeeded','aborted','preempted', 'back_home'],
default_outcome='succeeded',
child_termination_cb=self.jo_out_child_termination_cb,
outcome_cb=self.jo_out_outcome_cb)
with self.sm_jo_out:
Concurrence.add('SM_BACK_HOME', self.sm_jo_out_back)
Concurrence.add('SM_BOTH_BACK_HOME', self.sm_jo_out_bothback)
#####################################
# TOP LVL JO SM
#####################################
# State machine for JO
self.sm_jo = StateMachine(outcomes=['succeeded','aborted','preempted'])
with self.sm_jo:
StateMachine.add('AWAKE', self.sm_jo_awake, transitions={'succeeded':'succeeded', 'stop':'aborted', 'go_sleep':'SLEEP', 'go_out':'OUT'})
StateMachine.add('SLEEP', self.sm_jo_sleep, transitions={'succeeded':'succeeded', 'wake_up':'AWAKE'})
StateMachine.add('OUT', self.sm_jo_out, transitions={'succeeded':'succeeded', 'back_home':'AWAKE'})
#####################################
# TOP LVL CAROLE SM TODO
#####################################
# State machine for CAROLE
self.sm_carole = StateMachine(outcomes=['succeeded','aborted','preempted'])
with self.sm_carole:
StateMachine.add('WAIT3', MonitorState("/TEST/wait3", Empty, self.empty_cb), transitions={'valid':'PAUSE', 'preempted':'preempted', 'invalid':'PAUSE'})
StateMachine.add('PAUSE', Pause(),
transitions={'succeeded':'WAIT3',
'aborted':'aborted'})
#####################################
# TOP LVL EAT SM TODO
#####################################
# State machine for EAT
self.sm_eat = StateMachine(outcomes=['succeeded','aborted','preempted'])
with self.sm_eat:
StateMachine.add('WAIT2', MonitorState("/TEST/wait2", Empty, self.empty_cb), transitions={'valid':'PAUSE', 'preempted':'preempted', 'invalid':'PAUSE'})
StateMachine.add('PAUSE', Pause(),
transitions={'succeeded':'WAIT2',
'aborted':'aborted'})
#####################################
# TOP LVL SHOWER SM
#####################################
# State machine for SHOWER
self.sm_shower = StateMachine(outcomes=['succeeded','aborted','preempted'])
with self.sm_shower:
StateMachine.add('WAIT_SHOWER', MonitorState("/HOME/go_shower", Empty, self.empty_cb), transitions={'valid':'PREPARING_SHOWER', 'preempted':'preempted', 'invalid':'PREPARING_SHOWER'})
StateMachine.add('PREPARING_SHOWER', PreparingShower(),
transitions={'succeeded':'GO_SHOWER',
'aborted':'WAIT1'})
StateMachine.add('GO_SHOWER', GoShower(),
transitions={'succeeded':'STOP_SHOWER',
'aborted':'aborted'})
StateMachine.add('STOP_SHOWER', StopShower(),
transitions={'succeeded':'WAIT1',
'aborted':'aborted'})
#####################################
# TOP LVL SM
#####################################
# Create the top level state machine
self.sm_top = Concurrence(outcomes=['succeeded', 'stop'],
default_outcome='succeeded',
child_termination_cb=self.concurrence_child_termination_cb,
outcome_cb=self.concurrence_outcome_cb)
# Add nav_patrol, sm_recharge and a Stop() machine to sm_top
with self.sm_top:
Concurrence.add('SM_JO', self.sm_jo)
Concurrence.add('SM_CAROLE', self.sm_carole)
Concurrence.add('SM_EAT', self.sm_eat)
Concurrence.add('SM_SHOWER', self.sm_shower)
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm_top.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def __init__(self):
rospy.init_node('clean_house', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Initialize a number of parameters and variables
setup_task_environment(self)
# Turn the room locations into SMACH move_base action states
nav_states = {}
for room in self.room_locations.iterkeys():
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = self.room_locations[room]
move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(15.0),
server_wait_timeout=rospy.Duration(10.0))
nav_states[room] = move_base_state
''' Create individual state machines for assigning tasks to each room '''
# Create a state machine for the living room subtask(s)
sm_living_room = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Then add the subtask(s)
with sm_living_room:
StateMachine.add('VACUUM_FLOOR', VacuumFloor('living_room', 5), transitions={'succeeded':'','aborted':'','preempted':''})
# Create a state machine for the kitchen subtask(s)
sm_kitchen = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Then add the subtask(s)
with sm_kitchen:
StateMachine.add('MOP_FLOOR', MopFloor('kitchen', 5), transitions={'succeeded':'','aborted':'','preempted':''})
# Create a state machine for the bathroom subtask(s)
sm_bathroom = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Then add the subtasks
with sm_bathroom:
StateMachine.add('SCRUB_TUB', ScrubTub('bathroom', 7), transitions={'succeeded':'MOP_FLOOR'})
StateMachine.add('MOP_FLOOR', MopFloor('bathroom', 5), transitions={'succeeded':'','aborted':'','preempted':''})
# Create a state machine for the hallway subtask(s)
sm_hallway = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Then add the subtasks
with sm_hallway:
StateMachine.add('VACUUM_FLOOR', VacuumFloor('hallway', 5), transitions={'succeeded':'','aborted':'','preempted':''})
# Initialize the overall state machine
sm_clean_house = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Build the clean house state machine from the nav states and room cleaning states
with sm_clean_house:
StateMachine.add('START', nav_states['hallway'], transitions={'succeeded':'LIVING_ROOM','aborted':'LIVING_ROOM','preempted':'LIVING_ROOM'})
''' Add the living room subtask(s) '''
StateMachine.add('LIVING_ROOM', nav_states['living_room'], transitions={'succeeded':'LIVING_ROOM_TASKS','aborted':'KITCHEN','preempted':'KITCHEN'})
# When the tasks are done, continue on to the kitchen
StateMachine.add('LIVING_ROOM_TASKS', sm_living_room, transitions={'succeeded':'KITCHEN','aborted':'KITCHEN','preempted':'KITCHEN'})
''' Add the kitchen subtask(s) '''
StateMachine.add('KITCHEN', nav_states['kitchen'], transitions={'succeeded':'KITCHEN_TASKS','aborted':'BATHROOM','preempted':'BATHROOM'})
# When the tasks are done, continue on to the bathroom
StateMachine.add('KITCHEN_TASKS', sm_kitchen, transitions={'succeeded':'BATHROOM','aborted':'BATHROOM','preempted':'BATHROOM'})
''' Add the bathroom subtask(s) '''
StateMachine.add('BATHROOM', nav_states['bathroom'], transitions={'succeeded':'BATHROOM_TASKS','aborted':'HALLWAY','preempted':'HALLWAY'})
# When the tasks are done, return to the hallway
StateMachine.add('BATHROOM_TASKS', sm_bathroom, transitions={'succeeded':'HALLWAY','aborted':'HALLWAY','preempted':'HALLWAY'})
''' Add the hallway subtask(s) '''
StateMachine.add('HALLWAY', nav_states['hallway'], transitions={'succeeded':'HALLWAY_TASKS','aborted':'','preempted':''})
# When the tasks are done, stop
StateMachine.add('HALLWAY_TASKS', sm_hallway, transitions={'succeeded':'','aborted':'','preempted':''})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('clean_house', sm_clean_house, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = sm_clean_house.execute()
if len(task_list) > 0:
message = "Ooops! Not all chores were completed."
message += "The following rooms need to be revisited: "
message += str(task_list)
else:
message = "All chores complete!"
rospy.loginfo(message)
easygui.msgbox(message, title="Finished Cleaning")
intro_server.stop()
return ggoal
smach.StateMachine.add(
'GRASP',
SimpleActionState( 'overhead_grasp',
OverheadGraspAction,
goal_cb = grasp_goal_cb,
input_keys = ['grasp_poses']),
remapping = {'grasp_poses':'object_poses'},
transitions = { 'succeeded': 'succeeded',
'aborted':'THREE_TRIES' })
return sm
if __name__ == '__main__':
rospy.init_node('smach_sm_grasp')
sm = sm_grasp()
sis = IntrospectionServer('Grasping', sm, '/SM_GRASPING')
sis.start()
outcome = sm.execute()
sis.stop()
def __init__(self):
rospy.init_node('patrol_smach_concurrence', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Initialize a number of parameters and variables
setup_task_environment(self)
# Track success rate of getting to the goal locations
self.n_succeeded = 0
self.n_aborted = 0
self.n_preempted = 0
# A variable to hold the last/current navigation goal
self.last_nav_state = None
# A flag to indicate whether or not we are rechargin
self.recharging = False
# A list to hold then navigation goa
nav_states = list()
# Turn the waypoints into SMACH states
for waypoint in self.waypoints:
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = waypoint
move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(40.0),
server_wait_timeout=rospy.Duration(10.0))
nav_states.append(move_base_state)
# Create a MoveBaseAction state for the docking station
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = self.docking_station_pose
nav_docking_station = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(40.0),
server_wait_timeout=rospy.Duration(10.0))
# Initialize the navigation state machine
self.sm_nav = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
# Add the nav states to the state machine with the appropriate transitions
with self.sm_nav:
StateMachine.add('NAV_STATE_0', nav_states[0], transitions={'succeeded':'NAV_STATE_1','aborted':'NAV_STATE_1'})
StateMachine.add('NAV_STATE_1', nav_states[1], transitions={'succeeded':'NAV_STATE_2','aborted':'NAV_STATE_2'})
StateMachine.add('NAV_STATE_2', nav_states[2], transitions={'succeeded':'NAV_STATE_3','aborted':'NAV_STATE_3'})
StateMachine.add('NAV_STATE_3', nav_states[3], transitions={'succeeded':'NAV_STATE_4','aborted':'NAV_STATE_4'})
StateMachine.add('NAV_STATE_4', nav_states[0], transitions={'succeeded':'','aborted':''})
# Register a callback function to fire on state transitions within the sm_nav state machine
self.sm_nav.register_transition_cb(self.nav_transition_cb, cb_args=[])
# Initialize the recharge state machine
self.sm_recharge = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_recharge:
StateMachine.add('NAV_DOCKING_STATION', nav_docking_station, transitions={'succeeded':''})
#StateMachine.add('RECHARGE_BATTERY', ServiceState('battery_simulator/set_battery_level', SetBatteryLevel, 100, response_cb=self.recharge_cb),
#transitions={'succeeded':''})
# Create the nav_patrol state machine using a Concurrence container
self.nav_patrol = Concurrence(outcomes=['succeeded', 'recharge', 'stop'],
default_outcome='succeeded',
child_termination_cb=self.concurrence_child_termination_cb,
outcome_cb=self.concurrence_outcome_cb)
# Add the sm_nav machine and a battery MonitorState to the nav_patrol machine
with self.nav_patrol:
Concurrence.add('SM_NAV', self.sm_nav)
Concurrence.add('MONITOR_BATTERY', MonitorState("battery_level", Float32, self.battery_cb))
# Create the top level state machine
self.sm_top = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
# Add nav_patrol, sm_recharge and a Stop() machine to sm_top
with self.sm_top:
StateMachine.add('PATROL', self.nav_patrol, transitions={'succeeded':'PATROL', 'recharge':'RECHARGE', 'stop':'STOP'})
StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded':'PATROL'})
StateMachine.add('STOP', Stop(), transitions={'succeeded':''})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm_top.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def main():
rospy.init_node('tinker_mission_manipulation')
trans = tf.TransformListener()
rospy.loginfo("Waiting for tf ...")
rospy.sleep(3)
assert (len(trans.getFrameStrings()) > 0)
state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with state:
def kinect_callback(userdata, result):
userdata.objects = []
objects = []
sum_x = 0
for obj in result.objects.objects:
position = obj.pose.pose.pose.position
if position.y > 0.5 or position.y < -0.5:
continue
_levels = [0.10, 0.44, 0.78, 1.12, 1.46, 1.80]
for l in _levels:
if fabs(l - position.z) < 0.05:
position.z = l + 0.05
if position.z > 1.1:
position.z += 0.05
obj.header.stamp = rospy.Time(0)
kinect_point = PointStamped(header=obj.header, point=position)
odom_point = trans.transformPoint('odom', kinect_point)
sum_x += odom_point.point.x
rospy.loginfo(colored('[Kinect Object(odom)] from:(%f %f %f)', 'yellow'), odom_point.point.x,
odom_point.point.y, odom_point.point.z)
objects.append(odom_point)
avg_x = sum_x / len(objects)
for from_point in objects:
to_point = copy.deepcopy(from_point)
to_point.point.x = avg_x
to_point.point.z = find_div(from_point.point.z)
userdata.objects.append({'from': from_point, 'to': to_point})
rospy.loginfo(colored('[Kinect Object(odom)] to:(%f %f %f)', 'yellow'), to_point.point.x,
to_point.point.y, to_point.point.z)
rospy.loginfo(colored('Total Object: %d','green'), len(objects))
return 'succeeded'
StateMachine.add('Arm_Mode_Kinect', ArmModeState(ArmModeState.Arm_Mode_Kinect),
transitions={'succeeded': 'Start_Button'})
StateMachine.add('Start_Button', MonitorStartButtonState(),
transitions={'valid': 'Start_Button', 'invalid': 'S_Kinect_Recognition'})
StateMachine.add('S_Kinect_Recognition',
ServiceState(service_name='/kinect_find_objects',
service_spec=FindObjects,
input_keys=['objects'],
output_keys=['objects'],
response_cb=kinect_callback),
transitions={'succeeded': 'Generate_Report'})
StateMachine.add('Generate_Report', GenerateReportState(image='result.png', text='object_names.txt'),
transitions={'succeeded': 'IT_Objects_Iterator'} )
objects_iterator = Iterator(outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['objects'], output_keys=[],
it=lambda: state.userdata.objects, it_label='target',
exhausted_outcome='succeeded')
with objects_iterator:
fetch_object_sequence = Sequence(outcomes=['succeeded', 'aborted', 'continue', 'preempted'],
input_keys=['target'],
connector_outcome='succeeded')
with fetch_object_sequence:
Sequence.add('Speak', SpeakState('New Object recognized'))
Sequence.add('Gripper_Photo', GripperState(GripperState.GRIPPER_OPEN))
Sequence.add('Move_For_Photo', MoveArmState(Point(-0.7, 0, 0), target_key='from'),
transitions={'aborted':'continue'})
concurrence = Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='succeeded',
child_termination_cb=lambda x: True,
input_keys=['target'])
with concurrence:
Concurrence.add('Move_Fetch', MoveArmState(Point(0.06, 0, 0), target_key='from'))
Concurrence.add('Gripper_Laser_sensor', MonitorState('/gripper_laser_sensor', Bool, cond_cb=lambda x,y: False))
Sequence.add('Move_Fetch_Concurrence', concurrence)
Sequence.add('Gripper_Fetch', GripperState(GripperState.GRIPPER_CLOSE))
Sequence.add('Move_Fetch_Back', MoveArmState(Point(-0.7, 0, 0), target_key='from'))
Sequence.add('Move_Down', MoveArmState(Point(-0.6, 0, 0), target_key='to'))
Sequence.add('Move_Put', MoveArmState(Point(0, 0, 0), target_key='to'))
Sequence.add('Gripper_Put', GripperState(GripperState.GRIPPER_OPEN))
Sequence.add('Move_Put_Back', MoveArmState(Point(-0.6, 0, 0), target_key='to'), transitions={'succeeded': 'continue'})
Iterator.set_contained_state('Seq_Fetch_Object', fetch_object_sequence, loop_outcomes=['continue'])
# end of objects_iterator
StateMachine.add('IT_Objects_Iterator', objects_iterator,
transitions= {'succeeded': 'A_Move_Reset', 'aborted': 'A_Move_Reset'})
StateMachine.add('A_Move_Reset', ArmModeState(ArmModeState.Arm_Mode_Init),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted'})
# Run state machine introspection server for smach viewer
intro_server = IntrospectionServer('tinker_mission_manipulation', state, '/tinker_mission_manipulation')
intro_server.start()
outcome = state.execute()
rospy.spin()
intro_server.stop()
def __init__(self):
rospy.init_node('petit_smach_ai', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Create a list to hold the target quaternions (orientations)
quaternions = list()
# First define the corner orientations as Euler angles
euler_angles = (0, pi, pi/2, -pi/2, pi/4, 0)
# Then convert the angles to quaternions
for angle in euler_angles:
q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz')
q = Quaternion(*q_angle)
quaternions.append(q)
# Create a list to hold the waypoint poses
self.waypoints = list()
self.square_size = 1.0
# Append each of the four waypoints to the list. Each waypoint
# is a pose consisting of a position and orientation in the map frame.
self.waypoints.append(Pose(Point(-0.4, 0.65, 0.0), quaternions[0]))
self.waypoints.append(Pose(Point(-1.0, 0.50, 0.0), quaternions[1]))
self.waypoints.append(Pose(Point(-0.4, 0.35, 0.0), quaternions[2]))
self.waypoints.append(Pose(Point(-0.8, 0.65, 0.0), quaternions[3]))
self.waypoints.append(Pose(Point(-1.2, 0.30, 0.0), quaternions[4]))
self.waypoints.append(Pose(Point(-0.4, 0.657, 0.0), quaternions[5]))
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('/PETIT/cmd_vel', Twist)
self.stopping = False
self.recharging = False
self.robot_side = 1
# State machine for Actions
self.sm_actions = StateMachine(outcomes=['succeeded','aborted','preempted'])
with self.sm_actions:
# Calib X
StateMachine.add('CALIBRATE_X', CalibX(),
transitions={'succeeded':'CALIBRATE_Y',
'aborted':'aborted'})
# Calib Y
StateMachine.add('CALIBRATE_Y', CalibY(),
transitions={'succeeded':'MAX_SPEED_1',
'aborted':'aborted'})
# PICK-UP
# Change speed
StateMachine.add('MAX_SPEED_1', MaxLinearSpeed(70000),
transitions={'succeeded':'GOTO_1_1',
'aborted':'aborted'})
# NavPoint 1
StateMachine.add('GOTO_1_1', Nav2Waypoint(self.waypoints[0]),
transitions={'succeeded':'MAX_SPEED_2',
'aborted':'aborted'})
# Change speed
StateMachine.add('MAX_SPEED_2', MaxLinearSpeed(40000),
transitions={'succeeded':'PAUSE_1',
'aborted':'aborted'})
# Pause
StateMachine.add('PAUSE_1', Pause(1.0),
transitions={'succeeded':'FORWARD_1',
'aborted':'aborted'})
# Avancer
StateMachine.add('FORWARD_1', MoveForward(0.15),
transitions={'succeeded':'FORKS_10',
'aborted':'aborted'})
# Monter fourches
StateMachine.add('FORKS_10', Forks(90),
transitions={'succeeded':'FORKS_11',
'aborted':'aborted'})
StateMachine.add('FORKS_11', Forks(87),
transitions={'succeeded':'FORKS_12',
'aborted':'aborted'})
StateMachine.add('FORKS_12', Forks(83),
transitions={'succeeded':'BACKWARD_1',
'aborted':'aborted'})
# Reculer
StateMachine.add('BACKWARD_1', MoveForward(-0.15),
transitions={'succeeded':'ROTATE_1',
'aborted':'aborted'})
# Rotation
StateMachine.add('ROTATE_1', MoveRotate(pi),
transitions={'succeeded':'MAX_SPEED_3',
'aborted':'aborted'})
# DROP-OFF
# Change speed
StateMachine.add('MAX_SPEED_3', MaxLinearSpeed(70000),
transitions={'succeeded':'GOTO_2_1',
'aborted':'aborted'})
# NavPoint 2
StateMachine.add('GOTO_2_1', Nav2Waypoint(self.waypoints[1]),
transitions={'succeeded':'MAX_SPEED_4',
'aborted':'aborted'})
# Change speed
StateMachine.add('MAX_SPEED_4', MaxLinearSpeed(40000),
transitions={'succeeded':'PAUSE_2',
'aborted':'aborted'})
# Pause
StateMachine.add('PAUSE_2', Pause(1.0),
transitions={'succeeded':'FORWARD_2',
'aborted':'aborted'})
# Avancer
StateMachine.add('FORWARD_2', MoveForward(0.13),
transitions={'succeeded':'FORKS_20',
'aborted':'aborted'})
# Baisser fourches
StateMachine.add('FORKS_20', Forks(87),
transitions={'succeeded':'FORKS_21',
'aborted':'aborted'})
StateMachine.add('FORKS_21', Forks(91),
transitions={'succeeded':'FORKS_22',
'aborted':'aborted'})
StateMachine.add('FORKS_22', Forks(95),
transitions={'succeeded':'BACKWARD_2',
'aborted':'aborted'})
# Reculer
StateMachine.add('BACKWARD_2', MoveForward(-0.15),
transitions={'succeeded':'ROTATE_2',
'aborted':'aborted'})
# Rotation
StateMachine.add('ROTATE_2', MoveRotate(0),
transitions={'succeeded':'GOTO_3_1',
'aborted':'aborted'})
# NavPoint 3
StateMachine.add('GOTO_3_1', Nav2Waypoint(self.waypoints[2]),
transitions={'succeeded':'PAUSE_3',
'aborted':'aborted'})
# Pause
StateMachine.add('PAUSE_3', Pause(2.0),
transitions={'succeeded':'GOTO_4_1',
'aborted':'aborted'})
# NavPoint 4
StateMachine.add('GOTO_4_1', Nav2Waypoint(self.waypoints[3]),
transitions={'succeeded':'PAUSE_4',
'aborted':'aborted'})
# Pause
StateMachine.add('PAUSE_4', Pause(2.0),
transitions={'succeeded':'GOTO_2_2',
'aborted':'aborted'})
# PICK-UP
# NavPoint 2
StateMachine.add('GOTO_2_2', Nav2Waypoint(self.waypoints[1]),
transitions={'succeeded':'PAUSE_5',
'aborted':'aborted'})
# Pause
StateMachine.add('PAUSE_5', Pause(1.0),
transitions={'succeeded':'FORWARD_3',
'aborted':'aborted'})
# Avancer
StateMachine.add('FORWARD_3', MoveForward(0.15),
transitions={'succeeded':'FORKS_30',
'aborted':'aborted'})
# Monter fourches
StateMachine.add('FORKS_30', Forks(90),
transitions={'succeeded':'FORKS_31',
'aborted':'aborted'})
StateMachine.add('FORKS_31', Forks(87),
transitions={'succeeded':'FORKS_32',
'aborted':'aborted'})
StateMachine.add('FORKS_32', Forks(83),
transitions={'succeeded':'BACKWARD_3',
'aborted':'aborted'})
# Reculer
StateMachine.add('BACKWARD_3', MoveForward(-0.15),
transitions={'succeeded':'ROTATE_3',
'aborted':'aborted'})
# Rotation
StateMachine.add('ROTATE_3', MoveRotate(0),
transitions={'succeeded':'GOTO_1_2',
'aborted':'aborted'})
# DROP-OFF
# NavPoint 1
StateMachine.add('GOTO_1_2', Nav2Waypoint(self.waypoints[5]),
transitions={'succeeded':'PAUSE_6',
'aborted':'aborted'})
# Pause
StateMachine.add('PAUSE_6', Pause(1.0),
transitions={'succeeded':'FORWARD_4',
'aborted':'aborted'})
# Avancer
StateMachine.add('FORWARD_4', MoveForward(0.13),
transitions={'succeeded':'FORKS_40',
'aborted':'aborted'})
# Baisser fourches
StateMachine.add('FORKS_40', Forks(87),
transitions={'succeeded':'FORKS_41',
'aborted':'aborted'})
StateMachine.add('FORKS_41', Forks(91),
transitions={'succeeded':'FORKS_42',
'aborted':'aborted'})
StateMachine.add('FORKS_42', Forks(95),
transitions={'succeeded':'BACKWARD_4',
'aborted':'aborted'})
# Reculer
StateMachine.add('BACKWARD_4', MoveForward(-0.15),
transitions={'succeeded':'ROTATE_4',
'aborted':'aborted'})
# Rotation
StateMachine.add('ROTATE_4', MoveRotate(pi),
transitions={'succeeded':'GOTO_5_1',
'aborted':'aborted'})
# NavPoint 5
StateMachine.add('GOTO_5_1', Nav2Waypoint(self.waypoints[4]),
transitions={'succeeded':'CALIBRATE_X',
'aborted':'aborted'})
# Create the top level state machine
self.sm_top = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
# Add nav_patrol, sm_recharge and a Stop() machine to sm_top
with self.sm_top:
StateMachine.add('ACTIONS', self.sm_actions, transitions={'succeeded':'ACTIONS', 'aborted':'STOP'})
#StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded':'PATROL'})
StateMachine.add('STOP', Stop(), transitions={'succeeded':''})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm_top.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def __init__(self):
rospy.init_node('petit_smach_ai', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Create a list to hold the target quaternions (orientations)
quaternions = list()
# First define the corner orientations as Euler angles
euler_angles = (pi/2, pi, 3*pi/2, 0)
# Then convert the angles to quaternions
for angle in euler_angles:
q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz')
q = Quaternion(*q_angle)
quaternions.append(q)
# Create a list to hold the waypoint poses
self.waypoints = list()
self.square_size = 1.0
# Append each of the four waypoints to the list. Each waypoint
# is a pose consisting of a position and orientation in the map frame.
self.waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[3]))
self.waypoints.append(Pose(Point(self.square_size, 0.0, 0.0), quaternions[0]))
self.waypoints.append(Pose(Point(self.square_size, self.square_size, 0.0), quaternions[1]))
self.waypoints.append(Pose(Point(0.0, self.square_size, 0.0), quaternions[2]))
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('/PETIT/cmd_vel', Twist)
self.stopping = False
self.recharging = False
self.robot_side = 1
# State machine for Action1
self.sm_action1 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action1.userdata.mandibles_sleep = 0.1
with self.sm_action1:
StateMachine.add('OPEN_MANDIBLES', OpenMandibles(),
transitions={'succeeded':'NAV_WAYPOINT',
'aborted':'aborted'})
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'UPDATE_DROPCUBE_OBJ',
'aborted':'aborted'})
# StateMachine.add('UPDATE_DROPCUBE_OBJ', UpdateObjectiveDropCubes(),
# transitions={'succeeded':'CLOSE_MANDIBLES',
# 'aborted':'aborted'})
StateMachine.add('UPDATE_DROPCUBE_OBJ', ServiceState('/PETIT/update_priority', UpdatePriority, request_cb=self.requestPrioCube_cb, response_cb=self.updatePrioCube_cb,
output_keys=['waypoint_out'],
input_keys=['robot_side']),
transitions={'succeeded':'HCLOSE_MANDIBLES',
'aborted':'HCLOSE_MANDIBLES'},
remapping={'waypoint_out':'patrol_waypoint'})
StateMachine.add('HCLOSE_MANDIBLES', HalfCloseMandibles(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Action2
self.sm_action2 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action2.userdata.speed = -0.1;
self.sm_action2.userdata.distance = 0.3;
self.sm_action2.userdata.mandibles_sleep = 0.1
with self.sm_action2:
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'OPEN_MANDIBLES',
'aborted':'aborted'})
StateMachine.add('OPEN_MANDIBLES', OpenMandibles(),
transitions={'succeeded':'MOVE_BACKWARD',
'aborted':'aborted'})
StateMachine.add('MOVE_BACKWARD', MoveForward(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Action3
self.sm_action3 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action3.userdata.mandibles_sleep = 0.1
self.sm_action3.userdata.speed = -0.1;
self.sm_action3.userdata.distance = 0.2;
with self.sm_action3:
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'MOVE_PUSH',
'aborted':'aborted'})
StateMachine.add('MOVE_PUSH', MovePush(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Action4
self.sm_action4 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action4.userdata.mandibles_sleep = 0.1
self.sm_action4.userdata.speed_x = 0.1
self.sm_action4.userdata.speed_y = 0.1
self.sm_action4.userdata.distance = 0.5;
with self.sm_action4:
StateMachine.add('CLOSE_MANDIBLES', CloseMandibles(),
transitions={'succeeded':'NAV_WAYPOINT',
'aborted':'aborted'})
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'SLIDE',
'aborted':'aborted'})
StateMachine.add('SLIDE', Slide(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Action5
self.sm_action5 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action5.userdata.mandibles_sleep = 0.1
with self.sm_action5:
StateMachine.add('OPEN_MANDIBLES', OpenMandibles(),
transitions={'succeeded':'NAV_WAYPOINT',
'aborted':'aborted'})
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'UPDATE_DROPSHELL_OBJ',
'aborted':'aborted'})
# StateMachine.add('UPDATE_DROPSHELL_OBJ', UpdateObjectiveDropShell(),
# transitions={'succeeded':'ALMOSTCLOSE_MANDIBLES',
# 'aborted':'aborted'})
StateMachine.add('UPDATE_DROPSHELL_OBJ', ServiceState('/PETIT/update_priority', UpdatePriority, request_cb=self.requestPrioShell_cb, response_cb=self.updatePrioShell_cb,
output_keys=['waypoint_out'],
input_keys=['robot_side']),
transitions={'succeeded':'ALMOSTCLOSE_MANDIBLES',
'aborted':'ALMOSTCLOSE_MANDIBLES'})
StateMachine.add('ALMOSTCLOSE_MANDIBLES', AlmostCloseMandibles(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Action6
self.sm_action6 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action6.userdata.mandibles_sleep = 0.1
self.sm_action6.userdata.speed = 0.1;
self.sm_action6.userdata.distance = 0.2;
with self.sm_action6:
StateMachine.add('OPEN_MANDIBLES', OpenMandibles(),
transitions={'succeeded':'NAV_WAYPOINT',
'aborted':'aborted'})
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'FORWARD',
'aborted':'aborted'})
StateMachine.add('FORWARD', MoveForward(),
transitions={'succeeded':'UPDATE_DROPSHELL_OBJ',
'aborted':'aborted'})
# StateMachine.add('UPDATE_DROPSHELL_OBJ', UpdateObjectiveDropShell(),
# transitions={'succeeded':'HCLOSE_MANDIBLES',
# 'aborted':'aborted'})
StateMachine.add('UPDATE_DROPSHELL_OBJ', ServiceState('/PETIT/update_priority', UpdatePriority, request_cb=self.requestPrioShell_cb, response_cb=self.updatePrioShell_cb,
output_keys=['waypoint_out'],
input_keys=['robot_side']),
transitions={'succeeded':'HCLOSE_MANDIBLES',
'aborted':'HCLOSE_MANDIBLES'})
StateMachine.add('HCLOSE_MANDIBLES', HalfCloseMandibles(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Action7
self.sm_action7 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action7.userdata.mandibles_sleep = 0.1
with self.sm_action7:
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Actions
self.sm_actions = StateMachine(outcomes=['succeeded','aborted','preempted'])
self.sm_actions.userdata.waypoints = self.waypoints
with self.sm_actions:
StateMachine.add('PICK_WAYPOINT', ServiceState('/PETIT/get_objective', GetObjective, response_cb=self.objective_cb,
output_keys=['waypoint_out'],
outcomes=['action1','action2','action3','action4','action5','action6','action7','aborted','succeeded','preempted']),
transitions={'action1':'SM_ACTION1','action2':'SM_ACTION2','action3':'SM_ACTION3','action4':'SM_ACTION4','action5':'SM_ACTION5','action6':'SM_ACTION6','action7':'SM_ACTION7','aborted':'SM_ACTION1'},
remapping={'waypoint_out':'patrol_waypoint'})
#StateMachine.add('PICK_WAYPOINT', PickWaypoint(),
# transitions={'action1':'SM_ACTION1','action2':'SM_ACTION2','action3':'SM_ACTION3','action4':'SM_ACTION4','action5':'SM_ACTION5','action6':'SM_ACTION6','aborted':'SM_ACTION1'},
# remapping={'waypoint_out':'patrol_waypoint'})
StateMachine.add('SM_ACTION1', self.sm_action1, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('SM_ACTION2', self.sm_action2, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('SM_ACTION3', self.sm_action3, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('SM_ACTION4', self.sm_action4, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('SM_ACTION5', self.sm_action5, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('SM_ACTION6', self.sm_action6, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('SM_ACTION7', self.sm_action7, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('REMOVE_OBJECTIVE', RemoveObjective(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'},
remapping={'waypoint_in':'remove_waypoint'})
# State machine with concurrence
self.sm_concurrent = Concurrence(outcomes=['succeeded', 'stop'],
default_outcome='succeeded',
child_termination_cb=self.concurrence_child_termination_cb,
outcome_cb=self.concurrence_outcome_cb)
# Add the sm_actions machine and a battery MonitorState to the nav_patrol machine
with self.sm_concurrent:
Concurrence.add('SM_ACTIONS', self.sm_actions)
Concurrence.add('MONITOR_TIME', MonitorState("/GENERAL/remain", Int32, self.time_cb))
Concurrence.add('MONITOR_BATTERY', MonitorState("/PETIT/adc", Int32, self.battery_cb))
# Create the top level state machine
self.sm_top = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
# Add nav_patrol, sm_recharge and a Stop() machine to sm_top
with self.sm_top:
# @smach.cb_interface()
# def requestPrioCube_cb(userdata, request):
# update_request = UpdatePriority().Request
# update_request.goal.pose.position.x = userdata.robot_side*(1.500 - 0.300)
# update_request.goal.pose.position.y = 0.800
# update_request.prio = 100
# return update_request
StateMachine.add('WAIT_COLOR', MonitorState("/GENERAL/color", Int32, self.color_cb), transitions={'valid':'WAIT_START', 'preempted':'WAIT_START', 'invalid':'WAIT_START'})
StateMachine.add('WAIT_START', MonitorState("/GENERAL/start", Empty, self.start_cb), transitions={'valid':'CONCURRENT', 'preempted':'CONCURRENT', 'invalid':'CONCURRENT'})
StateMachine.add('CONCURRENT', self.sm_concurrent, transitions={'succeeded':'CONCURRENT', 'stop':'STOP'})
#StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded':'PATROL'})
StateMachine.add('STOP', Stop(), transitions={'succeeded':''})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm_top.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def __init__(self):
rospy.init_node('carry_smach_move', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Create a list to hold the target quaternions (orientations)
quaternions = list()
# First define the corner orientations as Euler angles
euler_angles = (0.0, 3*pi/2, pi/2, 3*pi/2, pi/2, 3*pi/2, 3*pi/2, 0.0, pi, 0.0, pi, 3*pi/2)
# Then convert the angles to quaternions
for angle in euler_angles:
q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz')
q = Quaternion(*q_angle)
quaternions.append(q)
# Create a list to hold the waypoint poses
self.waypoints = list()
# Append each of the four waypoints to the list. Each waypoint
# is a pose consisting of a position and orientation in the map frame.
self.waypoints.append(Pose(Point(0.0, -3.0, 0.0), quaternions[0]))
self.waypoints.append(Pose(Point(2.0, -2.0, 0.0), quaternions[1]))
self.waypoints.append(Pose(Point(2.9, 1.0, 0.0), quaternions[2]))
self.waypoints.append(Pose(Point(3.0, 1.2, 0.0), quaternions[3]))
self.waypoints.append(Pose(Point(4.23, 1.1, 0.0), quaternions[4]))
self.waypoints.append(Pose(Point(4.23, 1.1, 0.0), quaternions[5]))
self.waypoints.append(Pose(Point(4.4, -0.6, 0.0), quaternions[6]))
self.waypoints.append(Pose(Point(2.3, 1.7, 0.0), quaternions[7]))
self.waypoints.append(Pose(Point(2.3, 1.7, 0.0), quaternions[8]))
self.waypoints.append(Pose(Point(-1.3, 0.7, 0.0), quaternions[9]))
self.waypoints.append(Pose(Point(-1.3, 0.7, 0.0), quaternions[10]))
self.waypoints.append(Pose(Point(-1.6, 0.1, 0.0), quaternions[11]))
# State machine
#self.sm_out_main = StateMachine(outcomes=['succeeded','aborted','preempted'])
#self.sm_out_main.userdata.goal = self.waypoints[2];
#with self.sm_out_main:
# StateMachine.add('GO_OUT_MAIN_ROOM', Nav2Waypoint(),
# transitions={'succeeded':'succeeded',
# 'aborted':'aborted'})
# Concurrent State machine
self.sm_in_main_room = Concurrence(outcomes=['succeeded','aborted','preempted','go_kitchen','go_bedroom','go_sleep'],
default_outcome='succeeded',
child_termination_cb=self.in_main_room_child_termination_cb,
outcome_cb=self.in_main_room_outcome_cb)
self.sm_in_main_room.userdata.goal = self.waypoints[1];
with self.sm_in_main_room:
Concurrence.add('GO_TO_KITCHEN', MonitorState("/CARRY/go_kitchen", Empty, self.empty_cb))
Concurrence.add('GO_TO_BEDROOM', MonitorState("/CARRY/go_bedroom", Empty, self.empty_cb))
Concurrence.add('GO_TO_SLEEP', MonitorState("/CARRY/go_sleep", Empty, self.empty_cb))
#Concurrence.add('GO_TO_POINT', Nav2Waypoint(self.waypoints[1]))
# Concurrent State machine
self.sm_sleep = Concurrence(outcomes=['succeeded','aborted','preempted','go_kitchen','go_bedroom','go_main_room'],
default_outcome='succeeded',
child_termination_cb=self.in_sleep_child_termination_cb,
outcome_cb=self.in_sleep_outcome_cb)
self.sm_sleep.userdata.goal = self.waypoints[0];
with self.sm_sleep:
Concurrence.add('GO_TO_KITCHEN', MonitorState("/CARRY/go_kitchen", Empty, self.empty_cb))
Concurrence.add('GO_TO_BEDROOM', MonitorState("/CARRY/go_bedroom", Empty, self.empty_cb))
Concurrence.add('GO_TO_MAIN_ROOM', MonitorState("/CARRY/go_main_room", Empty, self.empty_cb))
#Concurrence.add('GO_TO_POINT', Nav2Waypoint(self.waypoints[0]))
# Concurrent State machine
self.sm_in_kitchen = Concurrence(outcomes=['succeeded','aborted','preempted','go_main_room','go_bedroom','go_sleep'],
default_outcome='succeeded',
child_termination_cb=self.in_kitchen_child_termination_cb,
outcome_cb=self.in_kitchen_outcome_cb)
self.sm_in_kitchen.userdata.goal = self.waypoints[6];
with self.sm_in_kitchen:
Concurrence.add('GO_TO_MAIN_ROOM', MonitorState("/CARRY/go_main_room", Empty, self.empty_cb))
Concurrence.add('GO_TO_BEDROOM', MonitorState("/CARRY/go_bedroom", Empty, self.empty_cb))
Concurrence.add('GO_TO_SLEEP', MonitorState("/CARRY/go_sleep", Empty, self.empty_cb))
#Concurrence.add('GO_TO_POINT', Nav2Waypoint(self.waypoints[6]))
# Concurrent State machine
self.sm_in_bedroom = Concurrence(outcomes=['succeeded','aborted','preempted','go_main_room','go_kitchen','go_sleep'],
default_outcome='succeeded',
child_termination_cb=self.in_bedroom_child_termination_cb,
outcome_cb=self.in_bedroom_outcome_cb)
self.sm_in_bedroom.userdata.goal = self.waypoints[11];
with self.sm_in_bedroom:
Concurrence.add('GO_TO_MAIN_ROOM', MonitorState("/CARRY/go_main_room", Empty, self.empty_cb))
Concurrence.add('GO_TO_KITCHEN', MonitorState("/CARRY/go_kitchen", Empty, self.empty_cb))
Concurrence.add('GO_TO_SLEEP', MonitorState("/CARRY/go_sleep", Empty, self.empty_cb))
#Concurrence.add('GO_TO_POINT', Nav2Waypoint(self.waypoints[11]))
# Create the top level state machine
self.sm_top = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
# Add nav_patrol, sm_recharge and a Stop() machine to sm_top
with self.sm_top:
StateMachine.add('IN_MAIN_ROOM', self.sm_in_main_room, transitions={'succeeded':'IN_MAIN_ROOM',
'go_kitchen':'NAV_M2K_M',
'go_sleep':'IN_SLEEP',
'go_bedroom':'NAV_M2B_M'})
StateMachine.add('IN_KITCHEN', self.sm_in_kitchen, transitions={'succeeded':'succeeded',
'go_main_room':'NAV_K2M_K',
'go_sleep':'NAV_K2S_K',
'go_bedroom':'NAV_K2B_K'})
StateMachine.add('IN_BEDROOM', self.sm_in_bedroom, transitions={'succeeded':'succeeded',
'go_kitchen':'NAV_B2K_B',
'go_sleep':'NAV_B2S_B',
'go_main_room':'NAV_B2M_B'})
StateMachine.add('IN_SLEEP', self.sm_sleep, transitions={'succeeded':'succeeded',
'go_kitchen':'NAV_S2K_M',
'go_main_room':'IN_MAIN_ROOM',
'go_bedroom':'NAV_S2B_M'})
StateMachine.add('NAV_M2K_M', Nav2Waypoint(self.waypoints[2]),
transitions={'succeeded':'NAV_M2K_K',
'aborted':'aborted'})
StateMachine.add('NAV_M2K_K', Nav2Waypoint(self.waypoints[5]),
transitions={'succeeded':'NAV_M2K_END',
'aborted':'aborted'})
StateMachine.add('NAV_M2K_END', Nav2Waypoint(self.waypoints[6]),
transitions={'succeeded':'IN_KITCHEN',
'aborted':'aborted'})
StateMachine.add('NAV_K2M_K', Nav2Waypoint(self.waypoints[4]),
transitions={'succeeded':'NAV_K2M_M',
'aborted':'aborted'})
StateMachine.add('NAV_K2M_M', Nav2Waypoint(self.waypoints[3]),
transitions={'succeeded':'NAV_K2M_END',
'aborted':'aborted'})
StateMachine.add('NAV_K2M_END', Nav2Waypoint(self.waypoints[1]),
transitions={'succeeded':'IN_MAIN_ROOM',
'aborted':'aborted'})
StateMachine.add('NAV_M2B_M', Nav2Waypoint(self.waypoints[2]),
transitions={'succeeded':'NAV_M2B_C',
'aborted':'aborted'})
StateMachine.add('NAV_M2B_C', Nav2Waypoint(self.waypoints[8]),
transitions={'succeeded':'NAV_M2B_B',
'aborted':'aborted'})
StateMachine.add('NAV_M2B_B', Nav2Waypoint(self.waypoints[10]),
transitions={'succeeded':'NAV_M2B_END',
'aborted':'aborted'})
StateMachine.add('NAV_M2B_END', Nav2Waypoint(self.waypoints[11]),
transitions={'succeeded':'IN_BEDROOM',
'aborted':'aborted'})
StateMachine.add('NAV_K2S_K', Nav2Waypoint(self.waypoints[4]),
transitions={'succeeded':'NAV_K2S_M',
'aborted':'aborted'})
StateMachine.add('NAV_K2S_M', Nav2Waypoint(self.waypoints[3]),
transitions={'succeeded':'NAV_K2S_END',
'aborted':'aborted'})
StateMachine.add('NAV_K2S_END', Nav2Waypoint(self.waypoints[0]),
transitions={'succeeded':'IN_SLEEP',
'aborted':'aborted'})
StateMachine.add('NAV_K2B_K', Nav2Waypoint(self.waypoints[4]),
transitions={'succeeded':'NAV_K2B_C',
'aborted':'aborted'})
StateMachine.add('NAV_K2B_C', Nav2Waypoint(self.waypoints[8]),
transitions={'succeeded':'NAV_K2B_B',
'aborted':'aborted'})
StateMachine.add('NAV_K2B_B', Nav2Waypoint(self.waypoints[10]),
transitions={'succeeded':'NAV_K2B_END',
'aborted':'aborted'})
StateMachine.add('NAV_K2B_END', Nav2Waypoint(self.waypoints[11]),
transitions={'succeeded':'IN_BEDROOM',
'aborted':'aborted'})
StateMachine.add('NAV_B2K_B', Nav2Waypoint(self.waypoints[9]),
transitions={'succeeded':'NAV_B2K_C',
'aborted':'aborted'})
StateMachine.add('NAV_B2K_C', Nav2Waypoint(self.waypoints[7]),
transitions={'succeeded':'NAV_B2K_K',
'aborted':'aborted'})
StateMachine.add('NAV_B2K_K', Nav2Waypoint(self.waypoints[5]),
transitions={'succeeded':'NAV_B2K_END',
'aborted':'aborted'})
StateMachine.add('NAV_B2K_END', Nav2Waypoint(self.waypoints[6]),
transitions={'succeeded':'IN_KITCHEN',
'aborted':'aborted'})
StateMachine.add('NAV_B2S_B', Nav2Waypoint(self.waypoints[9]),
transitions={'succeeded':'NAV_B2S_C',
'aborted':'aborted'})
StateMachine.add('NAV_B2S_C', Nav2Waypoint(self.waypoints[7]),
transitions={'succeeded':'NAV_B2S_M',
'aborted':'aborted'})
StateMachine.add('NAV_B2S_M', Nav2Waypoint(self.waypoints[3]),
transitions={'succeeded':'NAV_B2S_END',
'aborted':'aborted'})
StateMachine.add('NAV_B2S_END', Nav2Waypoint(self.waypoints[0]),
transitions={'succeeded':'IN_SLEEP',
'aborted':'aborted'})
StateMachine.add('NAV_B2M_B', Nav2Waypoint(self.waypoints[9]),
transitions={'succeeded':'NAV_B2M_C',
'aborted':'aborted'})
StateMachine.add('NAV_B2M_C', Nav2Waypoint(self.waypoints[7]),
transitions={'succeeded':'NAV_B2M_M',
'aborted':'aborted'})
StateMachine.add('NAV_B2M_M', Nav2Waypoint(self.waypoints[3]),
transitions={'succeeded':'NAV_B2M_END',
'aborted':'aborted'})
StateMachine.add('NAV_B2M_END', Nav2Waypoint(self.waypoints[1]),
transitions={'succeeded':'IN_MAIN_ROOM',
'aborted':'aborted'})
StateMachine.add('NAV_S2B_M', Nav2Waypoint(self.waypoints[2]),
transitions={'succeeded':'NAV_S2B_C',
'aborted':'aborted'})
StateMachine.add('NAV_S2B_C', Nav2Waypoint(self.waypoints[8]),
transitions={'succeeded':'NAV_S2B_B',
'aborted':'aborted'})
StateMachine.add('NAV_S2B_B', Nav2Waypoint(self.waypoints[10]),
transitions={'succeeded':'NAV_S2B_END',
'aborted':'aborted'})
StateMachine.add('NAV_S2B_END', Nav2Waypoint(self.waypoints[11]),
transitions={'succeeded':'IN_BEDROOM',
'aborted':'aborted'})
StateMachine.add('NAV_S2K_M', Nav2Waypoint(self.waypoints[2]),
transitions={'succeeded':'NAV_S2K_K',
'aborted':'aborted'})
StateMachine.add('NAV_S2K_K', Nav2Waypoint(self.waypoints[5]),
transitions={'succeeded':'NAV_S2K_END',
'aborted':'aborted'})
StateMachine.add('NAV_S2K_END', Nav2Waypoint(self.waypoints[6]),
transitions={'succeeded':'IN_KITCHEN',
'aborted':'aborted'})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('carry_sm', self.sm_top, '/SM_CARRY_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm_top.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def tasker():
rospy.init_node('tasker')
wfg = WaitForGoalState() # We don't want multiple subscribers so we need one WaitForGoal state
runner = SMACHRunner(rgoap_subclasses)
## sub machines
sq_move_to_new_goal = Sequence(outcomes=['succeeded', 'aborted', 'preempted'],
connector_outcome='succeeded')
with sq_move_to_new_goal:
Sequence.add('WAIT_FOR_GOAL', wfg)
Sequence.add('MOVE_BASE_RGOAP', MoveBaseRGOAPState(runner))
sq_autonomous_rgoap = Sequence(outcomes=['preempted'],
connector_outcome='succeeded')
with sq_autonomous_rgoap:
Sequence.add('SLEEP_UNTIL_ENABLED', get_sleep_until_smach_enabled_smach())
Sequence.add('AUTONOMOUS_RGOAP', AutonomousRGOAPState(runner),
transitions={'succeeded':'SLEEP_UNTIL_ENABLED',
'aborted':'SLEEP'})
Sequence.add('SLEEP', SleepState(5),
transitions={'succeeded':'SLEEP_UNTIL_ENABLED'})
## tasker machine
sm_tasker = StateMachine(outcomes=['succeeded', 'aborted', 'preempted',
'field_error', 'undefined_task'],
input_keys=['task_goal'])
with sm_tasker:
## add all tasks to be available
# states using rgoap
StateMachine.add('MOVE_TO_NEW_GOAL_RGOAP', sq_move_to_new_goal)
StateMachine.add('INCREASE_AWARENESS_RGOAP', IncreaseAwarenessRGOAPState(runner))
StateMachine.add('AUTONOMOUS_RGOAP_CYCLE', sq_autonomous_rgoap)
StateMachine.add('AUTONOMOUS_RGOAP_SINGLE_GOAL', AutonomousRGOAPState(runner))
# states from uashh_smach
StateMachine.add('LOOK_AROUND', get_lookaround_smach())
StateMachine.add('GLIMPSE_AROUND', get_lookaround_smach(glimpse=True))
StateMachine.add('MOVE_ARM_CRAZY', get_lookaround_smach(crazy=True))
StateMachine.add('MOVE_TO_RANDOM_GOAL', get_random_goal_smach())
StateMachine.add('MOVE_TO_NEW_GOAL_AND_RETURN', task_go_and_return.get_go_and_return_smach())
StateMachine.add('PATROL_TO_NEW_GOAL', task_patrol.get_patrol_smach())
StateMachine.add('MOVE_AROUND', task_move_around.get_move_around_smach())
StateMachine.add('SLEEP_FIVE_SEC', SleepState(5))
## now the task receiver is created and automatically links to
## all task states added above
task_states_labels = sm_tasker.get_children().keys()
task_states_labels.sort() # sort alphabetically and then by _RGOAP
task_states_labels.sort(key=lambda label: '_RGOAP' in label, reverse=True)
task_receiver_transitions = {'undefined_outcome':'undefined_task'}
task_receiver_transitions.update({l:l for l in task_states_labels})
StateMachine.add('TASK_RECEIVER',
UserDataToOutcomeState(task_states_labels,
'task_goal',
lambda ud: ud.task_id),
task_receiver_transitions)
sm_tasker.set_initial_state(['TASK_RECEIVER'])
rospy.loginfo('tasker starting, available tasks: %s', ', '.join(task_states_labels))
pub = rospy.Publisher('/task/available_tasks', String, latch=True)
thread.start_new_thread(rostopic.publish_message, (pub, String, [', '.join(task_states_labels)], 1))
asw = ActionServerWrapper('activate_task', TaskActivationAction,
wrapped_container=sm_tasker,
succeeded_outcomes=['succeeded'],
aborted_outcomes=['aborted', 'undefined_task'],
preempted_outcomes=['preempted'],
goal_key='task_goal'
)
# Create and start the introspection server
sis = IntrospectionServer('smach_tasker_action', sm_tasker, '/SM_ROOT')
sis.start()
asw.run_server()
rospy.spin()
sis.stop()
