def _test_smach_execute2():
rospy.init_node('smach')
rospy.on_shutdown(myhook)
sq = Sequence(outcomes=['succeeded', 'aborted', 'preempted'],
connector_outcome='succeeded')
with sq:
Sequence.add('CHECK',
CheckSmachEnabledState(),
transitions={'aborted': 'SLEEP'})
Sequence.add('SLEEP', SleepState(10))
rospy.loginfo("Executing test...")
TransformListenerSingleton.get()
#outcome = sq.execute()
#rospy.loginfo("outcome: %s" % outcome)
# Create and start the introspection server
sis = smach_ros.IntrospectionServer('server_name', sq, '/SM_ROOT')
sis.start()
# Execute the state machine
print "EXECUTING.."
outcome = sq.execute()
# Wait for ctrl-c to stop the application
print "SPINNING.ms."
rospy.spin()
#rospy.signal_shutdown("shutting down now")
sis.stop()
def navigate_behind_cube(target, cube, squares):
sm = Sequence(outcomes=['ok', 'err'], connector_outcome='ok')
with sm:
Sequence.add('choose', ChooseNewNavGoalState(target, cube, squares,
0.5))
Sequence.add('goto', NavigateToGoalState())
return sm
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 create_sequence(list_of_states, connector_outcome="succeeded", state_names=[]):
"""Creates a Sequence (container type) that connects the provided states.
The first state in the list will also be the first state in the sequence, and so on.
All states in list_of_states must have the outcomes ["preempted", "succeeded", "aborted"].
Args:
list_of_states (list[State]): states that should be connected in a sequence
connector_outcome (str, optional):
The outcome that causes a transition to the next state. Defaults to "succeeded".
state_names (list, optional):
names that the states in the sequence are given. Defaults to [].
Returns:
Sequence: a Sequence of connected states
"""
container = Sequence(
outcomes=["preempted", "succeeded", "aborted"],
connector_outcome=connector_outcome,
)
with container:
if len(list_of_states) == len(state_names):
for name, state in zip(state_names, list_of_states):
Sequence.add(name, state)
else: # no state names provided
counter = 0
for state in list_of_states:
Sequence.add("State-%d" % counter, state)
counter += 1
return container
def move_to_obstacle(): # type: () -> StateMachine
sq = Sequence(outcomes=['ok'], connector_outcome='ok')
with sq:
Sequence.add('FOLLOW', LineFollowState(forward_speed, PIDController(kp=kp, ki=ki, kd=kd), white_filter,
TransitionAt(proximity_detector)))
Sequence.add('STOP', StopState())
return sq
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 _test_smach_execute2():
rospy.init_node('smach')
rospy.on_shutdown(myhook)
sq = Sequence(
outcomes = ['succeeded','aborted','preempted'],
connector_outcome = 'succeeded')
with sq:
Sequence.add('CHECK', CheckSmachEnabledState(),
transitions={'aborted':'SLEEP'})
Sequence.add('SLEEP', SleepState(10))
rospy.loginfo("Executing test...")
TransformListenerSingleton.get()
#outcome = sq.execute()
#rospy.loginfo("outcome: %s" % outcome)
# Create and start the introspection server
sis = smach_ros.IntrospectionServer('server_name', sq, '/SM_ROOT')
sis.start()
# Execute the state machine
print "EXECUTING.."
outcome = sq.execute()
# Wait for ctrl-c to stop the application
print "SPINNING.ms."
rospy.spin()
#rospy.signal_shutdown("shutting down now")
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 on_ramp(): # type: () -> StateMachine
sq = Sequence(outcomes=['ok'], connector_outcome='ok')
with sq:
Sequence.add('GOAL_ONRAMP',
NavigateToNamedPoseState('on_ramp'),
transitions={'err': 'ABSORB'})
Sequence.add('ABSORB', AbsorbResultState())
return sq
def move_to_stop_line(lower=False): # type: (bool) -> StateMachine
detector = RedDetector() if not lower else RedLowerDetector()
sq = Sequence(outcomes=['ok'], connector_outcome='ok')
with sq:
# Forward until stop line
Sequence.add('FOLLOW', LineFollowState(forward_speed, PIDController(kp=kp, ki=ki, kd=kd), white_filter,
TransitionAfter(detector)))
Sequence.add('STOP', StopState())
return sq
def execute(self, ud):
sq = Sequence(outcomes=['ok', 'err'], connector_outcome='ok')
with sq:
Sequence.add('SELECT_NUMBER', SelectNumberState(min=1, max=8))
Sequence.add('GOAL', NavigateToNumberState())
outcome = sq.execute(ud)
if outcome == 'ok':
notify_unmarked()
return 'ok'
def main():
rospy.init_node('mission_planner')
sq = Sequence(outcomes=['ok', 'err'],
connector_outcome='ok',
input_keys=['coord'])
with sq:
Sequence.add('move-to-coord', MoveRoverToCoord())
sq.execute({'coord': (1.0, 1.0)})
def ext_plan():
sq = Sequence(outcomes=['succeeded', 'aborted', 'preempted'], connector_outcome='succeeded')
sq.userdata.position = EXT_POSITION;
plan = gensm_plan_vis_exec(PlanExtAxisState(), confirm=CONFIRM, visualize=VISUALIZE, execute=EXECUTE)
with sq:
Sequence.add("EXTA", plan)
return sq
def add_waypoints(waypoints):
for key, (x, y), angle in waypoints:
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'odom'
goal.target_pose.pose.position.x = x
goal.target_pose.pose.position.y = y
goal.target_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, radians(angle)))
Sequence.add(key, SimpleActionState('move_base', MoveBaseAction, goal=goal),
transitions={'preempted': Transitions.FAILURE, 'aborted': Transitions.FAILURE})
def execute(self, ud):
sq = Sequence(outcomes=['ok', 'err'],
connector_outcome='ok',
output_keys=['green_shape'])
with sq:
Sequence.add('LOCATION2',
Location2State(),
transitions={'err': 'TURN'})
Sequence.add('TURN', RotateState(np.pi))
return sq.execute(ud)
def execute(self, ud):
sq = Sequence(outcomes=['ok', 'err'], connector_outcome='ok')
with sq:
Sequence.add('GOAL_OFFRAMP_START',
NavigateToNamedPoseState('off_ramp_start'),
transitions={'err': 'GOAL_OFFRAMP_END'})
Sequence.add('GOAL_OFFRAMP_END',
NavigateToNamedPoseState('off_ramp_end'))
sq.execute(ud)
return 'ok'
def main():
# Make a placeholder state machine for now. Can replace with something
# interesting later.
rospy.init_node('state_machine')
sm = Sequence(outcomes=['preempted'], connector_outcome='ok')
with sm:
Sequence.add('forward', forward())
Sequence.add('turn', turn(), {'ok': 'forward'})
sm.execute()
def location2(): # type: () -> StateMachine
sq = Sequence(outcomes=['ok'],
connector_outcome='ok',
output_keys=['green_shape'])
with sq:
Sequence.add('LOCATION2',
Location2State(),
transitions={'err': 'TURN'})
Sequence.add('TURN', RotateState(np.pi))
return sq
def GoMoveit(robot, arms):
sq = Sequence(outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['side', 'trayectory_name'],
connector_outcome='succeeded')
with sq:
Sequence.add('TRAYECTORY',
basic.SetPositionNamed(robot, arms, blind=False))
return sq
def GoHomeSafe(robot, arms):
sq = Sequence(outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['side', 'joint_goal'],
connector_outcome='succeeded')
sq.userdata.home_safe = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
with sq:
Sequence.add(
'PRE2_PRE1',
basic.SetPositionNamed(robot,
arms,
blind=True,
init='pre_2',
goal='pre_1'))
Sequence.add('SLEEP', basic.Sleep(robot, 1))
Sequence.add('PRE1_HOME',
basic.JointGoal(robot),
remapping={
'joint_goal': 'joint_goal',
'side': 'side'
})
Sequence.add('HOME',
basic.JointGoal(robot),
remapping={
'joint_goal': 'home_safe',
'side': 'side'
})
return sq
def execute(self, ud):
sq = Sequence(outcomes=['ok'], connector_outcome='ok')
with sq:
Sequence.add(
'FOLLOW',
LineFollowState(forward_speed,
PIDController(kp=kp, ki=ki,
kd=kd), white_filter,
TransitionAt(proximity_detector)))
Sequence.add('STOP', StopState())
return sq.execute(ud)
def off_ramp(): # type: () -> StateMachine
sq = Sequence(outcomes=['ok'], connector_outcome='ok')
with sq:
Sequence.add('INIT_AMCL', InitAMCLState(initial_pose=lambda: get_named_pose('initial_pose'), clear_costmaps=True))
Sequence.add('GOAL_OFFRAMP_START', NavigateToNamedPoseState('off_ramp_start'), transitions={'err': 'GOAL_OFFRAMP_END'})
Sequence.add('GOAL_OFFRAMP_END', NavigateToNamedPoseState('off_ramp_end'), transitions={'err': 'ABSORB'})
Sequence.add('ABSORB', AbsorbResultState())
return sq
def get_go_and_return_smach():
sq = Sequence(outcomes=['aborted', 'preempted'],
connector_outcome='succeeded')
sq.userdata.goal_position_x = 1
sq.userdata.goal_position_y = 0
sq.userdata.goal_position_yaw = 1
sq.userdata.saved_position_x = 1
sq.userdata.saved_position_y = 0
sq.userdata.saved_position_yaw = 1
wfg = move_base.WaitForGoalState() # We don't want multiple subscribers so we need one WaitForGoal state
with sq:
### Add states to the container
# save position
Sequence.add('SAVE_ROBOT_POSITION', move_base.ReadRobotPositionState(),
remapping={'x':'saved_position_x',
'y':'saved_position_y',
'yaw':'saved_position_yaw'},
)
# wait for new goal
Sequence.add('WAIT_FOR_GOAL', wfg,
remapping={'x':'goal_position_x',
'y':'goal_position_y',
'yaw':'goal_position_yaw'}
)
# nav to goal
Sequence.add('MOVE_BASE_GO', move_base.MoveBaseState('/map'),
remapping={'x':'goal_position_x',
'y':'goal_position_y',
'yaw':'goal_position_yaw'
}
)
# wait
Sequence.add('PAUSE_AT_GOAL', util.SleepState(2))
# nav back
Sequence.add('MOVE_BASE_RETURN', move_base.MoveBaseState('/map'),
remapping={'x':'saved_position_x',
'y':'saved_position_y',
'yaw':'saved_position_yaw'
},
transitions={'succeeded':'SAVE_ROBOT_POSITION'}
)
return sq
def enter_split(): # type: () -> StateMachine
sq = Sequence(outcomes=['ok'], connector_outcome='ok')
with sq:
Sequence.add('FOLLOW_W', LineFollowState(forward_speed, PIDController(kp=kp, ki=ki, kd=kd), white_filter,
TransitionAt(RedDetector())))
Sequence.add('FOLLOW_R', LineFollowState(forward_speed, PIDController(kp=kp, ki=ki, kd=kd), red_filter,
TransitionAfter(RedDetector())))
Sequence.add('STOP', StopState())
Sequence.add('TURN', RotateState(np.pi / 2 * 2 / 3))
return sq
def add_waypoints(waypoints):
for key, (x, y), angle in waypoints:
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'odom'
goal.target_pose.pose.position.x = x
goal.target_pose.pose.position.y = y
goal.target_pose.pose.orientation = Quaternion(
*quaternion_from_euler(0, 0, radians(angle)))
Sequence.add(key,
SimpleActionState('move_base', MoveBaseAction, goal=goal),
transitions={
'preempted': Transitions.FAILURE,
'aborted': Transitions.FAILURE
})
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 execute(self, ud):
seq = Sequence(outcomes=['ok', 'err'], connector_outcome='ok')
with seq:
for index, waypoint in enumerate(ud.waypoints): # type: int
seq.add('NavigateTo{}'.format(index),
NavigateToWaypointState(waypoint))
return seq.execute()
def test_action_client_timeout(self):
"""Test simple action state server timeout"""
sq = Sequence(['succeeded', 'aborted', 'preempted'], 'succeeded')
sq.userdata['g1'] = g1
with sq:
# Test single goal policy
Sequence.add(
'GOAL_STATIC',
SimpleActionState("reference_action_not_available",
TestAction,
goal=g1,
server_wait_timeout=rospy.Duration(1.0)))
sq_outcome = sq.execute()
def run(self):
sq = Sequence(outcomes=['Completed_Successfully', 'Aborted'],
connector_outcome='Completed_Successfully')
with sq:
# Iterate through each dist in front, going that dist
# and then drawing a dot.
for i, dist in enumerate(self.dists_in_front):
go_fwd_lbl = 'Go Forward %d' % i
sleep_lbl = 'Sleep %d' % i
Sequence.add(go_fwd_lbl,
GoForward(dist),
transitions={'Aborted': 'Aborted'})
Sequence.add(sleep_lbl,
Draw(),
transitions={'Aborted': 'Aborted'})
# start the state machine
return sq.execute()
def get_random_goal_smach(frame="/base_link"):
"""Return a SMACH Sequence for navigation to a newly randomly calulated goal.
Combines CalcRandomGoalState with MoveBaseState
frame: defaults to /base_link
"""
sq = Sequence(outcomes=["succeeded", "aborted", "preempted"], connector_outcome="succeeded")
sq.userdata.x = 0
sq.userdata.y = 0
sq.userdata.yaw = 0
with sq:
# implicit usage of above userdata
Sequence.add("CALC_RANDOM_GOAL", CalcRandomGoalState())
Sequence.add("MOVE_RANDOM_GOAL", MoveBaseState(frame))
return sq
def get_random_goal_smach(frame='/base_link'):
"""Return a SMACH Sequence for navigation to a newly randomly calulated goal.
Combines CalcRandomGoalState with MoveBaseState
frame: defaults to /base_link
"""
sq = Sequence(outcomes=['succeeded', 'aborted', 'preempted'], connector_outcome='succeeded')
sq.userdata.x = 0
sq.userdata.y = 0
sq.userdata.yaw = 0
with sq:
# implicit usage of above userdata
Sequence.add("CALC_RANDOM_GOAL", CalcRandomGoalState())
Sequence.add("MOVE_RANDOM_GOAL", MoveBaseState(frame))
return sq
def location1(cam_pixel_to_point): # type: (CamPixelToPointServer) -> StateMachine
sq = Sequence(outcomes=['ok'], connector_outcome='ok')
with sq:
Sequence.add('TURN_L', RotateState(np.pi/2))
Sequence.add('LOCATION1', Location1State(cam_pixel_to_point), transitions={'err': 'TURN_R'})
Sequence.add('TURN_R', RotateState(-np.pi/2))
return sq
def draw_points(self, waypoints):
""" Draws the given waypoints.
waypoints : a list of point objects with x, y, and z fields.
"""
rows = self.extract_rows(waypoints)
sq = Sequence(outcomes=['Completed_Successfully', 'Aborted'],
connector_outcome='Completed_Successfully')
list_of_rows = []
# How far did we travel?
dists_back = []
for row in rows:
abs_dists_in_front = [point.y for point in row]
# Keep track of the farthest distance traveled
dists_back.append(abs_dists_in_front[-1])
dists_in_front = self.abs_to_rel(abs_dists_in_front)
list_of_rows.append(dists_in_front)
# assume all lines are spaced equally, and that lines are
# seperated perfectly on the x axis
line_spacing = abs(rows[0][0].x - rows[1][0].x)
print 'Line Spacing: %f' % line_spacing
with sq:
# Turn to the left initially to start things off.
Sequence.add('Initial Turn',
Turn(-1 * math.pi / 2),
transitions={'Aborted': 'Aborted'})
for i, row in enumerate(rows):
print 'Added row ' + str(i) + ' to State Machine'
dists_in_front = list_of_rows[i]
# Draw a Row
Sequence.add('Draw Row %d' % i,
DrawRow(dists_in_front),
transitions={'Aborted': 'Aborted'})
# If we're on the last line, no need to return
if i < len(rows) - 1:
# Return back to the beginning of row
Sequence.add('Go Back %d' % i,
GoForward(-1 * dists_back[i]),
transitions={'Aborted': 'Aborted'})
# Go to the next line
Sequence.add('Carriage Return %d' % i,
CarriageReturn(line_spacing),
transitions={'Aborted': 'Aborted'})
return sq.execute()
def away_plan():
sq = Sequence(outcomes=['succeeded', 'aborted', 'preempted'], connector_outcome='succeeded')
pose = PoseStamped()
pose.header.frame_id = 'base_link'
pose.pose.position.x = AWAY_X
pose.pose.position.y = AWAY_Y
pose.pose.position.z = AWAY_Z
pose.pose.orientation = DRAW_ORIENTATION
sq.userdata.goal = pose
sq.userdata.ik_link = AWAY_HAND_LINK
sq.userdata.frame_id = FRAME_ID
plan = gensm_plan_vis_exec(Plan1ToXtionPoseState(), confirm=CONFIRM, visualize=VISUALIZE, execute=EXECUTE)
with sq:
Sequence.add("EXTA", plan)
return sq
def GoHome(robot, arms):
sq = Sequence(outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['side', 'trayectory_name'],
connector_outcome='succeeded')
with sq:
Sequence.add(
'PRE2_PRE1',
basic.SetPositionNamed(robot,
arms,
blind=True,
init='pre_2',
goal='pre_1'))
Sequence.add(
'PRE1_HOME',
basic.JO(robot, arms, blind=False, init='pre_1', goal='home'))
return sq
def execute(self, ud):
sq = Sequence(outcomes=['ok', 'err'], connector_outcome='ok')
with sq:
Sequence.add('TURN_L', RotateState(np.pi / 2))
Sequence.add('LOCATION1',
Location1State(),
transitions={'err': 'TURN_R'})
Sequence.add('TURN_R', RotateState(-np.pi / 2))
return sq.execute(ud)
def test_action_preemption(self):
"""Test action preemption"""
sq = Sequence(['succeeded', 'aborted', 'preempted'], 'succeeded')
class SlowRunningState(State):
def __init__(self):
State.__init__(self,
outcomes=['succeeded', 'aborted', 'preempted'])
def execute(self, ud):
start_time = rospy.Time.now()
while rospy.Time.now() - start_time < rospy.Duration(10):
rospy.sleep(0.05)
if self.preempt_requested():
self.service_preempt()
return 'preempted'
return 'succeeded'
with sq:
Sequence.add('PREEMPT_ME', SlowRunningState())
asw = ActionServerWrapper('preempt_action_sm',
TestAction,
sq,
succeeded_outcomes=['succeeded'],
aborted_outcomes=['aborted'],
preempted_outcomes=['preempted'])
asw.run_server()
ac = SimpleActionClient('preempt_action_sm', TestAction)
ac.wait_for_server(rospy.Duration(30))
ac.send_goal(g1)
rospy.sleep(5.0)
ac.cancel_goal()
start_time = rospy.Time.now()
while ac.get_state() == GoalStatus.ACTIVE and rospy.Time.now(
) - start_time < rospy.Duration(30):
rospy.sleep(0.5)
assert ac.get_state() == GoalStatus.PREEMPTED
def park_into_pose(goal, offset=0.5): # type: (Callable[[], PoseStamped]) -> StateMachine
v = 0.2
end_offset = 0.09
dt = offset / v
sq = Sequence(outcomes=['ok', 'err'], connector_outcome='ok')
with sq:
sq.add('MOVE_TO_OFFSET', NavigateToGoalState(offset_goal(goal, offset)))
# sq.add('MOVE_FORWARD', ForwardState(v, dt))
sq.add('MOVE_FORWARD', MoveToState(offset_goal(goal, -end_offset), v))
sq.add('STOP', StopState())
return sq
def create_fish_sequence():
seq = Sequence(outcomes=[Transitions.SUCCESS, Transitions.FAILURE],
connector_outcome=Transitions.SUCCESS)
approach = (
('approach', mirror_point(0.60, 0.3), -90),
('approach2', mirror_point(0.60, 0.15), -90),
)
drop = (
('get_out', mirror_point(0.85, 0.25), -180),
('drop', mirror_point(1.22, 0.12), -180),
)
with seq:
add_waypoints(approach)
Sequence.add('calage', FishApproachState())
Sequence.add('grab_fish', FishAndHoldState())
add_waypoints(drop)
Sequence.add('drop_fish', FishDropState())
Sequence.add('end_fishing', FishCloseState())
return seq
def get_per_goal_subsmach():
sq = Sequence(outcomes=['succeeded', 'aborted', 'preempted'],
connector_outcome='succeeded',
input_keys=['x', 'y', 'yaw'])
with sq:
# check for permission
Sequence.add('SLEEP_UNTIL_ENABLED', util.get_sleep_until_smach_enabled_smach())
# nav to goal
Sequence.add('MOVE_BASE_GOAL', move_base.MoveBaseState(),
remapping={'x':'x',
'y':'y',
'yaw':'yaw'}
)
# wait
Sequence.add('PAUSE_AT_GOAL', util.SleepState(5))
return sq
def test_runner():
rospy.init_node('runner_test')
runner = Runner()
sq = Sequence(outcomes=['succeeded', 'aborted', 'preempted'],
connector_outcome='succeeded')
wfg = WaitForGoalState() # We don't want multiple subscribers so we need one WaitForGoal state
with sq:
Sequence.add('SLEEP', SleepState(5))
Sequence.add('WAIT_FOR_GOAL', wfg,
transitions={'aborted':'SLEEP'})
Sequence.add('MOVE_BASE_RGOAP', MoveBaseRGOAPState(runner),
transitions={'succeeded':'SLEEP'})
execute_smach_container(sq, enable_introspection=True)
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 get_patrol_smach():
sq = Sequence(outcomes=['succeeded', 'aborted', 'preempted'],
connector_outcome='succeeded')
sq.userdata.saved_position_x = 1
sq.userdata.saved_position_y = 0
sq.userdata.saved_position_yaw = 1
sq.userdata.goal_position_1_x = 1
sq.userdata.goal_position_1_y = 0
sq.userdata.goal_position_1_yaw = 1
sq.userdata.goal_position_2_x = 1
sq.userdata.goal_position_2_y = 0
sq.userdata.goal_position_2_yaw = 1
wfg = move_base.WaitForGoalState() # We don't want multiple subscribers so we need one WaitForGoal state
with sq:
## Add states to the container
# save position
Sequence.add('SAVE_ROBOT_POSITION', move_base.ReadRobotPositionState(),
remapping={'x':'saved_position_x',
'y':'saved_position_y',
'yaw':'saved_position_yaw'},
)
## command details
# wait for goal 1
Sequence.add('WAIT_FOR_GOAL_1', wfg,
remapping={'x':'goal_position_1_x',
'y':'goal_position_1_y',
'yaw':'goal_position_1_yaw'}
)
# wait for goal 2
Sequence.add('WAIT_FOR_GOAL_2', wfg,
remapping={'x':'goal_position_2_x',
'y':'goal_position_2_y',
'yaw':'goal_position_2_yaw'}
)
## action
# goal 1
Sequence.add('MOVE_GOAL_1', get_per_goal_subsmach(),
remapping={'x':'goal_position_1_x',
'y':'goal_position_1_y',
'yaw':'goal_position_1_yaw'
},
transitions={'aborted':'MOVE_GOAL_2'}
)
# goal 2
Sequence.add('MOVE_GOAL_2', get_per_goal_subsmach(),
remapping={'x':'goal_position_2_x',
'y':'goal_position_2_y',
'yaw':'goal_position_2_yaw'},
transitions={'aborted':'MOVE_GOAL_1',
'succeeded':'MOVE_GOAL_1'} # continue with goal 1
)
## ending
# nav back
Sequence.add('MOVE_BASE_RETURN', move_base.MoveBaseState(),
remapping={'x':'saved_position_x',
'y':'saved_position_y',
'yaw':'saved_position_yaw'}
)
return sq
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():
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 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()
def _get_vertical_sequence(task, x, y, z, phi, grab_width, parent_frame):
sq = Sequence(
outcomes = ['succeeded','aborted','preempted'],
connector_outcome = 'succeeded')
sq.userdata.x = x
sq.userdata.y = y
sq.userdata.z = z
sq.userdata.z_pre = z + PRE_GRAB_OFFSET # cm pre grab position offset
sq.userdata.phi = phi
sq.userdata.parent_frame = parent_frame
with sq:
Sequence.add('MOVE_ARM_GRAB_PRE',
MoveArmVerticalGrabState(),
remapping={'x':'x',
'y':'y',
'z':'z_pre',
'phi':'phi',
'parent_frame':'parent_frame'}
)
if(task == 'grab'):
Sequence.add('MOVE_GRIPPER_OPEN',
move_joints.get_move_gripper_state(GRIPPER_MAX_WIDTH))
Sequence.add('MOVE_ARM_GRAB',
MoveArmVerticalGrabState(),
remapping={'x':'x',
'y':'y',
'z':'z',
'phi':'phi',
'parent_frame':'parent_frame'}
)
if(task == 'grab'):
Sequence.add('MOVE_GRIPPER_CLOSE',
move_joints.get_move_gripper_state(grab_width))
else:
Sequence.add('MOVE_GRIPPER_OPEN',
move_joints.get_move_gripper_state(GRIPPER_MAX_WIDTH))
Sequence.add('MOVE_ARM_GRAB_POST',
MoveArmVerticalGrabState(),
remapping={'x':'x',
'y':'y',
'z':'z_pre',
'phi':'phi',
'parent_frame':'parent_frame'}
)
return sq
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 _get_transport_box_smach():
sq = Sequence(outcomes=['succeeded', 'aborted', 'preempted'],
connector_outcome='succeeded')
sq.userdata.gripper_x = 0.33
sq.userdata.gripper_y = 0
sq.userdata.gripper_z = 0.42
sq.userdata.gripper_phi = math.radians(10)
# sq.userdata.x = 0.33
# sq.userdata.y = 0
# sq.userdata.z = 0.42
# sq.userdata.phi = 0
with sq:
## Add states to the container
Sequence.add('MOVE_BASE_Forward', move_base.get_move_base_in_odom_state(1, 0))
Sequence.add('MOVE_ARM_GRAB_0',
grab_vertical.get_vertical_grab_sequence(-0.23, -0.5, 0.85 + 0.1,
math.radians(90),
BOX_THICKNESS,
"/base_link")
)
Sequence.add('MOVE_ARM_ZERO', move_arm.get_move_arm_to_zero_state())
Sequence.add('MOVE_BASE_Backward', move_base.get_move_base_in_odom_state(0, 0))
Sequence.add('MOVE_ARM_DROP_0',
grab_vertical.get_vertical_drop_sequence(-0.23, -0.5, 0.85 + 0.1,
math.radians(90),
BOX_THICKNESS,
"/base_link")
)
Sequence.add('MOVE_ARM_ZERO_2', move_arm.get_move_arm_to_zero_state())
# Sequence.add('MOVE_ARM_GRAB_0',
# grab_vertical.get_vertical_grab_sequence(-0.23, -0.5, 0.85+0.1,
# math.radians(90),
# BOX_THICKNESS,
# "/base_link")
# )
#
#
# Sequence.add('MOVE_ARM_ZERO', move_arm.get_move_arm_to_zero_state())
#
# Sequence.add('MOVE_BASE_Forward', move_base.get_move_base_in_odom_state(1, 0));
#
# Sequence.add('MOVE_ARM_DROP_0',
# grab_vertical.get_vertical_drop_sequence(-0.23, -0.5, 0.85+0.1,
# math.radians(90),
# BOX_THICKNESS,
# "/base_link")
# )
#
# Sequence.add('MOVE_ARM_ZERO_2', move_arm.get_move_arm_to_zero_state())
#
#
# Sequence.add('MOVE_BASE_Backward', move_base.get_move_base_in_odom_state(0, 0));
return sq
def main():
rospy.init_node('paint')
sq = Sequence(outcomes=['succeeded', 'aborted', 'preempted'], connector_outcome='succeeded')
with sq:
Sequence.add('OPEN_GRIPPER', GripperState(2, True), {'succeeded':'TURN', 'aborted':'HOME'})
Sequence.add("TURN", ext_plan(), transitions={'aborted':'HOME', 'succeeded':'AWAY'})
Sequence.add("AWAY", away_plan(), transitions={'aborted':'HOME', 'succeeded':'GRAB'})
Sequence.add("GRAB", grab_plan(sq), transitions={'aborted':'HOME', 'succeeded':'DRAW'})
Sequence.add("DRAW", draw_plan(), transitions={'aborted':'HOME', 'succeeded':'RELEASE'})
Sequence.add("RELEASE", GripperState(2, True), transitions={'aborted':'HOME', 'succeeded':'HOME'})
# TODO
# Open hand to release -> HOME
Sequence.add("HOME", home_plan(), transitions={'aborted':'POWER_OFF'})
Sequence.add("POWER_OFF", SetServoPowerOffState())
sis = smach_ros.IntrospectionServer('paint', sq, '/SM_ROOT')
sis.start()
os.system('clear')
outcome = sq.execute()
rospy.loginfo("State machine exited with outcome: " + outcome)
sis.stop()
