def main():
rospy.init_node('smach_usecase_executive')
sm_root = StateMachine(outcomes=['preempted', 'aborted', 'succeeded'])
with sm_root:
StateMachine.add('RESET',
ServiceState('/reset', Empty, request=EmptyRequest()),
transitions={'succeeded': 'SPAWN'})
StateMachine.add(
'SPAWN',
ServiceState('/spawn', Empty, request=EmptyRequest()),
#request=SpawnRequest(1.0, 1.0, 2.0, 'kkk')),
transitions={'aborted': 'aborted'})
# transitions={
# 'preemtped': 'preemtped',
# 'aborted': 'aborted',
# 'succeeded': 'succeeded'})
sis = IntrospectionServer('exmaple', sm_root, '/USE_CASE')
sis.start()
outcome = sm_root.execute()
rospy.spin()
def main():
rospy.init_node('smach_usecase_step_02')
# Create a SMACH state machine
sm0 = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
# Open the container
with sm0:
# Reset turtlesim
StateMachine.add('RESET', ServiceState('reset', std_srvs.srv.Empty),
{'succeeded': 'SPAWN'})
# Create a second turtle
StateMachine.add(
'SPAWN',
ServiceState('spawn',
turtlesim.srv.Spawn,
request=turtlesim.srv.SpawnRequest(
0.0, 0.0, 0.0, 'turtle2')))
# Execute SMACH tree
outcome = sm0.execute()
# Signal ROS shutdown (kill threads in background)
rospy.signal_shutdown('All done.')
def cam_capture():
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['bagfile_name', 'bagfile_topics'])
with sm:
smach.StateMachine.add('START_BAG_CAPTURE',
ServiceState('/bag_cap/capture',
BagCapture,
request_slots=['topics', 'dest']),
remapping={
'topics': 'bagfile_topics',
'dest': 'bagfile_name'
},
transitions={'succeeded': 'DELAY'})
smach.StateMachine.add('DELAY',
DelayState(),
transitions={'succeeded': 'STOP_BAG_CAPTURE'})
smach.StateMachine.add('STOP_BAG_CAPTURE',
ServiceState('/bag_cap/capture',
BagCapture,
request=BagCaptureRequest('', '')),
transitions={'succeeded': 'succeeded'})
return sm
def main():
rospy.init_node('tms_ts_smach_executive11')
sm_root = smach.StateMachine(['succeeded', 'aborted', 'preempted'])
with sm_root:
smach.StateMachine.add('move0',
ServiceState('rp_cmd',
rp_cmd,
request=rp_cmdRequest(
9001, False, 2003, [6017])),
transitions={'succeeded': 'control0'})
smach.StateMachine.add('control0',
ServiceState('ts_state_control',
ts_state_control,
request=ts_state_controlRequest(
0, 0, 0, 0, "")),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
sis = smach_ros.IntrospectionServer('tms_ts_smach_test', sm_root,
'/ROS_TMS')
sis.start()
outcome = sm_root.execute()
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 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 sm_search():
# Create a SMACH state machine
sm_search = smach.StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['tagid', 'explore_radius'])
# Open the container
with sm_search:
# Start the RFID recorder
smach.StateMachine.add('RECORDER_START',
ServiceState('/rfid_recorder/record',
RecorderSrv),
transitions={'succeeded': 'FLAPPER_START'})
# Start the ears flapping.
smach.StateMachine.add(
'FLAPPER_START',
ServiceState('/flapper/flap', FlapperSrv, request_slots=['tagid']),
transitions={'succeeded': 'EXPLORE_ROOM'},
# transitions = {'succeeded':'SLEEPER'},
remapping={'tagid': 'tagid'})
# smach.StateMachine.add('SLEEPER', Sleeper( 5.0 ), transitions = {'succeeded':'FLAPPER_STOP'})
# EXPLORE
def explore_response_cb(userdata, response):
# result is of ExploreRoomSrvResponse
return response.result
smach.StateMachine.add(
'EXPLORE_ROOM',
ServiceState(
'/explore/explore', # Default outcomes
ExploreRoomSrv,
request_slots=['radius'],
response_cb=explore_response_cb),
remapping={'radius': 'explore_radius'},
transitions={'succeeded': 'FLAPPER_STOP'}) # input
# Stop the ears flapping.
smach.StateMachine.add('FLAPPER_STOP',
ServiceState('/flapper/flap',
FlapperSrv,
request_slots=['tagid']),
transitions={'succeeded': 'RECORDER_STOP'},
remapping={'tagid': 'tagid'})
# Start the RFID recorder
smach.StateMachine.add('RECORDER_STOP',
ServiceState('/rfid_recorder/record',
RecorderSrv),
transitions={'succeeded': 'succeeded'})
return sm_search
def sm_rfid_servo_approach():
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['tagid'])
# Open the container
with sm:
# Initial RFID Ear Flapping
smach.StateMachine.add('FLAP_EARS',
ServiceState('/rfid_orient/flap', FlapEarsSrv),
transitions={'succeeded': 'ORIENT'})
# Orient towards tag
smach.StateMachine.add(
'ORIENT',
ServiceState('/rfid_orient/orient',
OrientSrv,
request_slots=[
'data'
]), #request = OrientSrvRequest( 'person ' )
transitions={'succeeded': 'SERVO'},
remapping={'data': 'tagid'}) # input
# Servoing is a basic state machine. Success means servoing finished @ obs.
smach.StateMachine.add(
'SERVO',
SimpleActionState(
'/rfid_servo/servo_act', ServoAction,
goal_slots=['tagid']), #goal = ServoGoal( 'person ' ),
transitions={'succeeded': 'TUCK_LEFT'},
remapping={'tagid': 'tagid'}) # input
# Tuck Left (non-blocking)
smach.StateMachine.add(
'TUCK_LEFT',
ServiceState(
'robotis/servo_left_pan_moveangle',
MoveAng,
request=MoveAngRequest(
1.350, 0.2,
0)), # ang (float), angvel (float), blocking (bool)
transitions={'succeeded': 'TUCK_RIGHT'})
# Tuck Right (non-blocking)
smach.StateMachine.add(
'TUCK_RIGHT',
ServiceState(
'robotis/servo_right_pan_moveangle',
MoveAng,
request=MoveAngRequest(
-1.350, 0.2,
0)), # ang (float), angvel (float), blocking (bool)
transitions={'succeeded': 'succeeded'})
return sm
def main():
rospy.init_node('smach_dialog')
sm = smach.StateMachine(['succeeded', 'aborted', 'preempted'])
sm.userdata.tts_text = 'yaaas'
sm.userdata.stt_text = 'nnooo'
with sm:
@smach.cb_interface(output_keys=['stt_text'])
def stt_result_cb(userdata, result):
userdata.stt_text = result.text
rospy.logwarn(result.text)
return 'succeeded'
@smach.cb_interface(input_keys=['tts_text', 'stt_text'])
def tts_request_cb(userdata, request):
tts_request = TalkRequest(userdata.stt_text)
time.sleep(1)
return tts_request
smach.StateMachine.add('RAW_INPUT',
RawInput(),
transitions={'succeeded': 'PROCESSOR'})
smach.StateMachine.add('PROCESSOR',
Processor(),
transitions={'succeeded': 'RAW_INPUT'})
smach.StateMachine.add('SYNTHESIZE_SPEECH',
ServiceState(
'/roboy/cognition/speech/synthesis/talk',
Talk,
request_cb=tts_request_cb,
input_keys=['tts_text', 'stt_text']),
transitions={'succeeded': 'RECOGNIZE_SPEECH'})
smach.StateMachine.add('BAR',
Bar(),
transitions={'outcome2': 'SYNTHESIZE_SPEECH'})
smach.StateMachine.add('RECOGNIZE_SPEECH',
ServiceState(
'/roboy/cognition/speech/recognition',
RecognizeSpeech,
response_cb=stt_result_cb,
output_keys=['stt_text']),
transitions={'succeeded': 'SYNTHESIZE_SPEECH'})
outcome = sm.execute()
def head_capture():
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['bagfile_name', 'bagfile_topics'])
with sm:
def PointAdd(x, y, z, dur, state, res):
pgoal = PointHeadGoal()
pgoal.target.header.frame_id = '/torso_lift_link'
pgoal.target.point.x = x
pgoal.target.point.y = y
pgoal.target.point.z = z
pgoal.min_duration = rospy.Duration(dur)
pgoal.max_velocity = 1.0
smach.StateMachine.add(
state,
SimpleActionState('/head_traj_controller/point_head_action',
PointHeadAction,
goal=pgoal),
transitions={'succeeded': res})
return
PointAdd(0.00, -1.00, -0.60, 5.0, 'PH1', 'START_BAG_CAPTURE')
smach.StateMachine.add('START_BAG_CAPTURE',
ServiceState('/bag_cap/capture',
BagCapture,
request_slots=['topics', 'dest']),
remapping={
'topics': 'bagfile_topics',
'dest': 'bagfile_name'
},
transitions={'succeeded': 'PH2'})
PointAdd(0.00, 1.00, -0.60, 15.0, 'PH2', 'PH3')
PointAdd(0.00, 1.00, -0.20, 3.0, 'PH3', 'PH4')
PointAdd(0.00, -1.00, -0.20, 15.0, 'PH4', 'PH5')
PointAdd(0.00, -1.00, 0.30, 3.0, 'PH5', 'PH6')
PointAdd(0.00, 1.00, 0.30, 15.0, 'PH6', 'PH7')
PointAdd(1.00, 0.00, 0.00, 7.5, 'PH7', 'STOP_BAG_CAPTURE')
smach.StateMachine.add('STOP_BAG_CAPTURE',
ServiceState('/bag_cap/capture',
BagCapture,
request=BagCaptureRequest('', '')),
transitions={'succeeded': 'succeeded'})
return sm
def test_service_cb(self):
"""Test calling a service with a callback."""
srv = rospy.Service('/empty', std_srvs.Empty, empty_server)
sm = StateMachine(['succeeded', 'aborted', 'preempted', 'done'])
with sm:
def foo_response_cb(userdata, response):
userdata.foo_var_out = 'foo!'
return 'succeeded'
StateMachine.add('FOO',
ServiceState('/empty',
std_srvs.Empty,
response_cb=foo_response_cb,
output_keys=['foo_var_out']),
remapping={'foo_var_out': 'sm_var'},
transitions={'succeeded': 'done'})
outcome = sm.execute()
rospy.logwarn("OUTCOME: " + outcome)
assert outcome == 'done'
def closest_ball(grabber_frame, distance_limit):
return ServiceState('ball_map/closest',
ClosestBall,
request=ClosestBallRequest(
grabber_frame=grabber_frame,
distance_limit=distance_limit),
response_slots=["position"])
def __init__(self, learning_time=5):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['name', 'purgeAll'],
output_keys=[])
with self:
# call request for SetDataBaseRequest
@smach.cb_interface(input_keys=['name', 'purgeAll'])
def face_database_request_cb(userdata, request):
data_base_request = SetDatabaseRequest()
data_base_request.databaseName = userdata.name
data_base_request.purgeAll = userdata.purgeAll
return data_base_request
#call request to control the database
smach.StateMachine.add('drop_face',
ServiceState(
'/pal_face/set_database',
SetDatabase,
request_cb=face_database_request_cb,
input_keys=['name', 'purgeAll']),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
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 __init__(self, grammar = None):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['grammar_name'],
output_keys=[])
with self:
smach.StateMachine.add('Dummy',
DummyState(),
transitions={'succeeded':'succeeded', 'aborted':'aborted'})
smach.StateMachine.add('PrepareData',
prepareData(grammar),
transitions={'succeeded':'Deactivate_Asr', 'aborted':'aborted'})
# Deactivating asr service to stop listening
@smach.cb_interface(input_keys=['grammar_name'])
def AsrServerRequestDeactivate_cb(userdata, request):
rospy.loginfo("Deactivating Asr service")
requ = ASRSrvRequest()
requ.requests = [ASRSrvRequest.ACTIVATION] #, ASRSrvRequest.GRAMMAR, ASRSrvRequest.LANGUAGE]
requ.activation.action = ASRActivation.DEACTIVATE
#requ.model.action = ASRLangModelMngmt.ENABLE
#requ.model.modelName = userdata.grammar_name
#requ.lang.language = 'en_US'
return requ
smach.StateMachine.add('Deactivate_Asr',
ServiceState('/asr_service',
ASRService,
request_cb = AsrServerRequestDeactivate_cb,
input_keys = ['grammar_name']),
transitions={'succeeded':'succeeded', 'aborted': 'aborted', 'preempted': 'preempted'})
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 __init__(self, direction='left'):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=[])
self.direction = direction
with self:
def nav_turn(userdata, request):
turn_request = NavigationTurnRequest()
if self.direction == 'left':
self.angle = -45
else:
self.angle = 45
turn_request.degree = self.angle
turn_request.enable = True
return turn_request
smach.StateMachine.add('turn',
ServiceState('/turn',
NavigationTurn,
request_cb=nav_turn),
transitions={
'succeeded': 'turn',
'aborted': 'turn',
'preempted': 'preempted'
})
def main():
rospy.init_node('smach_touch_react')
rate = rospy.Rate(10)
sm_root = smach.StateMachine(
outcomes=['success', 'failed', 'succeeded', 'aborted', 'preempted'])
with sm_root:
smach.StateMachine.add('TOUCH_CHECKER',
TouchCheck(),
transitions={
'success': 'SAY_SPEECH',
'waiting': 'TOUCH_CHECKER'
})
smach.StateMachine.add('SAY_SPEECH',
SaySpeech(),
transitions={'success': 'SLEEP'})
smach.StateMachine.add('SLEEP',
ServiceState('/pepper_robot/pose/rest', Empty))
# Execute SMACH plan
sis = smach_ros.IntrospectionServer('SM_Pepper', sm_root, '/SM_Pepper')
sis.start()
# Execute SMACH plan
outcome = sm_root.execute()
# Wait for ctrl-c to stop the application
rospy.spin()
sis.stop()
def __init__(self, angle = 120):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted','aborted'],
input_keys=[])
self.angle= angle
self.iterations = 0
with self:
self.userdata.angle = self.angle
def iterator(userdata, response):
self.iterations = self.iterations + 1
rospy.logwarn("ITERATIONS = " + str(self.iterations))
if self.iterations > 2:
return 'preempted'
return 'succeeded'
def nav_turn_start(userdata, request):
turn_request = NavigationTurnRequest()
turn_request.degree = self.angle
turn_request.enable = True
return turn_request
smach.StateMachine.add('turn',
ServiceState('/turn',
NavigationTurn,
request_cb = nav_turn_start,
response_cb = iterator),
transitions={'succeeded':'succeeded','aborted' : 'turn','preempted':'preempted'})
def __init__(self, sleep=1):
smach.StateMachine.__init__(self,
outcomes=['succeeded', 'preempted'],
input_keys=["in_learn_person"])
self.sleep = sleep
with self:
def Cheack_Elevator_Start(userdata, request):
start_request = EnableCheckElevatorRequest()
start_request.enable = True
return start_request
smach.StateMachine.add('INIT_VAR',
init_var(),
transitions={
'succeeded': "SLEEPS_UNTIL_ELEVATOR",
'preempted': 'preempted'
})
smach.StateMachine.add('SLEEPS_UNTIL_ELEVATOR',
sleep_until(self.sleep),
transitions={
'succeeded': 'START_CHECK_ELEVATOR',
'preempted': 'preempted'
})
#call request of start enrollment
smach.StateMachine.add('START_CHECK_ELEVATOR',
ServiceState(
'/check_elevator/enable',
EnableCheckElevator,
request_cb=Cheack_Elevator_Start),
transitions={
'succeeded': 'READ_TOPIC',
'aborted': 'Print_error',
'preempted': 'preempted'
})
smach.StateMachine.add('READ_TOPIC',
read_topic(),
transitions={
'succeeded': 'CHECK_STATUS',
'aborted': 'READ_TOPIC',
'preempted': 'preempted'
})
smach.StateMachine.add('CHECK_STATUS',
check_status(),
transitions={
'IM_IN': 'succeeded',
'IM_NOT_IN': 'READ_TOPIC',
'preempted': 'preempted'
})
smach.StateMachine.add('Print_error',
print_error(),
transitions={
'succeeded': 'READ_TOPIC',
'preempted': 'preempted'
})
def __init__(self,minConfidence=90.0):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=[],
output_keys=['standard_error','objects'])
with self:
# call request for Recognizer
@smach.cb_interface(input_keys=[])
def object_start_detect_request_cb(userdata, request):
start_request = RecognizerRequest()
start_request.enabled=True
start_request.minConfidence=minConfidence
return start_request
#call request of start recognizer
smach.StateMachine.add('Start_recognizer',
ServiceState('/object_detect/recognizer',
Recognizer,
request_cb = object_start_detect_request_cb,
input_keys = []),
transitions={'succeeded':'Read_Topic','aborted' : 'aborted','preempted':'preempted'})
# Wait learning_time, that the robot will be learning the object
smach.StateMachine.add(
'Read_Topic',
read_topic_objects(),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted',
'preempted': 'preempted'})
def reset_localisation():
request = SetPositionRequest()
request.frame = "map"
request.position = Point(1.2, 0.2, 0)
quat = quaternion_from_euler(0, 0, numpy.radians(90))
request.rotation = Quaternion(*quat)
return ServiceState('set_location', SetPosition, request=request)
def main():
rospy.init_node('smach_mini_drc_missions')
sm_root = smach.StateMachine.add('RESET',
ServiceState('reset', std_srvs.srv.Empty),
{})
sis = smach_ros.IntrospectionServer('intro_server', sm_root, '/Intro')
sis.start()
rospy.spin()
sis.stop()
outcome = sm_root.execute()
def cousins_demo():
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['person_id'])
with sm:
smach.StateMachine.add(
'ROTATE_BEFORE_MOVE',
ServiceState('/rotate_backup',
RotateBackupSrv,
request=RotateBackupSrvRequest(
3.14, 0.0)), # Full 180-deg spin.
transitions={'succeeded': 'MOVE_SHOW_OFF_POSE'})
gd = MoveBaseGoal()
gd.target_pose.header.frame_id = '/map'
gd.target_pose.header.stamp = rospy.Time(0)
gd.target_pose.pose.position.x = 2.956
gd.target_pose.pose.position.y = 3.047
gd.target_pose.pose.orientation.z = 0.349
gd.target_pose.pose.orientation.w = 0.937
smach.StateMachine.add(
'MOVE_SHOW_OFF_POSE',
SimpleActionState('/move_base', MoveBaseAction, goal=gd),
# transitions = {'succeeded':'READY_RETURN_HALLWAY'})
transitions={'succeeded': 'succeeded'})
# smach.StateMachine.add(
# 'READY_RETURN_HALLWAY',
# PrintStr('Ready to return to hallway [ENTER]'),
# transitions = { 'succeeded':'MOVE_HALLWAY' })
# go = MoveBaseGoal()
# go.target_pose.header.frame_id = '/map'
# go.target_pose.header.stamp = rospy.Time(0)
# go.target_pose.pose.position.x = -5.07
# go.target_pose.pose.position.y = 8.725
# go.target_pose.pose.orientation.z = 0.926
# go.target_pose.pose.orientation.w = 0.377
# smach.StateMachine.add(
# 'MOVE_HALLWAY',
# SimpleActionState( '/move_base',
# MoveBaseAction,
# goal = go ), # Back down the hallway
# transitions = {'succeeded':'succeeded'})
return sm
def smc0():
smc0 = smach.Concurrence(outcomes=['succeeded', 'aborted'],
default_outcome='aborted',
outcome_map={
'succeeded': {
'0random_move': 'succeeded',
'1sensing': 'succeeded'
}
},
child_termination_cb=lambda arg: True)
with smc0:
smach.Concurrence.add(
'0random_move',
ServiceState('rp_cmd',
rp_cmd,
request=rp_cmdRequest(9006, True, 2005, [0])))
smach.Concurrence.add(
'1sensing',
ServiceState('rp_cmd',
rp_cmd,
request=rp_cmdRequest(9007, True, 2005, [0])))
return smc0
def __init__(self):
Iterator.__init__(self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['log_mission'],
it=[],
output_keys=['log_mission'],
it_label='waypoint_id',
exhausted_outcome='succeeded')
self.register_start_cb(self.set_iterator_list)
with self:
self.sm_nav = StateMachine(
outcomes=['succeeded', 'preempted', 'aborted', 'continue'],
input_keys=['waypoint_id', 'log_mission'],
output_keys=['log_mission'])
self.sm_nav.register_start_cb(self.start_cb_nav)
self.sm_nav.register_termination_cb(self.termination_cb_nav)
with self.sm_nav:
StateMachine.add('MOVE_BASE',
SimpleActionState(
'move_base',
MoveBaseAction,
input_keys=['waypoint_id', 'log_mission'],
goal_cb=self.move_base_goal_cb),
transitions={
'succeeded': 'SAVE_DATA',
'aborted': 'continue'
})
self.save_data_service = ServiceState(
'make_data_acquisition',
RequestSaveData,
response_cb=self.save_data_response_cb)
StateMachine.add('SAVE_DATA',
self.save_data_service,
transitions={'succeeded': 'continue'})
# Close the sm_nav machine and add it to the iterator
Iterator.set_contained_state('FOLLOW_PATH',
self.sm_nav,
loop_outcomes=['continue'])
def __init__(self):
rospy.init_node('monitor_fake_battery', anonymous=False)
rospy.on_shutdown(self.shutdown)
# Set the low battery threshold (between 0 and 100)
self.low_battery_threshold = rospy.get_param('~low_battery_threshold',
50)
# Initialize the state machine
sm_battery_monitor = StateMachine(outcomes=[])
with sm_battery_monitor:
# Add a MonitorState to subscribe to the battery level topic
StateMachine.add(
'MONITOR_BATTERY',
MonitorState('battery_level', Float32, self.battery_cb),
transitions={
'invalid': 'RECHARGE_BATTERY',
'valid': 'MONITOR_BATTERY',
'preempted': 'MONITOR_BATTERY'
},
)
# Add a ServiceState to simulate a recharge using the set_battery_level service
StateMachine.add('RECHARGE_BATTERY',
ServiceState(
'battery_simulator/set_battery_level',
SetBatteryLevel,
request=100),
transitions={
'succeeded': 'MONITOR_BATTERY',
'aborted': 'MONITOR_BATTERY',
'preempted': 'MONITOR_BATTERY'
})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('monitor_battery',
sm_battery_monitor, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = sm_battery_monitor.execute()
intro_server.stop()
def __init__(self):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=[])
with self:
smach.StateMachine.add('SET_SOLENOID',
ServiceState(
'/set_solenoid',
Trigger,
request_cb=SetSolenoid.request_cb,
response_cb=SetSolenoid.response_cb,
input_keys=[]),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
def __init__(self):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'aborted', 'preempted'],
output_keys=['standard_error', 'asr_status'])
with self:
# Request
def AsrServerRequestStatus_cb(userdata, request):
requ = ASRSrvRequest()
requ.requests = [ASRSrvRequest.STATUS]
requ.activation.action = ASRActivation.ACTIVATE
requ.model.action = ASRLangModelMngmt.ENABLE
requ.model.modelName = ''
requ.lang.language = 'en_US'
userdata.standard_error = ''
return requ
# Callback
def asr_status_cb(userdata, resp):
rospy.loginfo(resp.response.status.active)
rospy.loginfo(resp.response.status.enabled_grammar)
rospy.loginfo(resp.response.status.language)
rospy.loginfo(resp.response.error_msg)
rospy.loginfo(resp.response.warn_msg)
self.userdata.standard_error = resp.response.error_msg
self.userdata.asr_status = resp
smach.StateMachine.add('ASRStatus',
ServiceState(
'/asr_server',
ASRService,
request_cb=AsrServerRequestStatus_cb,
response_cb=asr_status_cb,
output_keys=['standard_error']),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
