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 __init__(self, service_name, service_spec, **args):
"""
Init method also initializes timer and executions counter
"""
ServiceState.__init__(self, service_name = service_name, service_spec = service_spec, **args)
self.total_time = 0
self.num_executions = 0
def __init__(self, leftArmEnabled=True, rightArmEnabled=True):
ServiceState.__init__(self,
'/enable_arms_walking_srv',
SetArmsEnabled,
request=SetArmsEnabledRequest(
left_arm=leftArmEnabled,
right_arm=rightArmEnabled))
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('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 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 __init__(self, grammar, enabled):
"""Constructor for GrammarState.
@type grammar: string
@param grammar: The grammar name that you want enabled or disable.
@type enabled: boolean
@param enabled: If False, 0 or None, the grammar will be disabled. Enabled otherwise.
"""
def asr_request_cb(userdata, old_request):
if enabled:
print "ASR: Enabling grammar '%s'" % grammar
else:
print "ASR: Disabling grammar '%s'" % grammar
update = asrupdate()
update.enable_grammar = grammar
update.active = bool(enabled)
return update
ServiceState.__init__(self,
GRAMMAR_ACTION_NAME, recognizerService,
request_cb=asr_request_cb)
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 __init__(self): # Class constructor ServiceState.__init__(self, '/nao_shopping_list/checkObjects', checkObjects, outcomes=['succeeded'], input_keys=['shopping_list'], output_keys=['shopping_list'], response_cb=self.shopping_list_response_cb)
def __init__(self, frame='/map'):
ServiceState.__init__(
self,
'move_base/make_plan',
GetPlan,
input_keys=['x', 'y', 'yaw', 'start_x', 'start_y', 'start_yaw'],
request_cb=self.__request_cb)
self.frame = frame
def __init__(self, service, name_template="capture%03d.png"):
ServiceState.__init__(self,
service,
SaveFile,
request_cb=self._request_cb,
response_cb=self._response_cb)
self._name_template = name_template
self._n = 0
def __init__(self, frame="/map"):
ServiceState.__init__(
self,
"move_base/make_plan",
GetPlan,
input_keys=["x", "y", "yaw", "start_x", "start_y", "start_yaw"],
request_cb=self.__request_cb,
)
self.frame = frame
def __init__(self):
def resp_cb(ud, response):
ud.resp_text = response.question_guess_response
if response.is_guess:
return 'game_finished'
return 'continue_game'
ServiceState.__init__(self, '/akinator_srv', AkinatorQA,
output_keys=['resp_text'], request_slots=['question_response'],
outcomes=['game_finished', 'continue_game'], response_cb=resp_cb)
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):
event = BehaviourEventRequest()
event.name = 'presentToPerson'
#event.name = 'say'
#arg = BehaviourArgument()
#arg.key = 'text'
#arg.value = 'Hello World'
#event.arguments = [arg,]
ServiceState.__init__(self, '/behaviour', BehaviourEvent, request = event)
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 __init__(self):
service = '/clopema_planner/grasp_it'
ServiceState.__init__(self,
service,
ClopemaGraspIt,
request_slots=[
'ik_link', 'poses', 'frame_id',
'offset_minus', 'offset_plus'
],
response_cb=self.result_cb,
output_keys=['trajectory'])
def __init__(self):
service = '/clopema_planner/grasp_from_table'
ServiceState.__init__(self,
service,
ClopemaGraspFromTable,
request_slots=[
'ik_link', 'poses', 'frame_id', 'table_desk',
'offset_minus', 'offset_plus',
'offset_table_plus', 'offset_table_minus',
'grasping_angle'
],
response_cb=self.result_cb,
output_keys=['trajectory'])
def __init__(self, objective=None):
# Private attributes
input_keys = []
if not objective:
input_keys = ['objective']
self._objective = None
elif not isinstance(objective, Pose2D):
self._objective = Pose2D(objective[0], objective[1], objective[2])
else:
self._objective = objective
# Class constructor
ServiceState.__init__(self, '/cmd_pose_srv', CmdPoseService, outcomes=['succeeded'], request_cb=self.move_to_request_cb, input_keys=input_keys)
def __init__(self, twist_msg):
self._twist = twist_msg
input_keys = []
if not twist_msg:
input_keys = ['velocity']
# Method to define the request
def walk_request_cb(ud, request):
if (not self._twist):
self._twist = ud.velocity
walk_request = CmdVelServiceRequest()
walk_request.twist = self._twist
return walk_request
ServiceState.__init__(self, '/cmd_vel_srv', CmdVelService, input_keys=input_keys, request_cb=walk_request_cb)
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 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 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 __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 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, 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, 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, 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,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 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, 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, 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 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 __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 execute(self, ud):
try:
params = { 'travel_speed_sfl' : 0.45 }
config = self._dyn_reconfigure_client.update_configuration(params)
except:
print 'Error setting travel_speed_sfl parameter'
if self.feedback != None :
ud.action_feedback = self.feedback
return ServiceState.execute(self, ud)
def check_objects_id(self):
rospy.set_param("coord_translator_print_info", False)
self._print_title("CHECKING OBJECTS ID")
if not self.object_names:
self._print_warning("Not checking. 'object_names' is empty")
elif self.__check_service(OBJECT_TRANSLATOR_SERVICE) == succeeded:
obj_translator_result = ServiceState(OBJECT_TRANSLATOR_SERVICE, ObjectTranslator, request_cb=self.obj_request_cb, response_cb=self.obj_response_cb, request_key='obj_name')
userdata_hacked = {}
for obj_name in self.object_names:
userdata_hacked["obj_name"] = obj_name
if obj_translator_result.execute(userdata_hacked) == succeeded:
self._print_info("Translating '%s': OK (ID = %s)" % (obj_name, self.object_id))
else:
self.ALL_OK = False
self._print_fatal("Translating '%s': FAILED (ID NOT IN '%s')" % (obj_name, OBJECT_TRANSLATOR_SERVICE))
rospy.set_param("coord_translator_print_info", True)
def check_map_locations(self):
rospy.set_param("coord_translator_print_info", False)
self._print_title("CHECKING MAP LOCATIONS")
if not self.map_locations:
self._print_warning("Not checking. 'map_locations' is empty")
elif self.__check_service(COORD_TRANSLATOR_SERVICE) == succeeded:
loc_translator_result = ServiceState(COORD_TRANSLATOR_SERVICE, LocationTranslator, response_cb=self.loc_response_cb, request_key='room_name')
userdata_hacked = {}
for place in self.map_locations:
userdata_hacked["room_name"] = place
if loc_translator_result.execute(userdata_hacked) == succeeded:
self._print_info("Translating '%s': OK (%.2f, %.2f, %.2f)" % (str(place), self.coordinates.x, self.coordinates.y, self.coordinates.z))
else:
self.ALL_OK = False
self._print_fatal("Translating '%s': FAILED" % str(place))
rospy.set_param("coord_translator_print_info", True)
def execute(self, userdata):
srv_out = ServiceState.execute(self, userdata)
rospy.logdebug('srv_out was: ' + srv_out + ', response.positions_ok is: %r' % self._response.positions_ok)
# rospy.loginfo('srv_out was: ' + srv_out + ', userdata[\'positions_ok\'] is: %r' % userdata['positions_ok'])
# rospy.loginfo('srv_out was: ' + srv_out + ', userdata.positions_ok is: %r' % userdata.positions_ok)
# if srv call was successfull but the joint motion wasn't, override return value
if srv_out == 'succeeded' and self._response.positions_ok != True:
return 'aborted'
return srv_out
def __init__(self, frame='/map'):
ServiceState.__init__(self, 'move_base/make_plan', GetPlan,
input_keys=['x', 'y', 'yaw', 'start_x', 'start_y', 'start_yaw'],
request_cb=self.__request_cb)
self.frame = frame
def __init__(self, feedback_to_provide=None):
ServiceState.__init__(self, '/RosPatrolService/start', Empty, output_keys=['action_feedback'])
self._dyn_reconfigure_client = dynamic_reconfigure.client.Client('/move_base/PalLocalPlanner')
self.feedback = feedback_to_provide
def __init__(self):
ServiceState.__init__(self, '/stop_recognition', Empty)
def __init__(self):
ServiceState.__init__(self, '/RosPatrolService/stop', Empty)
def __init__(self):
ServiceState.__init__(self, '/stop_walk_srv', Empty)
