def main():
rospy.init_node('follow_waypoints')
sm = StateMachine(outcomes=['success'])
with sm:
StateMachine.add('GET_PATH',
GetPath(),
transitions={'success': 'FOLLOW_PATH'},
remapping={'waypoints': 'waypoints'})
StateMachine.add('FOLLOW_PATH',
FollowPath(),
transitions={'success': 'PATH_COMPLETE'},
remapping={'waypoints': 'waypoints'})
StateMachine.add('PATH_COMPLETE',
PathComplete(),
transitions={'success': 'GET_PATH'})
outcome = sm.execute()
def start(poses_received):
#rospy.init_node('fsm_goal', anonymous=True)
poses = poses_received
sm = StateMachine(outcomes=['success'])
sm.userdata.sm_input = 1
with sm:
StateMachine.add('A',
A(poses[0]),
transitions={
'1': 'B',
'0': 'A'
},
remapping={
'input': 'sm_input',
'output': 'input'
})
StateMachine.add('B',
B(poses[1]),
transitions={
'1': 'C',
'0': 'B'
},
remapping={
'input': 'sm_input',
'output': 'input'
})
StateMachine.add('C',
C(poses[2]),
transitions={
'1': 'A',
'0': 'C'
},
remapping={
'input': 'sm_input',
'output': 'input'
})
sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
sis.start()
sm.execute()
rospy.spin()
sis.stop()
def __init__(self):
global object_name
rospy.init_node('grasp_task')
rospy.logwarn('grasp test is beginning')
xm_arm_moveit_name('before_grasp_bag')
self.sm_Grasp = StateMachine(
outcomes=['succeeded', 'aborted', 'error'])
with self.sm_Grasp:
self.sm_Grasp.userdata.arm_ps = PointStamped()
self.sm_Grasp.userdata.name = object_name
StateMachine.add('FIND_OBJECT',
FindObject(),
remapping={
'name': 'name',
'object_pos': 'arm_ps'
},
transitions={
'succeeded': 'GRASP',
'aborted': 'FIND_OBJECT',
'error': 'error'
})
self.sm_Grasp.userdata.arm_mode_1 = 3
StateMachine.add('GRASP',
GraspSmach(),
remapping={
'arm_ps': 'arm_ps',
'arm_mode': 'arm_mode_1'
},
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'error': 'error'
})
# self.sm_Grasp.userdata.move_point = Point(-0.1,0.0,0.0)
# StateMachine.add('MOVE_BACK',
# SimpleMove(),
# remapping={'point':'move_point'},
# transitions={'succeeded':'PLACE','aborted':'PLACE','error':'error'})
outcome = self.sm_Grasp.execute()
def __init__(self):
self.sm = StateMachine(outcomes=["success", "failure", "preempted"])
self.outcome = None
self.action_server = None
with self.sm:
StateMachine.add("GOTO_WAYPOINT",
GoToWaypointState(),
transitions={
"success": "GOTO_WAYPOINT",
"finished": "success",
"failure": "failure",
"preempted": "preempted"
},
remapping={
"waypoints": "sm_waypoints",
"waypoint_index_in": "sm_waypoint_index",
"waypoint_index_out": "sm_waypoint_index",
"action_server": "sm_action_server",
})
def polygon(sides):
polygon = StateMachine(outcomes=['success'])
with polygon:
# Add all but the final side
for i in xrange(sides - 1):
StateMachine.add('SIDE_{0}'.format(i + 1),
Drive(1),
transitions={'success':'TURN_{0}'.format(i + 1)})
# Add all the turns
for i in xrange(sides - 1):
StateMachine.add('TURN_{0}'.format(i + 1),
Turn(360.0 / sides),
transitions={'success':'SIDE_{0}'.format(i + 2)})
# Add the final side
StateMachine.add('SIDE_{0}'.format(sides),
Drive(1),
transitions={'success':'success'})
return polygon
def __init__(self):
rospy.init_node('xm_nav_test')
rospy.on_shutdown(self.shutdown)
self.xm_nav = StateMachine(outcomes=['succeeded','aborted','error','None'])
with self.xm_nav:
#gpsr_target={
# 'livingroom': {'pos': Pose(Point(5.809, -5.834, 0.000),Quaternion(0.000, 0.000, -0.743, 0.670)), 'mode': 1 },
# 'diningroom_table_1': {'pos': Pose(Point(4.069, -1.749, 0.000),Quaternion(0.000, 0.000, 1.000, 0.005)), 'mode': 1 }
#}
self.xm_nav.userdata.xm_goal=gpsr_target['livingroom']['pos']
StateMachine.add('NAV_1',
NavStack(),
transitions={'succeeded':'NAV_2','aborted':'aborted','error':'error','None':'NAV_1'})
self.xm_nav.userdata.xm_goal=gpsr_target['livingroom']['pos']
StateMachine.add('NAV_2',
NavStack(),
transitions={'succeeded':'succeeded','aborted':'aborted','error':'error','None':'NAV_2'})
run = self.xm_nav.execute()
if run == 'succeeded' :
self.smach_bool = True
def main():
rospy.init_node('smach_example_state_machine')
# Create the top level SMACH state machine
sm_top = StateMachine(outcomes=['outcome6'])
# Open the countainer
with sm_top:
StateMachine.add('BAS', Bas(), transitions={'outcome3': 'CON'})
# Create the sub SMACH state machine
sm_con = Concurrence(
outcomes=['outcome4', 'outcome5'],
default_outcome='outcome4',
outcome_map={'outcome5': {
'FOO': 'outcome2',
'BAR': 'outcome1'
}})
# Open the countainer
with sm_con:
# Add states to the container
Concurrence.add('FOO', Foo())
Concurrence.add('BAR', Bar())
StateMachine.add('CON',
sm_con,
transitions={
'outcome4': 'CON',
'outcome5': 'outcome6'
})
sis = IntrospectionServer('example', sm_top, '/SM_PATH')
sis.start()
# Execute SMACH plan
outcome = sm_top.execute()
rospy.spin()
sis.stop()
def __init__(self):
rospy.init_node('enter_room')
rospy.on_shutdown(self.shutdown)
self.sm_enter = StateMachine(outcomes= ['succeeded','aborted','preempted'])
self.nav_states_ = WayPoint().nav_states_
with self.sm_enter :
# navpose is the pose of xm_arm
self.sm_enter.userdata.mode = 0
StateMachine.add('NAV_POSE',
ArmCmd(),
transitions={'succeeded':'WAIT1','aborted':'NAV_POSE'},
remapping = {'mode':'mode'})
# this time (rec =1) may need to justfy to satisfy the scene
self.sm_enter.userdata.delay_time =1.0
StateMachine.add('WAIT1',
Wait(),
transitions={'succeeded':'DOOR'},
remapping = {'rec':'delay_time'})
StateMachine.add('DOOR',
DoorDetect().door_detect_,
transitions={'invalid':'NAV1','valid':'DOOR'})
StateMachine.add('NAV1',
self.nav_states_[0],
transitions={'succeeded':'TURN_POSE','aborted':'NAV1'})
pt = Point()
pt.x =0
pt.y=0
pt.z= pi/8
self.sm_enter.userdata.turn_pose = pt
StateMachine.add('TURN_POSE',
SimpleMove(),
transitions={'succeeded':'SPEAK','aborted':'SPEAK'},
remapping = {'turn_pose':'turn_pose'})
self.sm_enter.userdata.sentences = 'please look at me'
StateMachine.add('SPEAK',
Speak(),
transitions={'succeeded':'succeeded'},
remapping ={'sentences':'sentences'})
self.smach_bool = False
self.sm_enter.execute()
self.smach_bool =True
def test_userdata_remapping(self):
"""Test remapping of userdata."""
sm = StateMachine(['done', 'preempted', 'aborted'])
with sm:
StateMachine.add('SETTER',
Setter(), {'done': 'GETTER'},
remapping={'a': 'x'})
StateMachine.add('GETTER',
Getter(), {'done': 'done'},
remapping={
'a': 'x',
'b': 'y'
})
outcome = sm.execute()
assert outcome == 'done'
assert 'x' in sm.userdata
assert 'y' in sm.userdata
assert sm.userdata.x == 'A'
assert sm.userdata.y == 'A'
def test_concurrence(self):
"""Test concurrent container."""
sm = StateMachine(['done', 'succeeded'])
with sm:
cc = Concurrence(['succeeded', 'done'],
default_outcome='done',
outcome_map={'succeeded': {
'SETTER': 'done'
}})
sm.add('CONCURRENT', cc)
with cc:
Concurrence.add('SETTER', Setter())
Concurrence.add('GETTER', Getter())
outcome = sm.execute()
assert outcome == 'succeeded'
assert 'a' in cc.userdata
assert 'b' in cc.userdata
assert cc.userdata.a == 'A'
assert cc.userdata.b == 'A'
def __init__(self):
rospy.init_node('xm_arm_smach')
rospy.on_shutdown(self.shutdown)
xm_arm_moveit_name('nav_pose')
rospy.logerr('Link Start!')
self.arm_stack_service = rospy.Service('arm_stack', xm_PickOrPlace,
self.callback)
self.sm_ArmStack = StateMachine(outcomes=['succeeded', 'error'],
input_keys=['arm_point', 'arm_mode'])
with self.sm_ArmStack:
StateMachine.add('PICK',
DirectIK(),
remapping={
'arm_ps': 'arm_point',
'arm_mode': 'arm_mode'
},
transitions={
'succeeded': 'succeeded',
'error': 'error'
})
rospy.spin()
def get_move_around_smach():
sm = StateMachine(outcomes=['preempted'])
#sm.set_initial_state('CHECK_MOVEMENT')
with sm:
StateMachine.add('SLEEP_UNTIL_ENABLED',
util.get_sleep_until_smach_enabled_smach(),
transitions={'succeeded': 'MOVE_RANDOMLY'})
StateMachine.add('MOVE_RANDOMLY',
move_base.get_random_goal_smach('/base_link'),
transitions={
'succeeded': 'SLEEP_UNTIL_ENABLED',
'aborted': 'SLEEP_UNTIL_ENABLED'
})
# StateMachine.add("ARM_LOOK_AROUND", look_around.get_lookaround_smach(util.SleepState(LOOKAROUND_SLEEP_DURATION)),
# StateMachine.add('ARM_LOOK_AROUND', util.SleepState(1), # mockup
return sm
def __init__(self):
rospy.init_node('susu')
rospy.on_shutdown(self.shutdown)
self.smach_bool = False
self.susu = StateMachine(['succeeded','aborted','error'])
with self.susu:
self.susu.userdata.rec = 10.0
self.susu.userdata.sentences = 'how many women in this house?'
self.susu.userdata.welcome = 'you are not my master!'
self.susu.userdata.target = list()
self.susu.userdata.action = list()
StateMachine.add('Listen',
GetSignal(),
transitions={'succeeded':'See','aborted':'See','error':'error'})
StateMachine.add('See',
RunNode(),
transitions={'succeeded':'Wait','aborted':'Wait'})
StateMachine.add('Wait',
Wait(),
transitions={'succeeded':'Close','error':'error'},
remapping={'rec':'rec'})
StateMachine.add('Close',
CloseKinect(),
transitions={'succeeded':'Welcome','aborted':'Welcome'})
# StateMachine.add('Ask',
# Speak(),
# transitions={'succeeded':'Listen1','aborted':'Listen1','error':'error'})
# StateMachine.add('Listen1',
# GetSignal(),
# transitions={'succeeded':'Welcome','aborted':'Welcome','error':'error'})
# # StateMachine.add('Wait1',
# # Wait(),
# # transitions={'succeeded':'Welcome','error':'error'})
StateMachine.add('Welcome',
Speak(),
transitions = {'succeeded':'succeeded','aborted':'aborted','error':'error'},
remapping={'sentences':'welcome'})
out = self.susu.execute()
rospy.logerr(out)
def create_machine(topic, base_frame):
# Construct state machine
sm = StateMachine(outcomes=['succeeded',
'aborted',
'preempted'],
input_keys=['goal_message'])
with sm:
StateMachine.add('move_base',
SimpleActionState(topic, MoveBaseAction,
goal_key='goal_message'),
transitions={'succeeded': 'succeeded',
'aborted': 'close_goal',
'preempted': 'move_base'},
remapping={'goal_message': 'goal_message'})
def close_goal_cb(userdata, goal):
distance = uniform(0.01, 0.15)
angle = uniform(0, 2 * pi)
x, y = distance * cos(angle), distance * sin(angle)
drive_goal = MoveBaseGoal()
drive_goal.target_pose.header.frame_id = base_frame
drive_goal.target_pose.header.stamp = rospy.get_rostime()
drive_goal.target_pose.pose.position.x = x
drive_goal.target_pose.pose.position.y = y
drive_goal.target_pose.pose.orientation.w = 1.0
return drive_goal
StateMachine.add('close_goal',
SimpleActionState(topic, MoveBaseAction,
goal_cb=close_goal_cb),
transitions={'succeeded': 'move_base',
'aborted': 'close_goal',
'preempted': 'close_goal'})
return sm
def build_state_machine(self):
'''
建立状态机
'''
if not self.sm_carry_object is None:
self.sm_carry_object = None
self.sm_carry_object = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_carry_object:
StateMachine.add('GRASP_OBJECT', GraspObject(), transitions={'succeeded':'GET_LOCATION',
'aborted':'END_ABORT'})
StateMachine.add('GET_LOCATION', GetLocation(), transitions={'succeeded':'NAV_TO_WAYPOINT',
'aborted':'END_ABORT'},
remapping={'waypoint_out':'nav_waypoint'})
StateMachine.add('NAV_TO_WAYPOINT', Nav2Waypoint(), transitions={'succeeded':'RELEASE_OBJECT',
'aborted':'END_ABORT'},
remapping={'waypoint_in':'nav_waypoint'})
StateMachine.add('RELEASE_OBJECT', ReleaseObject(), transitions={'succeeded':'TURN_BODY',
'aborted':'END_ABORT'})
StateMachine.add('TURN_BODY', TurnBody(), transitions={'succeeded':'GRASP_OBJECT',
'aborted':'END_ABORT'})
StateMachine.add('END_ABORT', EndAbort(), transitions={'succeeded':'succeeded'})
StateMachine.add('END_SUCESS', EndSuccess(), transitions={'succeeded':'succeeded'})
def create_plan_executor():
pe = StateMachine(['succeeded', 'aborted', 'preempted'],
input_keys=['plan_goal'],
output_keys=['plan_result'])
pe.userdata.plan_feedback = PlanFeedback()
pe.userdata.waypoint_index = 0
pe.userdata.waypoints = None
keys = ['plan_index', 'plan', 'plan_goal', 'plan_result', 'plan_feedback']
with pe:
StateMachine.add('BUILD_PLAN', _BuildPlan(input_keys=keys, output_keys=keys),
transitions={'succeeded': 'RUN_PLAN', 'aborted': 'aborted', 'preempted': 'preempted'})
StateMachine.add('RUN_PLAN', SimpleActionState('move_base', MoveBaseGoal,
goal_cb=_next_goal,
result_cb=_goal_complete,
input_keys=keys,
output_keys=keys,
outcomes=['next', 'succeeded', 'aborted', 'preempted']),
transitions={'next': 'RUN_PLAN', 'succeeded': 'succeeded', 'aborted': 'aborted',
'preempted': 'preempted'})
return pe
def __init__(self):
rospy.init_node('smach_haha')
rospy.on_shutdown(self.shutdown)
self.smach_top = Concurrence(
outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='succeeded',
outcome_map={'succeeded': {
'STATE_1': 'succeeded'
}},
child_termination_cb=self.child_termination_cb)
with self.smach_top:
self.sm_state2 = StateMachine(outcomes=['succeeded'])
with self.sm_state2:
StateMachine.add('FIRST',
State2(),
transitions={'succeeded': 'SECOND'})
StateMachine.add('SECOND',
State1(),
transitions={'succeeded': 'FIRST'})
Concurrence.add('STATE_1', State1())
Concurrence.add('STATE_2', self.sm_state2)
haha = self.smach_top.execute()
def get_execute_sm():
# This state machine is large, so we will only instantiate once and point references to one copy
global shared_execute_sm
if shared_execute_sm:
return shared_execute_sm
shared_execute_sm = StateMachine(outcomes=["succeeded", "preempted", "aborted"], input_keys=["goal"],
output_keys=["msg_for_operator"])
with shared_execute_sm:
control_flow.inject_userdata_auto("_SET_DEFAULT_MSG_FOR_OPERATOR", "msg_for_operator", "")
repeat_state = control_flow.RepeatN(0)
StateMachine.add_auto("RESET_REPEAT", control_flow.ResetRepeat(repeat_state), ["succeeded"])
StateMachine.add("EXECUTE_GOAL", states.ExecuteGoal(),
transitions={"preempted": "RECOVERY_REPEAT_GATE",
"aborted": "RECOVERY_REPEAT_GATE"})
StateMachine.add("EXECUTE_RECOVERY_GOAL", states.ExecuteGoal(),
transitions={"preempted": "RECOVERY_REPEAT_GATE",
"aborted": "RECOVERY_REPEAT_GATE"})
StateMachine.add("RECOVERY_REPEAT_GATE", repeat_state,
transitions={"repeat": "RECOVER", "done": "aborted"})
StateMachine.add("RECOVER", get_shared_recover_from_failure_sm(),
transitions={'succeeded': "EXECUTE_RECOVERY_GOAL"})
return shared_execute_sm
def __init__(self):
rospy.init_node('test_vision', anonymous=False)
rospy.on_shutdown(self.shutdown)
rospy.loginfo('------------start test----------------')
self.test_bool = False
self.test_vision = StateMachine(
outcomes=['succeeded', 'aborted', 'error'])
with self.test_vision:
self.test_vision.userdata.name = 'person'
StateMachine.add(
'FINDOBJ',
RunNode(),
transitions={
'succeeded': 'CLOSENODE',
'aborted': 'aborted'
},
)
StateMachine.add('CLOSENODE',
CloseKinect(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
# self.test_vision.userdata.pos_xm =Pose()
# StateMachine.add('GET_PEOPLE_POS',
# FindPeople().find_people_,
# transitions ={'invalid':'succeeded','valid':'error','preempted':'aborted'},
# remapping = {'pos_xm':'pos_xm'})
# rospy.loginfo(self.test_vision.userdata.pos_xm)
out = self.test_vision.execute()
# if out =='error':
# rospy.logerr('no position past!')
# self.test_bool = False
# else:
self.test_bool = True
def navigate_behind_cube2(
marker, cube,
squares): # type: (ARTag, ARCube, List[ParkingSquare]) -> StateMachine
def goal_pose():
marker_square = closest_square(marker.pose.pose.position, squares)
cube_square = closest_square(cube.pose.pose.position, squares)
cube_number = max(2, min(4, cube_square.number))
if marker_square.number > cube_number:
target_square = next(s for s in squares
if s.number == cube_number - 1)
else:
target_square = next(s for s in squares
if s.number == cube_number + 1)
return target_square.pose
def direction():
marker_square = closest_square(marker.pose.pose.position, squares)
cube_square = closest_square(cube.pose.pose.position, squares)
if cube_square.number > marker_square.number:
direction = 'ccw'
else:
direction = 'cw'
return direction
sm = StateMachine(outcomes=['ok', 'err'])
with sm:
StateMachine.add('park',
park_into_pose(goal_pose, 0.4),
transitions={'ok': 'CHOOSE_DIRECTION'})
StateMachine.add('CHOOSE_DIRECTION',
ReturnFunctionState(direction, ['ccw', 'cw']),
transitions={
'ccw': 'TURN_CCW',
'cw': 'TURN_CW'
})
StateMachine.add('TURN_CCW', RotateState(angle=np.pi / 2))
StateMachine.add('TURN_CW', RotateState(angle=-np.pi / 2))
return sm
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 create_grasp_sm():
sm = StateMachine(outcomes=['succeeded','aborted','preempted'],
input_keys=['closest_index','segmentation_result'])
with sm:
def grasp_goal_cb(userdata, goal):
grasp_goal = GraspObjectGoal()
grasp_goal.category_id = -1
grasp_goal.graspable_object = userdata.segmentation_result.graspable_objects[userdata.closest_index]
grasp_goal.collision_object_name = userdata.segmentation_result.collision_object_names[userdata.closest_index]
grasp_goal.collision_support_surface_name = userdata.segmentation_result.collision_support_surface_name
return grasp_goal
StateMachine.add('GRASP_OBJECT',
SimpleActionState('grasp_object',
GraspObjectAction,
goal_cb=grasp_goal_cb,
input_keys=['closest_index',
'segmentation_result']),
remapping={'closest_index':'closest_index',
'segmentation_result':'segmentation_result'})
return sm
def simpleExplorationContainer():
sm_simple_exploration = StateMachine(outcomes=['preempted'])
with sm_simple_exploration:
StateMachine.add('SELECT_TARGET',
SelectTargetState('explore'),
transitions={
'succeeded': 'MOVE_BASE',
'aborted': 'SELECT_TARGET',
'preempted': 'preempted'
})
StateMachine.add('MOVE_BASE',
MoveBaseState(),
transitions={
'succeeded': 'SELECT_TARGET',
'aborted': 'SELECT_TARGET',
'preempted': 'preempted'
})
return sm_simple_exploration
def add_states(self):
sm_nested = StateMachine(outcomes=self.outcomes,
input_keys=self.input_keys,
output_keys=self.output_keys)
machine_count = 0
state_count = 0
StateMachine.add(self.title,
sm_nested,
transitions=self.transitions,
remapping=self.remapping)
with sm_nested:
for thing in self.things_to_add:
if thing == 's':
addition = self.state_machine_additions[state_count]
StateMachine.add(addition.title,
addition.state,
addition.transitions,
remapping=addition.remapping)
state_count += 1
else:
machine = self.state_machines[machine_count]
machine.add_states()
machine_count += 1
def main():
rospy.init_node('follow_waypoints')
sm = StateMachine(outcomes=['success'])
with sm:
StateMachine.add('GET_PATH',
GetPath(),
transitions={'success': 'FOLLOW_PATH'},
remapping={'waypoints': 'waypoints'})
StateMachine.add('FOLLOW_PATH',
FollowPath(),
transitions={'success': 'PATH_COMPLETE'},
remapping={'waypoints': 'waypoints'})
StateMachine.add('PATH_COMPLETE',
PathComplete(),
transitions={'success': 'GET_PATH'})
sis = smach_ros.IntrospectionServer('follow_waypoints', sm,
'/FOLLOW_WAYPOINTS')
sis.start()
outcome = sm.execute()
sis.stop()
def __init__(self):
State.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'])
setup_task_environment(self)
# Initialize the navigation state machine
self.sm_nav = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
self.sm_nav.userdata.waypoints = self.waypoints
with self.sm_nav:
StateMachine.add('PICK_WAYPOINT',
PickWaypoint(),
transitions={'succeeded': 'NAV_WAYPOINT'},
remapping={'waypoint_out': 'patrol_waypoint'})
StateMachine.add('NAV_WAYPOINT',
Nav2Waypoint(),
transitions={
'succeeded': 'PICK_WAYPOINT',
'aborted': 'PICK_WAYPOINT',
'preempted': ''
},
remapping={'waypoint_in': 'patrol_waypoint'})
pass
def __init__(self, keyPointManager):
State.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'])
self.keyPointManager = keyPointManager
self.sm_nav = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_nav:
StateMachine.add('PICK_KEYPOINT',
PickWaypointKeyPoint(self.keyPointManager),
transitions={'succeeded': 'NAV_KEYPOINT'},
remapping={'waypoint_out': 'patrol_waypoint'})
StateMachine.add('NAV_KEYPOINT',
Nav2Waypoint(),
transitions={
'succeeded': 'PICK_KEYPOINT',
'aborted': 'PICK_KEYPOINT',
'preempted': ''
},
remapping={'waypoint_in': 'patrol_waypoint'})
pass
def __init__(self):
rospy.init_node('speech_test')
rospy.on_shutdown(self.shutdown)
self.test = StateMachine(outcomes=['succeeded', 'aborted', 'error'])
with self.test:
self.test.userdata.target = list()
self.test.userdata.action = list()
self.test.userdata.task_num = 0
self.test.userdata.sentences = 'i am shall mon'
self.test.userdata.people_condition = list()
StateMachine.add('NAME',
NameAndThing(),
transitions={
'succeeded': 'succeeded',
'error': 'error',
'aborted': 'aborted'
})
#得到命令
# StateMachine.add('GETTASK',
# GetTask(),
# transitions={'succeeded':'SPEAK','aborted':'aborted'},
# remapping={'target':'target','action':'action','task_num':'task_num'})
# StateMachine.add('SPEAK',
# Speak(),
# transitions={'succeeded':'ANSWER','aborted':'aborted','error':'error'},
# remapping={'sentences':'sentences'})
# StateMachine.add('ANSWER',
# Anwser(),
# transitions={'succeeded':'succeeded','aborted':'aborted'})
rospy.loginfo(self.test.userdata.target)
rospy.loginfo(self.test.userdata.action)
rospy.loginfo(self.test.userdata.task_num)
intro_server = IntrospectionServer('test', self.test, '/SM_ROOT')
intro_server.start()
out = self.test.execute()
intro_server.stop()
def __init__(self):
rospy.init_node('test_gripper_smach', anonymous=False)
gripper_goal = xm_GripperCommandGoal()
gripper_goal.command.position = 0.10
gripper_goal.command.torque = 0.5
gripper_state = SimpleActionState(
'xm_move_gripper',
xm_GripperCommandAction,
goal=gripper_goal,
result_cb=self.gripper_state_cb,
exec_timeout=rospy.Duration(10),
server_wait_timeout=rospy.Duration(10))
self.gripper_smach = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.gripper_smach:
StateMachine.add('GRIPPER_OPEN',
gripper_state,
transitions={
'succeeded': '',
'aborted': '',
'preempted': ''
})
gripper_outcome = self.gripper_smach.execute()
rospy.loginfo('State Machine Outcome: ' + str(gripper_outcome))
def main():
rospy.init_node('wpirm_state_machine')
sm = StateMachine(outcomes=['stop'])
sm.userdata.waypoints = rospy.get_param('~waypoints')
sm.userdata.cur_waypoint = 0
sm.userdata.last_waypoint = rospy.get_param('~last_waypoint', default=3)
with sm:
StateMachine.add('GPS_FIX', GPSFix(),
transitions={'success':'NAV_TO_CONE'})
StateMachine.add('NAV_TO_CONE', NavToCone(),
transitions={'success':'TOUCH_CONE'},
remapping={'waypoints':'waypoints', 'cur_waypoint_in':'cur_waypoint'})
StateMachine.add('TOUCH_CONE', TouchCone(),
transitions={'success':'RESET_CONE', 'fail':'FAIL_TOUCH', 'last_cone':'stop'},
remapping={'cur_waypoint_in':'cur_waypoint', 'last_waypoint':'last_waypoint'})
StateMachine.add('FAIL_TOUCH', FailTouch(),
transitions={'success':'TOUCH_CONE'})
StateMachine.add('RESET_CONE', ResetCone(),
transitions={'success':'NAV_TO_CONE'},
remapping={'cur_waypoint_in':'cur_waypoint', 'cur_waypoint_out':'cur_waypoint'})
outcome = sm.execute()
