def main():
rospy.init_node('smach_example_actionlib_service_state')
# Register a gripper service
s = rospy.Service('gripper_srv', GripperSrv, gripper_srv)
# Create a SMACH state machine
sm0 = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
# Set userdata
sm0.userdata.max_effort = 9.0
sm0.userdata.position = Point()
sm0.userdata.gripper_input = 9.0
# Open the container
with sm0:
# Add states to the container
# Empty request message
smach.StateMachine.add(
'TRIGGER_GRIPPER_EMPTY_REQUEST',
smach_ros.ServiceState('gripper_srv', GripperSrv),
transitions={'succeeded': 'TRIGGER_GRIPPER_FIXED_REQUEST'})
# Fixed request message
smach.StateMachine.add(
'TRIGGER_GRIPPER_FIXED_REQUEST',
smach_ros.ServiceState('gripper_srv',
GripperSrv,
request=GripperSrvRequest(4.0, Point())),
transitions={'succeeded': 'TRIGGER_GRIPPER_USER_DATA_REQUEST'})
# Request from user data
smach.StateMachine.add(
'TRIGGER_GRIPPER_USER_DATA_REQUEST',
smach_ros.ServiceState('gripper_srv',
GripperSrv,
request_slots=['max_effort', 'position']),
transitions={'succeeded': 'TRIGGER_GRIPPER_REQUEST_CALLBACK'})
# Request callback
@smach.cb_interface(input_keys=['gripper_input'])
def gripper_request_cb(userdata, request):
gripper_request = GripperSrvRequest()
gripper_request.position.x = 2.0
gripper_request.max_effort = userdata.gripper_input
return gripper_request
smach.StateMachine.add('TRIGGER_GRIPPER_REQUEST_CALLBACK',
smach_ros.ServiceState(
'gripper_srv',
GripperSrv,
request_cb=gripper_request_cb,
input_keys=['gripper_input']),
transitions={'succeeded': 'succeeded'})
# Execute SMACH plan
outcome = sm0.execute()
rospy.signal_shutdown('All done.')
def PlaceObject(attempts=3):
""" Place a given object, retrying up to a given number of times """
it = smach.Iterator(
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['object_name', 'support_surf', 'place_pose'],
output_keys=[],
it=lambda: range(0, attempts),
it_label='attempt',
exhausted_outcome='aborted')
with it:
sm = smach.StateMachine(
outcomes=['succeeded', 'preempted', 'aborted', 'continue'],
input_keys=['object_name', 'support_surf', 'place_pose'],
output_keys=[])
with sm:
smach.StateMachine.add(
'PlaceObject',
smach_ros.SimpleActionState(
'place_object',
thorp_msgs.PlaceObjectAction,
goal_slots=['object_name', 'support_surf', 'place_pose'],
result_slots=[]),
remapping={
'object_name': 'object_name',
'support_surf': 'support_surf',
'place_pose': 'place_pose'
},
transitions={
'succeeded': 'succeeded',
'preempted': 'preempted',
'aborted': 'ClearOctomap'
})
smach.StateMachine.add('ClearOctomap',
smach_ros.ServiceState(
'clear_octomap', std_srvs.Empty),
transitions={
'succeeded': 'continue',
'preempted': 'preempted',
'aborted': 'aborted'
})
# TODOs:
# - we should open the gripper, in case we have picked an object
# - check error and, if collision between parts of the arm, move a bit the arm --> not enough info
# - this doesn't make too much sense as a loop... better try all our tricks and exit
# - can I reuse the same for place and MoveToTarget???
smach.Iterator.set_contained_state('', sm, loop_outcomes=['continue'])
return it
def main():
rospy.init_node('state_machine_node')
# Create a TOP level SMACH state machine
top_sm = smach.StateMachine(['succeeded', 'preempted', 'aborted'])
# Open the container
with top_sm:
# ===================================== SarThreads =====================================
# Callback for custom outcomes from SarThreads #
def out_cb(outcome_map):
# To be completed
return 'aborted'
# Create a Concurrence container
sarthreads_concurrence = smach.Concurrence(
outcomes=['missionAbort', 'aborted'],
default_outcome='missionAbort',
child_termination_cb=lambda so: True,
outcome_cb=out_cb
)
# Open the container
with sarthreads_concurrence:
# ===================================== SarBehavior =====================================
# Create a State Machine container
sarbehavior_sm = smach.StateMachine(
outcomes=['succeeded', 'preempted', 'aborted']
)
# Open the container
with sarbehavior_sm:
# ===================================== IdleThreads =====================================
# Callback for custom outcomes from IdleThreads #
def out_cb(outcome_map):
# To be completed
return 'aborted'
# Create a Concurrence container
idlethreads_concurrence = smach.Concurrence(
outcomes=['missionStart', 'aborted'],
default_outcome='missionStart',
child_termination_cb=lambda so: True,
outcome_cb=out_cb
)
# Open the container
with idlethreads_concurrence:
# ADD Idle to IdleThreads #
smach.Concurrence.add('Idle',
# NOT_SUPPORTED
)
# ADD IdleEventMonitoring to IdleThreads #
smach.Concurrence.add('IdleEventMonitoring',
smach_ros.MonitorState('bridge/events/mission_start',
SimpleEvent,
cond_cb=lambda ud, msg: False)
)
# ===================================== IdleThreads END =====================================
# ADD IdleThreads to SarBehavior #
smach.StateMachine.add('IdleThreads',
idlethreads_concurrence,
transitions={'missionStart':'TakeOff'}
)
# ADD TakeOff to SarBehavior #
smach.StateMachine.add('TakeOff',
smach_ros.SimpleActionState('cmd/takeoff',
TakeOffAction,
goal=TakeOffGoal(1.5)),
transitions={'succeeded':'Coverage'}
)
# ADD Coverage to SarBehavior #
smach.StateMachine.add('Coverage',
smach_ros.SimpleActionState('uav_coverage',
CoverageAction),
transitions={'succeeded':'SelectRover'}
)
# ADD SelectRover to SarBehavior #
smach.StateMachine.add('SelectRover',
smach_ros.SimpleActionState('cmd/assign_task',
AssignTaskAction,
goal_slots=['target_id', 'pose']),
transitions={'succeeded':'Tracking', 'aborted':'SelectRover'}
)
# ADD Tracking to SarBehavior #
smach.StateMachine.add('Tracking',
smach_ros.SimpleActionState('uav_tracking',
TrackingAction,
goal_slots=['target', 'cps']),
transitions={'succeeded':'Coverage', 'aborted':'LocalCoverage'}
)
# ADD LocalCoverage to SarBehavior #
smach.StateMachine.add('LocalCoverage',
smach_ros.SimpleActionState('uav_local_coverage',
CoverageAction),
transitions={'aborted':'Coverage', 'succeeded':'Tracking'}
)
# ===================================== SarBehavior END =====================================
# ADD SarBehavior to SarThreads #
smach.Concurrence.add('SarBehavior', sarbehavior_sm)
# ADD AbortEventMonitoring to SarThreads #
smach.Concurrence.add('AbortEventMonitoring',
smach_ros.MonitorState('bridge/events/mission_abort',
SimpleEvent,
cond_cb=lambda ud, msg: False)
)
# ===================================== SarThreads END =====================================
# ADD SarThreads to TOP state #
smach.StateMachine.add('SarThreads',
sarthreads_concurrence,
transitions={'missionAbort':'MissionAbort'}
)
# ===================================== MissionAbort =====================================
# Create a State Machine container
missionabort_sm = smach.StateMachine(
outcomes=['succeeded']
)
# Open the container
with missionabort_sm:
# ADD Land to MissionAbort #
smach.StateMachine.add('Land',
smach_ros.ServiceState('cmd/land',
Empty),
transitions={}
)
# ===================================== MissionAbort END =====================================
# ADD MissionAbort to TOP state #
smach.StateMachine.add('MissionAbort',
missionabort_sm,
transitions={'succeeded':'SarThreads'}
)
# Create and start the introspection server (uncomment if needed)
# sis = smach_ros.IntrospectionServer('smach_server', top_sm, '/SM_TOP')
# sis.start()
# Execute SMACH plan
outcome = top_sm.execute()
# Wait for ctrl-c to stop the application
rospy.spin()
transitions={
'succeeded': 'ObjectDetection',
'preempted': 'preempted',
'aborted': 'ObjectDetection'
})
smach.StateMachine.add('FoldArmAndRelax',
FoldArm(),
transitions={
'succeeded': 'RelaxArmAndStop',
'preempted': 'preempted',
'aborted': 'error'
})
smach.StateMachine.add('RelaxArmAndStop',
smach_ros.ServiceState('servos/relax_all',
arbotix_srvs.Relax),
transitions={
'succeeded': 'stop',
'preempted': 'stop',
'aborted': 'error'
})
# Construct action server wrapper for top-level sm to control it with keyboard commands
asw = smach_ros.ActionServerWrapper('user_commands_action_server',
thorp_msgs.UserCommandAction,
wrapped_container=sm,
succeeded_outcomes=['stop'],
aborted_outcomes=['aborted'],
preempted_outcomes=['error'],
goal_key='user_command',
feedback_key='ucmd_progress',
def ObjectDetection():
""" Object detection sub state machine; iterates over object_detection action state and recovery
mechanism until an object is detected, it's preempted or there's an error (aborted outcome) """
class ObjDetectedCondition(smach.State):
""" Check for the object detection result to retry if no objects where detected """
def __init__(self):
smach.State.__init__(
self,
outcomes=['preempted', 'satisfied', 'fold_arm', 'retry'],
input_keys=['od_attempt', 'object_names'],
output_keys=['od_attempt'])
def execute(self, userdata):
if self.preempt_requested():
self.service_preempt()
return 'preempted'
if len(userdata.object_names) > 0:
userdata.od_attempt = 0
return 'satisfied'
userdata.od_attempt += 1
if userdata.od_attempt == 1:
return 'fold_arm'
return 'retry'
sm = smach.StateMachine(
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['od_attempt', 'output_frame'],
output_keys=['objects', 'object_names', 'support_surf'])
with sm:
smach.StateMachine.add('ClearOctomap',
smach_ros.ServiceState('clear_octomap',
std_srvs.Empty),
transitions={
'succeeded': 'ObjectDetection',
'preempted': 'preempted',
'aborted': 'ObjectDetection'
})
smach.StateMachine.add(
'ObjectDetection',
smach_ros.SimpleActionState(
'object_detection',
thorp_msgs.DetectObjectsAction,
goal_slots=['output_frame'],
result_slots=['objects', 'object_names', 'support_surf']),
remapping={
'output_frame': 'output_frame',
'object_names': 'object_names',
'support_surf': 'support_surf'
},
transitions={
'succeeded': 'ObjDetectedCondition',
'preempted': 'preempted',
'aborted': 'aborted'
})
smach.StateMachine.add('ObjDetectedCondition',
ObjDetectedCondition(),
remapping={'object_names': 'object_names'},
transitions={
'satisfied': 'succeeded',
'preempted': 'preempted',
'fold_arm': 'FoldArm',
'retry': 'ClearOctomap'
})
smach.StateMachine.add('FoldArm',
FoldArm(),
transitions={
'succeeded': 'ClearOctomap',
'preempted': 'preempted',
'aborted': 'ClearOctomap'
})
return sm
def main():
rospy.init_node('state_machine_node')
if not rospy.has_param('~altitude'):
rospy.logerr('Altitude not specified, cannot perform simulation!')
return
# Create a TOP level SMACH state machine
top_sm = smach.StateMachine(['succeeded', 'preempted', 'aborted'])
# Open the container
with top_sm:
# ===================================== SarThreads =====================================
# Callback for custom outcomes from SarThreads
def out_cb(outcome_map):
if outcome_map['AbortEventMonitoring'] == 'invalid':
rospy.loginfo('Returning missionAbort Event')
return 'missionAbort'
return 'aborted'
# Create a Concurrence container
sarthreads_concurrence = smach.Concurrence(
outcomes=['missionAbort', 'aborted'],
default_outcome='missionAbort',
child_termination_cb=lambda so: True,
outcome_cb=out_cb)
# Open the container
with sarthreads_concurrence:
# ===================================== SarBehavior =====================================
# Create a State Machine container
sarbehavior_sm = smach.StateMachine(
outcomes=['succeeded', 'preempted', 'aborted'])
# Open the container
with sarbehavior_sm:
# ADD Idle to SarBehavior #
smach.StateMachine.add('Idle',
Idle(),
transitions={'succeeded': 'Takeoff'})
# ADD Takeoff to SarBehavior #
smach.StateMachine.add('Takeoff',
smach_ros.SimpleActionState(
'cmd/takeoff',
TakeoffAction,
goal=TakeoffGoal(
rospy.get_param('~altitude'))),
transitions={'succeeded': 'Coverage'})
# ADD Coverage to SarBehavior #
smach.StateMachine.add(
'Coverage',
smach_ros.SimpleActionState(
'uav_random_coverage',
CoverageAction,
goal=CoverageGoal(rospy.get_param('~altitude')),
result_slots=['target_id', 'target_pose']),
transitions={'succeeded': 'Tracking'},
remapping={
'target_id': 'target_id',
'target_pose': 'target_pose'
})
# ===================================== SelectRoverThreads =====================================
# Callback for custom outcomes
def selectrover_outcb(outcome_map):
if outcome_map['LostEventMonitoring'] == 'invalid':
rospy.loginfo('Returning target_lost Event')
return 'target_lost'
return outcome_map['TaskAllocation']
# Create a Concurrence container
selectrover_concurrence = smach.Concurrence(
input_keys=['target_id', 'target_pose'],
outcomes=['succeeded', 'aborted', 'target_lost'],
default_outcome='aborted',
child_termination_cb=lambda so: True,
outcome_cb=selectrover_outcb)
# Open the container
with selectrover_concurrence:
# ADD TaskAllocation to SelectRoverThreads #
smach.Concurrence.add(
'TaskAllocation',
smach_ros.SimpleActionState(
'cmd/task_allocation_auction',
TaskAllocationAction,
goal_slots=['task_id', 'task_pose'],
result_slots=['task_id', 'winner', 'task_pose']),
remapping={
'task_id': 'target_id',
'task_pose': 'target_pose'
})
# ADD LostEventMonitoring to SelectRoverThreads #
smach.Concurrence.add(
'LostEventMonitoring',
smach_ros.MonitorState('target_lost',
TargetPositionEvent,
cond_cb=lambda ud, msg: False))
# ===================================== SelectRoverThreads END =====================================
# ADD SelectRoverThreads to SarBehavior #
smach.StateMachine.add('SelectRover',
selectrover_concurrence,
transitions={
'succeeded': 'Tracking',
'aborted': 'SelectRover',
'target_lost': 'LocalCoverage'
})
def tracking_goal_cb(userdata, goal):
tracking_goal = TrackingGoal()
tracking_goal.target = userdata.target
tracking_goal.altitude = rospy.get_param('~altitude')
return tracking_goal
# ADD Tracking to SarBehavior #
smach.StateMachine.add('Tracking',
smach_ros.SimpleActionState(
'uav_simple_tracking',
TrackingAction,
input_keys=['target'],
goal_cb=tracking_goal_cb),
transitions={
'succeeded': 'Coverage',
'aborted': 'LocalCoverage'
},
remapping={'target': 'target_id'})
# ADD LocalCoverage to SarBehavior #
smach.StateMachine.add(
'LocalCoverage',
smach_ros.SimpleActionState(
'uav_local_coverage',
CoverageAction,
goal=CoverageGoal(rospy.get_param('~altitude')),
result_slots=['target_id', 'target_pose']),
transitions={
'aborted': 'Coverage',
'succeeded': 'SelectRover'
},
remapping={
'target_id': 'target_id',
'target_pose': 'target_pose'
})
# ===================================== SarBehavior END =====================================
# ADD SarBehavior to SarThreads #
smach.Concurrence.add('SarBehavior', sarbehavior_sm)
# ADD AbortEventMonitoring to SarThreads #
smach.Concurrence.add(
'AbortEventMonitoring',
smach_ros.MonitorState('bridge/events/mission_abort',
SimpleEvent,
cond_cb=lambda ud, msg: False))
# ===================================== SarThreads END =====================================
# ADD SarThreads to TOP state #
smach.StateMachine.add('SarThreads',
sarthreads_concurrence,
transitions={'missionAbort': 'MissionAbort'})
# ===================================== MissionAbort =====================================
# Create a State Machine container
missionabort_sm = smach.StateMachine(
outcomes=['succeeded', 'preempted', 'aborted'])
# Open the container
with missionabort_sm:
# ADD Land to MissionAbort #
smach.StateMachine.add('Land',
smach_ros.ServiceState(
'cmd/land', std_srvs.srv.Empty),
transitions={})
# ===================================== MissionAbort END =====================================
# ADD MissionAbort to TOP state #
smach.StateMachine.add('MissionAbort',
missionabort_sm,
transitions={'succeeded': 'SarThreads'})
# Create and start the introspection server (uncomment if needed)
sis = smach_ros.IntrospectionServer('smach_server', top_sm, '/SM_TOP')
sis.start()
# Execute SMACH plan
outcome = top_sm.execute()
# Wait for ctrl-c to stop the application
rospy.spin()
sis.stop()
class Challenge1():
rospy.init_node('MBZIRC_ch1_state_machine')
sm = smach.StateMachine(outcomes=['Done'])
with sm:
def exploration_response_cb(userdata, response):
print "Exploration Response:", response.navResponse
if response.navResponse == 'succeeded':
return 'succeeded'
else:
return 'aborted'
def landing_response_cb(userdata, response):
print 'Landing Response', response.navResponse
if response.navResponse == 'succeeded':
return 'succeeded'
else:
return 'aborted'
smach.StateMachine.add('INITIATING',
initialization(),
transitions={'succeeded': 'EXPLORATION'})
############################################3
# call service with request 1 and userdata #
############################################3
smach.StateMachine.add(
'EXPLORATION',
smach_ros.ServiceState(
'controllerService',
navigation,
request_slots=['srvtype'],
#request_cb = exploration_request_cb,
response_cb=exploration_response_cb,
input_keys=['srvtype']),
transitions={
'succeeded': 'MARKER_DETECTION',
'aborted': 'MARKER_DETECTION',
'preempted': 'MARKER_DETECTION'
})
############################################3
# MArker Detection and tracking # in this state the drone will not perform RTL becuase it is build it to hover
############################################3
smach.StateMachine.add('MARKER_DETECTION',
checkMarkerDetection(),
transitions={
'tracked': 'LANDING',
'explore': 'EXPLORATION'
})
############################################3
# call service with request 2 and userdata #
############################################3
smach.StateMachine.add(
'LANDING',
ServiceState(
'controllerService',
navigation,
#request_cb = landing_request_cb,
request_slots=['srvtype'],
response_cb=landing_response_cb,
input_keys=['srvtype']),
transitions={
'succeeded': 'Done',
'aborted': 'MARKER_DETECTION',
'preempted': 'MARKER_DETECTION'
})
#######################################################3
#Before the Done There should be a disarming state
#######################################################3
#I should publish to the offboard control to change to manual mode and then to Disarm
sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
sis.start()
# Execute SMACH plan
outcome = sm.execute()
rospy.spin()
sis.stop()
def main():
rospy.init_node('drone_executive')
sm_root = smach.StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with sm_root:
# CONNECTING is the initial state,
# if connected, goto LANDED state
@smach.cb_interface(outcomes=['aborted'])
def connect_resp_cb(userdata, response):
if response.success == False:
return 'aborted'
else:
return 'succeeded'
req_connect = ail_mav.srv.ConnectMavRequest('/mavros', True)
smach.StateMachine.add('CONNECTING',
smach_ros.ServiceState(
'/mav_interface_node/connect',
ail_mav.srv.ConnectMav,
req_connect,
response_cb=connect_resp_cb),
transitions={
'succeeded': 'LANDED',
'aborted': 'CONNECTING'
})
# Come to LANDED after CONNECTING or LANDING
# as defined in class Landed, hold 3 seconds, goto TAKINGOFF
smach.StateMachine.add('LANDED',
Landed(),
transitions={'takeoff_started': 'TAKINGOFF'})
# Come to TAKINGOFF after time of LANDED expires
# if responded success, goto
# if aborted, goto LANDING
@smach.cb_interface(outcomes=['aborted'])
def takeoff_resp_cb(userdata, response):
if response.success == False:
return 'aborted'
else:
return 'succeeded'
req_takeoff = ail_mav.srv.TakeoffRequest(1.0)
smach.StateMachine.add('TAKINGOFF',
smach_ros.ServiceState(
'/mav_interface_node/takeoff',
ail_mav.srv.Takeoff,
req_takeoff,
response_cb=takeoff_resp_cb),
transitions={
'succeeded': 'FLYING_TO_WAITING_ZONE',
'aborted': 'LANDING'
})
# Come to LANDING after TAKINGOFF aborted or FLYING_TO_LAND
# goto LANDED afterwards
smach.StateMachine.add(
'LANDING',
smach_ros.ServiceState(
'/mav_interface_node/land',
ail_mav.srv.Land), #,ail_mav.srv.LandRequest.Empty),
transitions={'succeeded': 'LANDED'})
#FLYING_TO_WAITING_ZONE
goal = mav_state_machine.msg.trackingGoal()
goal.orientation_final = 0.0
goal.pos_final.x = -1.0
goal.pos_final.y = -0.4
goal.pos_final.z = 1.8
smach.StateMachine.add('FLYING_TO_WAITING_ZONE',
smach_ros.SimpleActionState(
'/traj_plan_track_action_server',
mav_state_machine.msg.trackingAction, goal),
transitions={'succeeded': 'FLYING_TO_LAND'})
goal2 = mav_state_machine.msg.trackingGoal()
goal2.orientation_final = 0.0
goal2.pos_final.x = 0.0
goal2.pos_final.y = 0.0
goal2.pos_final.z = 1.0
smach.StateMachine.add('FLYING_TO_LAND',
smach_ros.SimpleActionState(
'/traj_plan_track_action_server',
mav_state_machine.msg.trackingAction,
goal2),
transitions={'succeeded': 'LANDING'})
sis = smach_ros.IntrospectionServer('intro_server', sm_root, '/SM_ROOT')
sis.start()
outcome = sm_root.execute()
rospy.spin()
sis.stop()
smach.StateMachine.add('ExecuteUserCommand',
ExecuteUserCommand(),
transitions={'start':'ObjectDetection',
'reset':'ObjectDetection',
'fold':'FoldArm',
'quit':'FoldArmAndQuit'})
# Object detection sub state machine; iterates over object_detection action state and recovery
# mechanism until an object is detected, it's preempted or there's an error (aborted outcome)
od_sm = smach.StateMachine(outcomes = ['succeeded','preempted','aborted'],
input_keys = ['od_attempt', 'frame', 'obj_names',
'named_pose_target_type', 'arm_folded_named_pose'],
output_keys = ['obj_names'])
with od_sm:
smach.StateMachine.add('ObjDetectionClearOctomap',
smach_ros.ServiceState('clear_octomap',
std_srv.Empty),
transitions={'succeeded':'ObjectDetectionOnce',
'preempted':'preempted',
'aborted':'ObjectDetectionOnce'})
smach.StateMachine.add('ObjectDetectionOnce',
smach_ros.SimpleActionState('object_detection',
ObjectDetectionAction,
goal_slots=['frame'],
result_slots=['obj_names']),
remapping={'frame':'frame',
'obj_names':'obj_names'},
transitions={'succeeded':'ObjDetectedCondition',
'preempted':'preempted',
'aborted':'aborted'})
sm = smach.StateMachine(outcomes=[
'FINISHED', 'ERROR', 'CONDITION_PREEMPTED', 'ACTION_PREEMPTED',
'ACTION_ERROR'
])
with sm:
smach.StateMachine.add(
'CONDITION',
smach_ros.MonitorState('/ros_cle_simulation/status', String,
time_30secs_cb), {
'valid': 'CONDITION',
'invalid': 'ACTION',
'preempted': 'CONDITION_PREEMPTED'
})
smach.StateMachine.add(
'ACTION',
smach_ros.ServiceState('/gazebo/set_visual_properties',
SetVisualProperties,
request=SetVisualPropertiesRequest(
'right_vr_screen', 'body',
'screen_glass', 'material:script:name',
'Gazebo/Red')), {
'succeeded': 'FINISHED',
'aborted': 'ACTION_ERROR',
'preempted': 'ACTION_PREEMPTED'
})
result = sm.execute()
rospy.loginfo(result)
remapping={'new_pose': 'place_pose'},
transitions={'succeeded': 'INC_PLACE_HEIGHT'})
smach.StateMachine.add('INC_PLACE_HEIGHT',
IncreasePlaceHeight(),
transitions={'succeeded': 'continue'})
smach.Iterator.set_contained_state('',
sc_sm,
loop_outcomes=['continue'])
smach.StateMachine.add('STACK_CUBES', sc_it, {
'succeeded': 'FOLD_ARM_AND_RELAX',
'aborted': 'error'
})
smach.StateMachine.add('CLEAR_GRIPPER',
smach_ros.ServiceState('clear_gripper',
std_srvs.Empty),
transitions={
'succeeded': 'OBJECT_DETECTION',
'preempted': 'preempted',
'aborted': 'aborted'
})
smach.StateMachine.add('FOLD_ARM',
FoldArm(),
transitions={
'succeeded': 'OBJECT_DETECTION',
'preempted': 'preempted',
'aborted': 'OBJECT_DETECTION'
})
smach.StateMachine.add('FOLD_ARM_AND_RELAX',
FoldArm(),
transitions={
def main():
rospy.init_node('ma_smach')
sm_root = smach.StateMachine(['succeeded', 'aborted', 'preempted'])
with sm_root:
smach.StateMachine.add('RESET',
smach_ros.ServiceState('reset', Empty),
transitions={'succeeded': 'SPAWN'})
smach.StateMachine.add('SPAWN',
smach_ros.ServiceState('spawn',
Spawn,
request=SpawnRequest(
1.0, 1.0, 0.0,
'mark')),
transitions={
'succeeded': 'TURTLE1',
'aborted': 'aborted',
'preempted': 'preempted'
})
#key point turtlesim xytheta is float type
smach.StateMachine.add('TURTLE1',
smach_ros.ServiceState(
'turtle1/teleport_absolute',
TeleportAbsolute,
request=TeleportAbsoluteRequest(
5.0, 1.0, 0.0)),
transitions={
'succeeded': 'CON',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Create the sub SMACH state machine
sm_con = smach.Concurrence(outcomes=['suc', 'abo', 'pre'],
default_outcome='abo',
outcome_map={
'suc': {
'TURTLE1_SHAPE': 'succeeded',
'DRAW_S': 'succeed'
},
'pre': {
'DRAW_S': 'preempt',
'TURTLE1_SHAPE': 'preempted'
}
})
smach.StateMachine.add('CON',
sm_con,
transitions={
'abo': 'CON',
'suc': 'succeeded',
'pre': 'preempted'
})
with sm_con:
smach.Concurrence.add(
'TURTLE1_SHAPE',
smach_ros.SimpleActionState('turtle_shape',
ShapeAction,
goal=ShapeGoal(11, 4.0)))
sm_draw = smach.StateMachine(['succeed', 'abort', 'preempt'])
with sm_draw:
smach.StateMachine.add('MONITOR_CLEAR',
smach_ros.MonitorState(
'mark/pose', Pose, clear_cb),
transitions={
'valid': 'MONITOR_CLEAR',
'invalid': 'TELE',
'preempted': 'preempt'
})
smach.StateMachine.add('TELE',
smach_ros.ServiceState(
'mark/teleport_absolute',
TeleportAbsolute,
request=TeleportAbsoluteRequest(
5.0, 5.0, 0.0)),
transitions={
'succeeded': 'MARK_SHAPE',
'aborted': 'abort',
'preempted': 'preempt'
})
ma_con = smach.Concurrence(
outcomes=[
'succeeded', 'aborted', 'preempted', 'interrupted'
],
default_outcome='aborted',
outcome_map={
'succeeded': {
'MONITOR': 'valid',
'DRAW': 'succeeded'
},
'preempted': {
'MONITOR': 'invalid',
'DRAW': 'preempted'
},
'interrupted': {
'MONITOR': 'invalid'
}
},
child_termination_cb=lambda state_outcomes: True)
smach.StateMachine.add('MARK_SHAPE',
ma_con,
transitions={
'interrupted': 'MONITOR_CLEAR',
'aborted': 'abort',
'preempted': 'preempt',
'succeeded': 'succeed'
})
with ma_con:
smach.Concurrence.add(
'DRAW',
smach_ros.SimpleActionState('mark_shape',
ShapeAction,
goal=ShapeGoal(6, 3.0)))
smach.Concurrence.add(
'MONITOR',
smach_ros.MonitorState("mark/pose", Pose, cond_cb))
smach.Concurrence.add('DRAW_S', sm_draw)
sis = smach_ros.IntrospectionServer('ma_smach', sm_root, '/SM_ROOT')
sis.start()
outcome = sm_root.execute()
rospy.spin()
sis.stop()
def define_for(self, robot):
with self:
def lock_piles_area_request_callback(userdata, request):
request = SetBoolRequest()
request.data = True
return request
def lock_piles_area_response_callback(userdata, response):
return 'succeeded' if response.success else 'aborted'
smach.StateMachine.add(
'LOCK_PILES_AREA',
smach_ros.ServiceState(
'lock_pick_region',
SetBool,
request_cb=lock_piles_area_request_callback,
response_cb=lock_piles_area_response_callback),
transitions={
'succeeded': 'GO_TO_PILES',
'aborted': 'SLEEP_AND_RETRY_LOCK_PILES_AREA'
})
smach.StateMachine.add(
'SLEEP_AND_RETRY_LOCK_PILES_AREA',
SleepAndRetry(3.0),
transitions={'succeeded': 'LOCK_PILES_AREA'})
smach.StateMachine.add(
'GO_TO_PILES',
GoTo().define_for(robot),
remapping={'waypoint': 'above_pile_pose'},
transitions={'succeeded': 'FREE_WAITING_SPOT'})
def free_waiting_spot_request_callback(userdata, request):
request = SetBoolRequest()
request.data = False
return request
def free_waiting_spot_response_callback(userdata, response):
return 'succeeded'
smach.StateMachine.add(
'FREE_WAITING_SPOT',
smach_ros.ServiceState(
'lock_wait_region',
SetBool,
request_cb=free_waiting_spot_request_callback,
response_cb=free_waiting_spot_response_callback),
transitions={'succeeded': 'PICK_ACTION'})
def pick_action_goal_callback(userdata, default_goal):
goal = mbzirc_comm_objs.msg.PickGoal(color=userdata.color)
return goal
smach.StateMachine.add('PICK_ACTION',
smach_ros.SimpleActionState(
robot.url + 'pick_action',
mbzirc_comm_objs.msg.PickAction,
input_keys=['color'],
goal_cb=pick_action_goal_callback),
transitions={
'succeeded': 'succeeded',
'aborted': 'BACK_TO_WAITING_AREA'
})
smach.StateMachine.add('BACK_TO_WAITING_AREA',
GoTo().define_for(robot),
remapping={'waypoint': 'waiting_pose'},
transitions={'succeeded': 'FREE_PILES'})
def free_piles_request_callback(userdata, request):
request = SetBoolRequest()
request.data = False
return request
def free_piles_response_callback(userdata, response):
return 'succeeded'
smach.StateMachine.add(
'FREE_PILES',
smach_ros.ServiceState(
'lock_pick_region',
SetBool,
request_cb=free_piles_request_callback,
response_cb=free_piles_response_callback),
transitions={'succeeded': 'aborted'})
return self
def define_for(self, robot):
with self:
def lock_wall_area_request_callback(userdata, request):
request = SetBoolRequest()
request.data = True
return request
def lock_wall_area_response_callback(userdata, response):
return 'succeeded' if response.success else 'aborted'
smach.StateMachine.add(
'LOCK_WALL_AREA',
smach_ros.ServiceState(
'lock_place_region',
SetBool,
request_cb=lock_wall_area_request_callback,
response_cb=lock_wall_area_response_callback),
transitions={
'succeeded': 'GO_SEGMENT_TO_THE_LEFT',
'aborted': 'SLEEP_AND_RETRY_LOCK_WALL_AREA'
})
smach.StateMachine.add('SLEEP_AND_RETRY_LOCK_WALL_AREA',
SleepAndRetry(1.0),
transitions={'succeeded': 'LOCK_WALL_AREA'})
smach.StateMachine.add(
'GO_SEGMENT_TO_THE_LEFT',
GoTo().define_for(robot),
remapping={'waypoint': 'segment_to_the_left_pose'},
transitions={'succeeded': 'FREE_PILES'})
def free_piles_request_callback(userdata, request):
request = SetBoolRequest()
request.data = False
return request
def free_piles_response_callback(userdata, response):
return 'succeeded'
smach.StateMachine.add(
'FREE_PILES',
smach_ros.ServiceState(
'lock_pick_region',
SetBool,
request_cb=free_piles_request_callback,
response_cb=free_piles_response_callback),
transitions={'succeeded': 'PLACE_ACTION'})
def place_action_goal_callback(userdata, default_goal):
goal = mbzirc_comm_objs.msg.PlaceGoal(
wall_y_offset=userdata.segment_offset)
return goal
smach.StateMachine.add('PLACE_ACTION',
smach_ros.SimpleActionState(
robot.url + 'place_action',
mbzirc_comm_objs.msg.PlaceAction,
input_keys=['segment_offset'],
goal_cb=place_action_goal_callback),
transitions={
'succeeded':
'BACK_TO_WAITING_AREA_SUCCEEDED',
'aborted': 'BACK_TO_WAITING_AREA_FAILED'
})
smach.StateMachine.add(
'BACK_TO_WAITING_AREA_SUCCEEDED',
GoTo().define_for(robot),
remapping={'waypoint': 'waiting_pose'},
transitions={'succeeded': 'FREE_WALL_SUCCEEDED'})
def free_wall_request_callback(userdata, request):
request = SetBoolRequest()
request.data = False
return request
def free_wall_response_callback(userdata, response):
return 'succeeded'
smach.StateMachine.add(
'FREE_WALL_SUCCEEDED',
smach_ros.ServiceState(
'lock_place_region',
SetBool,
request_cb=free_wall_request_callback,
response_cb=free_wall_response_callback),
transitions={'succeeded': 'succeeded'})
smach.StateMachine.add(
'BACK_TO_WAITING_AREA_FAILED',
GoTo().define_for(robot),
remapping={'waypoint': 'waiting_pose'},
transitions={'succeeded': 'FREE_WALL_FAILED'})
smach.StateMachine.add(
'FREE_WALL_FAILED',
smach_ros.ServiceState(
'lock_place_region',
SetBool,
request_cb=free_wall_request_callback,
response_cb=free_wall_response_callback),
transitions={'succeeded': 'aborted'})
return self
