def test_group(self):
"""Test adding a bunch of states with group args."""
class DoneState(State):
def __init__(self):
State.__init__(self, outcomes=['done'])
def execute(self, ud=None):
return 'done'
sm = StateMachine(['succeeded', 'done'])
with sm:
StateMachine.add('FAILSAUCE', DoneState())
transitions = {'aborted': 'FAILSAUCE', 'preempted': 'FAILSAUCE'}
with sm:
StateMachine.add(
'FIRST',
SimpleActionState('reference_action', TestAction, goal=g1),
transitions)
StateMachine.add(
'SECOND',
SimpleActionState('reference_action', TestAction, goal=g2),
transitions)
StateMachine.add(
'THIRD',
SimpleActionState('reference_action', TestAction, goal=g1),
transitions)
outcome = sm.execute()
assert outcome == 'done'
def TargetSelectorContainer(target_type):
sm_target_selector = StateMachine(['target_sent', 'aborted', 'preempted'])
with sm_target_selector:
target_selection_goal = targetSelectionGoal(target_type)
StateMachine.add('GET_TARGET',
SimpleActionState('/select_target',
SelectTargetAction,
goal=target_selection_goal,
result_key='target_pose'),
transitions={
'succeeded': 'MOVE_BASE',
'aborted': 'aborted',
'preempted': 'preempted'
},
remapping={'target_pose': 'next_target'})
StateMachine.add('MOVE_BASE',
SimpleActionState('/move_base',
MoveBaseAction,
goal_key='next_target'),
transitions={
'succeeded': 'target_sent',
'aborted': 'GET_TARGET',
'preempted': 'preempted'
})
return sm_target_selector
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 IdentificationSimpleContainer():
sm_identification_simple = StateMachine(['parked', 'aborted', 'preempted'])
with sm_identification_simple:
StateMachine.add('GET_VICTIMS',
SimpleActionState('/navigation/initial_turn',
InitialTurnAction),
transitions={'succeeded': 'GO_TO_VICTIM'})
StateMachine.add('GO_TO_VICTIM',
utils.TargetSelectorContainer('victim'),
transitions={
'target_sent': 'PARK',
'aborted': 'aborted',
'preempted': 'preempted'
})
StateMachine.add('PARK',
SimpleActionState('/navigation/initial_turn',
InitialTurnAction),
transitions={
'succeeded': 'parked',
'aborted': 'aborted',
'preempted': 'preempted'
})
return sm_identification_simple
def main():
rospy.init_node('tic_crane_state_machine')
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
with sm:
deploy_goal = msg.MoveToPositionGoal(position=500)
store_goal = msg.MoveToPositionGoal(position=0)
measure_goal = msg.MeasurementGoal(measurement_duration=7)
sm.add('DEPLOY',
SimpleActionState('move_to_target_pose',
msg.MoveToPositionAction,
goal=deploy_goal),
transitions={'succeeded': 'MEASURE'})
sm.add('MEASURE',
SimpleActionState('measurement',
msg.MeasurementAction,
goal=measure_goal),
transitions={'succeeded': 'STORE'})
sm.add('STORE',
SimpleActionState('move_to_target_pose',
msg.MoveToPositionAction,
goal=store_goal),
transitions={'succeeded': 'succeeded'})
outcome = sm.execute()
def main():
rospy.init_node("fsm_dummy")
sm = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
with sm:
StateMachine.add('INITIAL_TURN',
SimpleActionState('/initial_turn', InitialTurnAction),
transitions={
'succeeded': 'EXPLORATION',
'aborted': 'aborted',
'preempted': 'preempted'
})
StateMachine.add('EXPLORATION',
exploration.ExplorationContainer(),
transitions={
'victim_thermal': 'IDENTIFICATION_SIMPLE',
'victim_camera': 'IDENTIFICATION_TRACKING',
'aborted': 'aborted',
'preempted': 'preempted',
'time_out': 'AGGRESSIVE_EXPLORATION'
})
StateMachine.add('IDENTIFICATION_SIMPLE',
identification.IdentificationSimpleContainer(),
transitions={
'parked': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
StateMachine.add('IDENTIFICATION_TRACKING',
identification.IdentificationTrackingContainer(),
transitions={
'identification_finished': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
StateMachine.add('AGGRESSIVE_EXPLORATION',
SimpleActionState('/navigation/initial_turn',
InitialTurnAction),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
sis = smach_ros.IntrospectionServer('smach_server', sm, '/DUMMY_FSM')
sis.start()
smach_ros.set_preempt_handler(sm)
# Execute SMACH tree in a separate thread so that we can ctrl-c the script
smach_thread = threading.Thread(target=sm.execute)
smach_thread.start()
rospy.spin()
sis.stop()
def main():
rospy.init_node('smach_usecase_step_04')
# Construct static goals
polygon_big = turtle_actionlib.msg.ShapeGoal(edges = 11, radius = 4.0)
polygon_small = turtle_actionlib.msg.ShapeGoal(edges = 6, radius = 0.5)
# 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')),
{'succeeded':'TELEPORT1'})
# Teleport turtle 1
StateMachine.add('TELEPORT1',
ServiceState('turtle1/teleport_absolute', turtlesim.srv.TeleportAbsolute,
request = turtlesim.srv.TeleportAbsoluteRequest(5.0,1.0,0.0)),
{'succeeded':'TELEPORT2'})
# Teleport turtle 2
StateMachine.add('TELEPORT2',
ServiceState('turtle2/teleport_absolute', turtlesim.srv.TeleportAbsolute,
request = turtlesim.srv.TeleportAbsoluteRequest(9.0,5.0,0.0)),
{'succeeded':'BIG'})
# Draw a large polygon with the first turtle
StateMachine.add('BIG',
SimpleActionState('turtle_shape1',turtle_actionlib.msg.ShapeAction,
goal = polygon_big),
{'succeeded':'SMALL'})
# Draw a small polygon with the second turtle
StateMachine.add('SMALL',
SimpleActionState('turtle_shape2',turtle_actionlib.msg.ShapeAction,
goal = polygon_small))
# Attach a SMACH introspection server
sis = IntrospectionServer('smach_usecase_01', sm0, '/USE_CASE')
sis.start()
# Set preempt handler
smach_ros.set_preempt_handler(sm0)
# Execute SMACH tree in a separate thread so that we can ctrl-c the script
smach_thread = threading.Thread(target = sm0.execute)
smach_thread.start()
# Signal handler
rospy.spin()
def main():
rospy.init_node('smach_usecase_executive')
sm_root = smach.StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with sm_root:
smach.StateMachine.add('RESET',
ServiceState('reset', std_srvs.srv.Empty),
{'succeeded': 'SPAWN'})
request = turtlesim.srv.SpawnRequest(0.0, 0.0, 0.0, 'turtle2')
smach.StateMachine.add(
'SPAWN', ServiceState('spawn', turtlesim.srv.Spawn, request),
{'succeeded': 'TELEPORT1'})
teleport1 = turtlesim.srv.TeleportAbsoluteRequest(5.0, 1.0, 0.0)
smach.StateMachine.add(
'TELEPORT1',
ServiceState('turtle1/teleport_absolute',
turtlesim.srv.TeleportAbsolute, teleport1),
{'succeeded': 'TELEPORT2'})
teleport2 = turtlesim.srv.TeleportAbsoluteRequest(9.0, 5.0, 0.0)
smach.StateMachine.add(
'TELEPORT2',
ServiceState('turtle2/teleport_absolute',
turtlesim.srv.TeleportAbsolute, teleport2),
{'succeeded': 'SHAPES'})
shapes_concurrence = Concurrence(
outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='aborted',
outcome_map={
'succeeded': {
'BIG': 'succeeded',
'SMALL': 'succeeded'
}
})
smach.StateMachine.add('SHAPES', shapes_concurrence)
with shapes_concurrence:
Concurrence.add(
'BIG',
SimpleActionState('turtle_shape1', ShapeAction,
ShapeGoal(11, 4.0)), {'succeeded': 'SMALL'})
Concurrence.add(
'SMALL',
SimpleActionState('turtle_shape2', ShapeAction,
ShapeGoal(6, 0.5)))
sis = smach_ros.IntrospectionServer('intro_server', sm_root, '/Intro')
sis.start()
outcome = sm_root.execute()
rospy.spin()
sis.stop()
def __init__(self, helper_obj):
StateMachine.__init__(self,
outcomes=['complete', 'preempted', 'aborted'],
input_keys=['mission_data'],
output_keys=['mission_status'])
self.__helper_obj = helper_obj
with self:
# This state will prepare for the mission
StateMachine.add('PREPARE_MISSION',
PrepareMission4(self.__helper_obj),
transitions={
'ready': 'DEFAULT_HEAD_POSITION',
'error': 'complete'
})
# Set up action goal for deafult head position
default_position_pan, default_position_tilt = self.__helper_obj.CameraDefaultPos(
)
head_goal = point_headGoal()
head_goal.absolute = True
head_goal.pan = default_position_pan
head_goal.tilt = default_position_tilt
# Add the default camera position state. Which moves the head to the default position
StateMachine.add('DEFAULT_HEAD_POSITION',
SimpleActionState(
'head_control_node',
point_headAction,
goal=head_goal,
result_cb=self.defaultHeadPositionComplete,
output_keys=['mission_status']),
transitions={
'succeeded': 'MOVE',
'preempted': 'preempted',
'aborted': 'aborted'
})
# This state uses an Action to move the robot to the required goal
StateMachine.add('MOVE',
SimpleActionState('move_base',
MoveBaseAction,
goal_slots=['target_pose'],
result_cb=self.moveComplete,
output_keys=['mission_status']),
transitions={
'succeeded': 'complete',
'preempted': 'preempted',
'aborted': 'aborted'
},
remapping={'target_pose': 'destination'})
def create_action_state(step):
"""Take key value pair of the state of the state and create a SimpleActionState
based on the parameters in the config file"""
action_type = step.keys()[0]
name = step[action_type]
if action_type == "pose":
action_state = SimpleActionState('coordinate_action', CoordinateAction, goal=create_coordinate_goal(name))
elif action_type == "move":
action_state = SimpleActionState('move_base', MoveBaseAction, goal=create_move_goal(name))
else:
raise Exception("action_type pose and move are the only ones implemented")
rospy.logdebug("State with name")
return name.upper(), action_state
def __init__(self, pose_to_place=''):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['pose_to_place'],
output_keys=[])
with self:
smach.StateMachine.add("Prepare_data",
Prepare_data(pose_to_place),
transitions={
'succeeded': 'Place_Object_Goal',
'aborted': 'aborted'
})
smach.StateMachine.add('Place_Object_Goal',
Prepare_Place_Goal(),
transitions={'succeeded': 'Place_Object'})
smach.StateMachine.add('Place_Object',
SimpleActionState(
OBJECT_MANIPULATION_TOPIC,
ObjectManipulationAction,
goal_key='object_manipulation_goal',
input_keys=['standard_error'],
output_keys=['standard_error']),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
def __init__(self):
StateMachine.__init__(self,
outcomes=['succeeded', 'aborted', 'preempted'])
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
pose = Pose()
pose.position = Point(8.2, -3.0, 0.0)
pose.orientation = Quaternion(0.0, 0.0, 1.0, 0.0)
nav_goal.target_pose.pose = pose
self.nav_docking_station = SimpleActionState('move_base',
MoveBaseAction,
goal=nav_goal)
with self:
StateMachine.add('NAVIGATE_TO_OUTLET',
self.nav_docking_station,
transitions={
'succeeded': 'DOCKING',
'aborted': 'NAVIGATE_TO_OUTLET'
})
StateMachine.add('DOCKING',
ServiceState(
'battery_simulator/set_battery_level',
SetBatteryLevel, 100),
transitions={'succeeded': 'succeeded'})
def los_move(x, y, z=-0.5):
goal = MoveGoal()
goal.controller_name = 'LOS'
goal.target_pose.position = Point(x, y, z)
return SimpleActionState('move', MoveAction, goal=goal)
def __init__(self, object_to_detect_name=None):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['object_name'],
output_keys=['object_pose'])
with self:
smach.StateMachine.add('Prepare_data',
Prepare_data(object_to_detect_name),
transitions={'succeeded': 'Prepare_goal'})
smach.StateMachine.add('Prepare_goal',
prepare_object_detection_goal(),
transitions={'succeeded': 'Object_detect'})
def object_detect_result_cb(self, status, object_detect_result):
print('Object Detected: ' + str(object_detect_result))
self.object_pose = object_detect_result
return 'succeeded'
smach.StateMachine.add(
'Object_detect',
SimpleActionState(
objectDetect_topic,
RecognizeAction,
result_cb=object_detect_result_cb,
goal_key='object_detection_goal',
output_keys=['object_pose', 'standard_error']),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
def los_move(x, y, z=-0.5):
goal = MoveGoal()
goal.guidance_type = 'LOS'
goal.target_pose.position = Point(x, y, z)
return SimpleActionState(move_action_server, MoveAction, goal=goal)
def main():
rospy.init_node('smach_example_state_machine')
# Create a SMACH state machine
sm = smach.StateMachine(['succeeded','aborted','preempted'])
# Open the container
with sm:
# Add states to the container
smach.StateMachine.add('WAIT', Wait(),
transitions={'service':'SERVICE',
'action':'ACTION'})
smach.StateMachine.add('SERVICE',
ServiceState('service_test',
bool_bool),
transitions={'succeeded':'WAIT',
'aborted':'WAIT',
'preempted':'WAIT'})
smach.StateMachine.add('ACTION',
SimpleActionState('find_buoy',
VisionExampleAction),
transitions={'succeeded':'WAIT',
'aborted':'WAIT',
'preempted':'WAIT'})
# Execute SMACH plan
outcome = sm.execute()
def sm_best_vantage():
sm = smach.StateMachine(outcomes=['succeeded', 'aborted'],
input_keys=['rfid_reads', 'tagid'])
with sm:
smach.StateMachine.add(
'SELECT_BEST_VANTAGE',
BestVantage(),
remapping={
'tagid': 'tagid', # input (string)
'rfid_reads': 'rfid_reads', # input (RecorderReads)
'best_vantage': 'best_vantage', # output (PoseStamped)
'filtered_reads': 'filtered_reads'
}, # output (RecorderReads)
transitions={'succeeded': 'MOVE_BEST_VANTAGE'})
smach.StateMachine.add(
'MOVE_BEST_VANTAGE',
SimpleActionState('/move_base',
MoveBaseAction,
goal_slots=['target_pose']),
remapping={'target_pose': 'best_vantage'}, # input (PoseStamped)
transitions={
'aborted': 'SELECT_BEST_VANTAGE',
'preempted': 'aborted',
'succeeded': 'succeeded'
})
return sm
def main():
rospy.init_node('smach_example_state_machine')
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['outcome4', 'outcome5'])
# Open the container
with sm:
# Add states to the container
smach.StateMachine.add('FOO',
Foo(),
transitions={
'outcome1': 'BAR',
'outcome2': 'outcome4'
})
smach.StateMachine.add('BAR', Bar(), transitions={'outcome2': 'FOO'})
def gripper_result_cb(userdata, status, result):
return 'my_outcome'
smach.StateMachine.add('TRIGGER_GRIPPER',
SimpleActionState(
'action_server_namespace',
GetMap,
result_cb=gripper_result_cb,
output_keys=['gripper_output']),
transitions={'succeeded': 'APPROACH_PLUG'},
remapping={'gripper_output': 'userdata_output'})
# Execute SMACH plan
outcome = sm.execute()
def get_nav_approach(self):
def child_term_cb(outcome_map):
return True
def out_cb(outcome_map):
if outcome_map['DETECT_FORWARD_DISTANCE'] == 'reached':
return 'succeeded'
return 'shutdown'
sm_nav_approach_state = smach.Concurrence(
outcomes=['succeeded', 'shutdown'],
default_outcome='shutdown',
child_termination_cb=child_term_cb,
outcome_cb=out_cb,
output_keys=['nav_dist'])
with sm_nav_approach_state:
approach_goal = ApproachGoal()
approach_goal.forward_vel = 0.05
approach_goal.forward_mult = 0.50
smach.Concurrence.add(
'MOVE_FORWARD',
SimpleActionState('/approach_table/move_forward_act',
ApproachAction,
goal=approach_goal))
smach.Concurrence.add(
'DETECT_FORWARD_DISTANCE',
DetectForwardDistance(self.get_transform,
self.nav_approach_dist))
return sm_nav_approach_state
def __init__(self):
StateMachine.__init__(
self,
outcomes=['standby', 'exit', 'aborted', 'preempted'],
input_keys=['control_infos'])
self.register_termination_cb(self.set_timer)
with self:
StateMachine.add('CONTROL_MANAGER',
ControlManager(),
transitions={
'recharge': 'RECHARGE',
'assisted_teleop': 'ASSISTED_TELEOP',
'semi_autonomous': 'SEMI_AUTONOMOUS',
'standby': 'standby',
'exit': 'exit'
})
StateMachine.add('RECHARGE',
Recharge(),
transitions={'succeeded': 'standby'})
StateMachine.add('ASSISTED_TELEOP', AssistedTeleop())
StateMachine.add('SEMI_AUTONOMOUS',
SimpleActionState('move_base',
MoveBaseAction,
input_keys=['control_infos'],
goal_cb=self.move_base_goal_cb),
transitions={'succeeded': 'standby'})
def main():
rospy.init_node('move_gripper_client')
parser = argparse.ArgumentParser(
description='Client for Korus` move gripper action server')
parser.add_argument('gripper_angle', type=float, help='gripper angle')
args = parser.parse_args()
sm = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
sm.userdata.gripper_angle = args.gripper_angle
with sm:
def MoveGripperResultCb(userdata, status, result):
rospy.loginfo('action finished in state ' + str(status))
rospy.loginfo('action result: ')
rospy.loginfo(str(result))
smach.StateMachine.add(
'MoveGripper',
SimpleActionState('/korus/move_gripper',
MoveGripperAction,
goal_slots=['gripper_angle'],
result_cb=MoveGripperResultCb))
sm.execute()
def los_state(
goal_positon,
travel_speed=0.5,
sphere_of_acceptance=0.5,
action_server="/guidance_interface/los_server",
):
"""Creates a SimpleActionState for traveling to a goal position using LOS guidance.
Args:
goal_positon (geometry_msgs/Point): position drone should travel to.
start_position (geometry_msgs/Point): position drone starts in.
travel_speed (float, optional):
forward velocity drone shoudl travel at. Defaults to 0.5.
sphere_of_acceptance (float, optional):
action returns success when drone is inside this sphere. Defaults to 0.5.
action_server (str, optional):
name of los action server. Defaults to "/guidance_interface/los_server".
Returns:
SimpleActionState: state that travel to from start to goal using LOS guidance
"""
goal = LosPathFollowingGoal()
goal.next_waypoint = goal_positon
goal.forward_speed = travel_speed
goal.desired_depth = goal_positon.z
goal.sphereOfAcceptance = sphere_of_acceptance
return SimpleActionState(action_server, LosPathFollowingAction, goal)
def PatrolStateMachine(listOfWaypoints):
tempWaypoint = ()
waypoints = []
#convert waypoints to ros topic format
#waypoints.append([str(0), (listOfWaypoints[-1][1],listOfWaypoints[-1][0]), (0.0, 0.0, 0.0, 1.0)])
for i in range(0, len(listOfWaypoints), 1):
tempWaypoint = (listOfWaypoints[i][1], listOfWaypoints[i][0])
waypoints.append([str(i), tempWaypoint, (0.0, 0.0, 0.0, 1.0)])
#start state machine
patrol = StateMachine(['succeeded', 'aborted', 'preempted'])
with patrol:
for i, w in enumerate(waypoints):
goal_pose = MoveBaseGoal()
goal_pose.target_pose.header.frame_id = 'map'
goal_pose.target_pose.pose.position.x = w[1][0]
goal_pose.target_pose.pose.position.y = w[1][1]
goal_pose.target_pose.pose.position.z = 0.0
goal_pose.target_pose.pose.orientation.x = w[2][0]
goal_pose.target_pose.pose.orientation.y = w[2][1]
goal_pose.target_pose.pose.orientation.z = w[2][2]
goal_pose.target_pose.pose.orientation.w = w[2][3]
StateMachine.add(w[0],
SimpleActionState('move_base',
MoveBaseAction,
goal=goal_pose),
transitions={'succeeded': waypoints[(i + 1) % \
len(waypoints)][0]})
patrol.execute()
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 get_sm(self):
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
smach.StateMachine.add('NAV_APPROACH',
self.sm_nav_approach.get_sm(),
transitions={
'succeeded': 'TORSO_SETUP',
'shutdown': 'aborted'
})
# move torso up
tgoal = SingleJointPositionGoal()
tgoal.position = 0.300 # all the way up is 0.300
tgoal.min_duration = rospy.Duration(2.0)
tgoal.max_velocity = 1.0
smach.StateMachine.add(
'TORSO_SETUP',
SimpleActionState('torso_controller/position_joint_action',
SingleJointPositionAction,
goal=tgoal),
transitions={'succeeded': 'R_UNTUCK'})
# Untucks the right arm
smach.StateMachine.add(
'R_UNTUCK',
ServiceState('traj_playback/untuck_r_arm',
TrajPlaybackSrv,
request=TrajPlaybackSrvRequest(
False)), # if True, reverse trajectory
transitions={'succeeded': 'R_ADJUST_MIRROR'})
# Adjusts the mirror
smach.StateMachine.add(
'R_ADJUST_MIRROR',
ServiceState('traj_playback/r_adjust_mirror',
TrajPlaybackSrv,
request=TrajPlaybackSrvRequest(
False)), # if True, reverse trajectory
transitions={'succeeded': 'L_UNTUCK'})
# Untucks the left arm
smach.StateMachine.add(
'L_UNTUCK',
ServiceState('traj_playback/untuck_l_arm',
TrajPlaybackSrv,
request=TrajPlaybackSrvRequest(
False)), # if True, reverse trajectory
transitions={'succeeded': 'HEAD_REG_ALL'})
smach.StateMachine.add(
'HEAD_REG_ALL',
self.sm_ell_reg.get_sm(),
transitions={'succeeded': 'SETUP_TASK_CONTROLLER'})
smach.StateMachine.add('SETUP_TASK_CONTROLLER',
SetupTaskController())
return sm
def __init__(self, motion = None, time = None):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['manip_motion_to_play'],
output_keys=['standard_error'])
rospy.loginfo('Play Motion StateMachine')
with self:
#Prepare Play Motion Goal
smach.StateMachine.add('PrepareData',
prepareData(motion, time),
transitions={'succeeded':'Prepare_play_motion_goal', 'aborted':'aborted'})
smach.StateMachine.add('Prepare_play_motion_goal',
createPlayMotionGoal(),
transitions={'succeeded':'Play_Motion_Send', 'aborted':'aborted', 'preempted':'preempted'})
def play_motion_cb_result(userdata, result_status, result):
if result.error_code != 1: # 1 == SUCCEEDED
print "in not succeeded"
if result.error_code == -1:
userdata.standard_error = "Play Motion not available: Motion Not Found"
rospy.loginfo(userdata.standard_error)
elif result.error_code == -2:
userdata.standard_error = "Play Motion not available: Infeasible Reach Time"
rospy.loginfo(userdata.standard_error)
elif result.error_code == -3:
userdata.standard_error = "Play Motion not available: Controller Busy"
rospy.loginfo(userdata.standard_error)
elif result.error_code == -4:
userdata.standard_error = "Play Motion not available: Missing Controller"
rospy.loginfo(userdata.standard_error)
elif result.error_code == -5:
userdata.standard_error = "Play Motion not available: Trajectory Error"
rospy.loginfo(userdata.standard_error)
elif result.error_code == -6:
userdata.standard_error = "Play Motion not available: Goal Not Reached"
rospy.loginfo(userdata.standard_error)
#return 'succeeded'
elif result.error_code == -7:
userdata.standard_error = "Play Motion not available: Planner Offline"
rospy.loginfo(userdata.standard_error)
elif result.error_code == -8:
userdata.standard_error = "Play Motion not available: No Plan Found"
rospy.loginfo(userdata.standard_error)
elif result.error_code == -42:
userdata.standard_error = "Play Motion not available: Other Error"
rospy.loginfo(userdata.standard_error)
return 'aborted'
else:
userdata.standard_error = "Play Motion OK"
return 'succeeded'
#Send Play motion Goal
smach.StateMachine.add('Play_Motion_Send',
SimpleActionState('/play_motion', PlayMotionAction, goal_key='play_motion_sm_goal',
input_keys=['standard_error'],
output_keys=['standard_error'],
result_cb=play_motion_cb_result),
transitions={'succeeded':'succeeded', 'preempted':'preempted', 'aborted':'aborted'})
def drive_point(x, y, distance_threshold):
goal = GotoPointGoal()
goal.point = Point(x, y, 0)
goal.distance_threshold = distance_threshold
goal.target_frame = "map"
goal.reference_frame = "base_footprint"
return SimpleActionState('goto_point', GotoPointAction, goal=goal)
def test_alt_api(self):
"""Test adding with alt apis."""
sm = StateMachine(['succeeded', 'aborted', 'preempted'])
with sm.opened():
sm.add('FIRST',
SimpleActionState('reference_action', TestAction, goal=g1),
{})
sm.add('SECOND',
SimpleActionState('reference_action', TestAction, goal=g2),
{})
sm.add('THIRD',
SimpleActionState('reference_action', TestAction, goal=g1),
{})
outcome = sm.execute()
assert outcome == 'succeeded'
def test_action_server_wrapper(self):
"""Test action server wrapper."""
sq = Sequence(['succeeded', 'aborted', 'preempted'], 'succeeded')
sq.register_input_keys(['goal', 'action_goal', 'action_result'])
sq.register_output_keys(['action_result'])
with sq:
Sequence.add(
'GOAL_KEY',
SimpleActionState("reference_action",
TestAction,
goal_key='action_goal'))
Sequence.add(
'GOAL_SLOTS',
SimpleActionState("reference_action",
TestAction,
goal_slots=['goal']))
@cb_interface(input_keys=['action_result'],
output_keys=['action_result'])
def res_cb(ud, status, res):
ud.action_result.result = res.result + 1
Sequence.add(
'RESULT_CB',
SimpleActionState("reference_action",
TestAction,
goal=g1,
result_cb=res_cb))
asw = ActionServerWrapper('reference_action_sm',
TestAction,
sq,
succeeded_outcomes=['succeeded'],
aborted_outcomes=['aborted'],
preempted_outcomes=['preempted'],
expand_goal_slots=True)
asw.run_server()
ac = SimpleActionClient('reference_action_sm', TestAction)
ac.wait_for_server(rospy.Duration(30))
assert ac.send_goal_and_wait(
g1, rospy.Duration(30)) == GoalStatus.SUCCEEDED
assert ac.get_result().result == 1
def addDepthAction(self, sm):
sm.add('DEPTH', \
SimpleActionState('depthServer', \
depthAction, \
goal_cb=self.depthCallback), \
transitions={'succeeded':self.TASK,\
'preempted':'DEPTH',\
'aborted':'aborted'})
'''
