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()