def test_group(self): """Test adding a bunch of states with group args.""" class DoneState(State): def __init__(self): State.__init__(self, outcomes=['done']) def execute(self, ud=None): return 'done' sm = StateMachine(['succeeded', 'done']) with sm: StateMachine.add('FAILSAUCE', DoneState()) transitions = {'aborted': 'FAILSAUCE', 'preempted': 'FAILSAUCE'} with sm: StateMachine.add( 'FIRST', SimpleActionState('reference_action', TestAction, goal=g1), transitions) StateMachine.add( 'SECOND', SimpleActionState('reference_action', TestAction, goal=g2), transitions) StateMachine.add( 'THIRD', SimpleActionState('reference_action', TestAction, goal=g1), transitions) outcome = sm.execute() assert outcome == 'done'
def IdentificationSimpleContainer(): sm_identification_simple = StateMachine(['parked', 'aborted', 'preempted']) with sm_identification_simple: StateMachine.add('GET_VICTIMS', SimpleActionState('/navigation/initial_turn', InitialTurnAction), transitions={'succeeded': 'GO_TO_VICTIM'}) StateMachine.add('GO_TO_VICTIM', utils.TargetSelectorContainer('victim'), transitions={ 'target_sent': 'PARK', 'aborted': 'aborted', 'preempted': 'preempted' }) StateMachine.add('PARK', SimpleActionState('/navigation/initial_turn', InitialTurnAction), transitions={ 'succeeded': 'parked', 'aborted': 'aborted', 'preempted': 'preempted' }) return sm_identification_simple
def __init__(self): rospy.init_node('move_to_and_inspect_point_sm', anonymous=False) hsm = StateMachine(outcomes=['finished statemachine']) with hsm: StateMachine.add( 'GO_TO_POINT', SimpleActionState( 'pid_global', MoveAction, makeMoveGoal("pid_global_plan", -3.0, 0, -0.5, radius_of_acceptance = 2.0)), transitions = { "succeeded": 'INSPECT_POINT', "preempted": 'INSPECT_POINT', "aborted": 'INSPECT_POINT' }) StateMachine.add( 'INSPECT_POINT', SimpleActionState( 'inspect_point', MoveAction, makeMoveGoal("inspect_point", -3.0, 0.0, -0.5, radius_of_acceptance=2.0)), transitions = { 'succeeded': 'INSPECT_POINT', "preempted": 'INSPECT_POINT', "aborted": 'INSPECT_POINT' }) intro_server = IntrospectionServer(str(rospy.get_name()), hsm,'/SM_ROOT') intro_server.start() hsm.execute() #patrol.execute() print("State machine execute finished") intro_server.stop()
def main(): rospy.init_node('tic_crane_state_machine') sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) with sm: deploy_goal = msg.MoveToPositionGoal(position=500) store_goal = msg.MoveToPositionGoal(position=0) measure_goal = msg.MeasurementGoal(measurement_duration=7) sm.add('DEPLOY', SimpleActionState('move_to_target_pose', msg.MoveToPositionAction, goal=deploy_goal), transitions={'succeeded': 'MEASURE'}) sm.add('MEASURE', SimpleActionState('measurement', msg.MeasurementAction, goal=measure_goal), transitions={'succeeded': 'STORE'}) sm.add('STORE', SimpleActionState('move_to_target_pose', msg.MoveToPositionAction, goal=store_goal), transitions={'succeeded': 'succeeded'}) outcome = sm.execute()
def TargetSelectorContainer(target_type): sm_target_selector = StateMachine(['target_sent', 'aborted', 'preempted']) with sm_target_selector: target_selection_goal = targetSelectionGoal(target_type) StateMachine.add('GET_TARGET', SimpleActionState('/select_target', SelectTargetAction, goal=target_selection_goal, result_key='target_pose'), transitions={ 'succeeded': 'MOVE_BASE', 'aborted': 'aborted', 'preempted': 'preempted' }, remapping={'target_pose': 'next_target'}) StateMachine.add('MOVE_BASE', SimpleActionState('/move_base', MoveBaseAction, goal_key='next_target'), transitions={ 'succeeded': 'target_sent', 'aborted': 'GET_TARGET', 'preempted': 'preempted' }) return sm_target_selector
def __init__(self, input_keys=['tuck_left', 'tuck_right']): tuck_arms_uri = '/tuck_arms' SimpleActionState.__init__(self, tuck_arms_uri, TuckArmsAction, goal_cb=self.goal_cb, input_keys=input_keys)
def __init__(self, text=None, text_cb=None, wait_before_speaking=0, **kwargs): """ @param text: the text to speak out aloud (if `text_cb' is None) @param text_cb: a callback returning the text to speak out (if `text' is None) @param wait_before_speaking: how long to wait before speaking; it may be a number (in seconds) or a rospy.Duration @param **kwargs: input_keys, output_keys, etc. """ if not text and not text_cb: raise ValueError("Neither `text' nor `text_cb' where set!") elif text and text_cb: raise ValueError("You've set both `text' and `text_cb'!") if not isinstance(wait_before_speaking, rospy.Duration): wait_before_speaking = rospy.Duration(wait_before_speaking) def generic_goal_cb(userdata, old_goal): tts_goal = SoundGoal() tts_goal.wait_before_speaking = wait_before_speaking tts_goal.text = text if text else text_cb(userdata) print "Speaking:", tts_goal.text return tts_goal SimpleActionState.__init__(self, TTS_ACTION_NAME, SoundAction, goal_cb=generic_goal_cb, **kwargs)
def __init__(self): SimpleActionState.__init__(self, topics["plan_execution"], ExecutePlanAction, goal_cb=self.goal_cb, result_cb=self.result_cb, input_keys=['goal'], output_keys=['result'])
def main(): rospy.init_node("fsm_dummy") sm = StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) with sm: StateMachine.add('INITIAL_TURN', SimpleActionState('/initial_turn', InitialTurnAction), transitions={ 'succeeded': 'EXPLORATION', 'aborted': 'aborted', 'preempted': 'preempted' }) StateMachine.add('EXPLORATION', exploration.ExplorationContainer(), transitions={ 'victim_thermal': 'IDENTIFICATION_SIMPLE', 'victim_camera': 'IDENTIFICATION_TRACKING', 'aborted': 'aborted', 'preempted': 'preempted', 'time_out': 'AGGRESSIVE_EXPLORATION' }) StateMachine.add('IDENTIFICATION_SIMPLE', identification.IdentificationSimpleContainer(), transitions={ 'parked': 'succeeded', 'aborted': 'aborted', 'preempted': 'preempted' }) StateMachine.add('IDENTIFICATION_TRACKING', identification.IdentificationTrackingContainer(), transitions={ 'identification_finished': 'succeeded', 'aborted': 'aborted', 'preempted': 'preempted' }) StateMachine.add('AGGRESSIVE_EXPLORATION', SimpleActionState('/navigation/initial_turn', InitialTurnAction), transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted', 'preempted': 'preempted' }) sis = smach_ros.IntrospectionServer('smach_server', sm, '/DUMMY_FSM') sis.start() smach_ros.set_preempt_handler(sm) # Execute SMACH tree in a separate thread so that we can ctrl-c the script smach_thread = threading.Thread(target=sm.execute) smach_thread.start() rospy.spin() sis.stop()
def __init__(self, frame='/map'): SimpleActionState.__init__(self, 'move_base', MoveBaseAction, input_keys=['x', 'y', 'yaw'], goal_cb=self.__goal_cb) self.frame = frame
def __init__( self, goal_positon, travel_speed=0.5, sphere_of_acceptance=0.5, action_server="/guidance_interface/los_server", ): """A SimpleActionState for traveling to a goal position using LOS guidance. Args: goal_positon (geometry_msgs/Point): position drone should travel to. start_position (geometry_msgs/Point): position drone starts in. travel_speed (float, optional): forward velocity drone shoudl travel at. Defaults to 0.5. sphere_of_acceptance (float, optional): action returns success when drone is inside this sphere. Defaults to 0.5. action_server (str, optional): name of los action server. Defaults to "/guidance_interface/los_server". """ goal = LosPathFollowingGoal() goal.next_waypoint = goal_positon goal.forward_speed = travel_speed goal.desired_depth = goal_positon.z goal.sphereOfAcceptance = sphere_of_acceptance SimpleActionState.__init__(self, action_server, LosPathFollowingAction, goal)
def _result_cb(userdata, status, result): """ This function will analize the status of the response If the status is aborted, will call the action again with the same orientation, but with the distance -0,5. If the status now is succeeded, then call the move_by with x=0.5 in front. :type status: actionlib.GoalStatus :type result: MoveBaseResult """ if status != GoalStatus.SUCCEEDED: rospy.loginfo(C.BACKGROUND_YELLOW + "Retrying go to the target goal" + C.NATIVE_COLOR) new_pose = pose_at_distance(self.nav_goal.target_pose.pose, DISTANCE_TO_RETRY ) self.nav_goal.target_pose.pose = new_pose self.nav_goal.target_pose.header.stamp = rospy.Time.now() move_action = SimpleActionState(move_base_action_name, MoveBaseAction, goal=self.nav_goal) result_status = move_action.execute(userdata) if result_status == succeeded: new_goal = MoveBaseGoal() new_goal.target_pose.header.frame_id = FRAME_BASE_LINK new_goal.target_pose.pose.position.x = DISTANCE_TO_RETRY new_goal.target_pose.header.stamp = rospy.Time.now() move_action = SimpleActionState(MOVE_BY_ACTION_NAME , MoveBaseAction, goal=new_goal) return move_action.execute(userdata)
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, target_type): target_selection_goal = self.targetSelectionGoal(target_type) SimpleActionState.__init__(self, select_target_topic, SelectTargetAction, goal=target_selection_goal, result_key='target_pose')
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, vocabulary_list): if not isinstance(vocabulary_list, list): rospy.logerr('SetSpeechVocabularyState received something which is not a list') sys.exit(-1) elif vocabulary_list == []: rospy.logwarn('SetSpeechVocabularyState received an empty vocabulary_list') voc_goal = SetSpeechVocabularyGoal() voc_goal.words = vocabulary_list SimpleActionState.__init__(self, '/speech_vocabulary_action', SetSpeechVocabularyAction, goal=voc_goal)
def __init__(self): SimpleActionState.__init__( self, move_kinect_topic, MoveSensorAction, goal_cb=self.goal_cb, outcomes=['succeeded', 'aborted', 'preempted'], input_keys=['move_end_effector_msg'], output_keys=['move_end_effector_msg'])
def __init__(self, helper_obj): StateMachine.__init__(self, outcomes=['complete', 'preempted', 'aborted'], input_keys=['mission_data'], output_keys=['mission_status']) self.__helper_obj = helper_obj with self: # This state will prepare for the mission StateMachine.add('PREPARE_MISSION', PrepareMission4(self.__helper_obj), transitions={ 'ready': 'DEFAULT_HEAD_POSITION', 'error': 'complete' }) # Set up action goal for deafult head position default_position_pan, default_position_tilt = self.__helper_obj.CameraDefaultPos( ) head_goal = point_headGoal() head_goal.absolute = True head_goal.pan = default_position_pan head_goal.tilt = default_position_tilt # Add the default camera position state. Which moves the head to the default position StateMachine.add('DEFAULT_HEAD_POSITION', SimpleActionState( 'head_control_node', point_headAction, goal=head_goal, result_cb=self.defaultHeadPositionComplete, output_keys=['mission_status']), transitions={ 'succeeded': 'MOVE', 'preempted': 'preempted', 'aborted': 'aborted' }) # This state uses an Action to move the robot to the required goal StateMachine.add('MOVE', SimpleActionState('move_base', MoveBaseAction, goal_slots=['target_pose'], result_cb=self.moveComplete, output_keys=['mission_status']), transitions={ 'succeeded': 'complete', 'preempted': 'preempted', 'aborted': 'aborted' }, remapping={'target_pose': 'destination'})
def __init__(self, posture_name, speed): # Assert posture is known if not posture_name in self.POSTURES: rospy.logerr('Posture "%s" is an unknown posture! Known postures are: %s. Shutting down node...' % (posture_name, str(self.POSTURES))) sys.exit(-1) def body_pose_goal_cb(userdata, goal): goal = BodyPoseWithSpeedGoal() goal.posture_name = posture_name goal.speed = speed return goal SimpleActionState.__init__(self, '/body_pose_naoqi', BodyPoseWithSpeedAction, goal_cb=body_pose_goal_cb)
def __init__(self, pose, action_server="/guidance_interface/dp_server"): """A SimpleActionState that travels to a goal pose using our DP guidance. Only use when in close proximity of goal pose. Args: pose (geometry_msgs/Pose): Goal pose. action_server (str, optional): name of dp action server. Defaults to "/guidance_interface/dp_server". """ goal = MoveBaseGoal() goal.target_pose.pose = pose SimpleActionState.__init__(self, action_server, MoveBaseAction, goal)
def __init__(self, behavior_name=None): # Assert behavior is installed input_keys = [] if not behavior_name: input_keys = ['behavior_name'] else: _checkInstalledBehavior(behavior_name) def behavior_goal_cb(userdata, goal): goal = RunBehaviorGoal() goal.behavior = userdata.behavior_name if not behavior_name else behavior_name return goal SimpleActionState.__init__(self, '/run_behavior', RunBehaviorAction, goal_cb=behavior_goal_cb, input_keys=input_keys)
def create_action_state(step): """Take key value pair of the state of the state and create a SimpleActionState based on the parameters in the config file""" action_type = step.keys()[0] name = step[action_type] if action_type == "pose": action_state = SimpleActionState('coordinate_action', CoordinateAction, goal=create_coordinate_goal(name)) elif action_type == "move": action_state = SimpleActionState('move_base', MoveBaseAction, goal=create_move_goal(name)) else: raise Exception("action_type pose and move are the only ones implemented") rospy.logdebug("State with name") return name.upper(), action_state
def __init__(self, vocabulary_list): if not isinstance(vocabulary_list, list): rospy.logerr( 'SetSpeechVocabularyState received something which is not a list' ) sys.exit(-1) elif vocabulary_list == []: rospy.logwarn( 'SetSpeechVocabularyState received an empty vocabulary_list') voc_goal = SetSpeechVocabularyGoal() voc_goal.words = vocabulary_list SimpleActionState.__init__(self, '/speech_vocabulary_action', SetSpeechVocabularyAction, goal=voc_goal)
def __init__(self, object_to_detect_name=None): smach.StateMachine.__init__( self, outcomes=['succeeded', 'preempted', 'aborted'], input_keys=['object_name'], output_keys=['object_pose']) with self: smach.StateMachine.add('Prepare_data', Prepare_data(object_to_detect_name), transitions={'succeeded': 'Prepare_goal'}) smach.StateMachine.add('Prepare_goal', prepare_object_detection_goal(), transitions={'succeeded': 'Object_detect'}) def object_detect_result_cb(self, status, object_detect_result): print('Object Detected: ' + str(object_detect_result)) self.object_pose = object_detect_result return 'succeeded' smach.StateMachine.add( 'Object_detect', SimpleActionState( objectDetect_topic, RecognizeAction, result_cb=object_detect_result_cb, goal_key='object_detection_goal', output_keys=['object_pose', 'standard_error']), transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted' })
def __init__(self, pose_to_place=''): smach.StateMachine.__init__( self, outcomes=['succeeded', 'preempted', 'aborted'], input_keys=['pose_to_place'], output_keys=[]) with self: smach.StateMachine.add("Prepare_data", Prepare_data(pose_to_place), transitions={ 'succeeded': 'Place_Object_Goal', 'aborted': 'aborted' }) smach.StateMachine.add('Place_Object_Goal', Prepare_Place_Goal(), transitions={'succeeded': 'Place_Object'}) smach.StateMachine.add('Place_Object', SimpleActionState( OBJECT_MANIPULATION_TOPIC, ObjectManipulationAction, goal_key='object_manipulation_goal', input_keys=['standard_error'], output_keys=['standard_error']), transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted' })
def sm_best_vantage(): sm = smach.StateMachine(outcomes=['succeeded', 'aborted'], input_keys=['rfid_reads', 'tagid']) with sm: smach.StateMachine.add( 'SELECT_BEST_VANTAGE', BestVantage(), remapping={ 'tagid': 'tagid', # input (string) 'rfid_reads': 'rfid_reads', # input (RecorderReads) 'best_vantage': 'best_vantage', # output (PoseStamped) 'filtered_reads': 'filtered_reads' }, # output (RecorderReads) transitions={'succeeded': 'MOVE_BEST_VANTAGE'}) smach.StateMachine.add( 'MOVE_BEST_VANTAGE', SimpleActionState('/move_base', MoveBaseAction, goal_slots=['target_pose']), remapping={'target_pose': 'best_vantage'}, # input (PoseStamped) transitions={ 'aborted': 'SELECT_BEST_VANTAGE', 'preempted': 'aborted', 'succeeded': 'succeeded' }) return sm
def los_move(x, y, z=-0.5): goal = MoveGoal() goal.controller_name = 'LOS' goal.target_pose.position = Point(x, y, z) return SimpleActionState('move', MoveAction, goal=goal)
def execute(self, userdata): self._goal.planner_service_name = "ompl_planning/plan_kinematic_path" motion_plan_request = MotionPlanRequest() motion_plan_request.group_name = "SchunkArm" motion_plan_request.num_planning_attempts = 1 motion_plan_request.allowed_planning_time = rospy.Duration(5.0) motion_plan_request.planner_id = "" self._goal.motion_plan_request = motion_plan_request desired_pose = SimplePoseConstraint() desired_pose.header.frame_id = userdata.parent_frame desired_pose.link_name = "Gripper" desired_pose.pose.position.x = userdata.x desired_pose.pose.position.y = userdata.y desired_pose.pose.position.z = userdata.z quat = tf.transformations.quaternion_from_euler(-math.pi/2, math.pi/2, userdata.phi) desired_pose.pose.orientation = Quaternion(*quat) desired_pose.absolute_position_tolerance.x = 0.02 desired_pose.absolute_position_tolerance.y = 0.02 desired_pose.absolute_position_tolerance.z = 0.02 desired_pose.absolute_roll_tolerance = 0.04 desired_pose.absolute_pitch_tolerance = 0.04 desired_pose.absolute_yaw_tolerance = 0.04 add_goal_constraint_to_move_arm_goal(desired_pose, self._goal) srv_out = SimpleActionState.execute(self, userdata) return srv_out
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 get_nav_approach(self): def child_term_cb(outcome_map): return True def out_cb(outcome_map): if outcome_map['DETECT_FORWARD_DISTANCE'] == 'reached': return 'succeeded' return 'shutdown' sm_nav_approach_state = smach.Concurrence( outcomes=['succeeded', 'shutdown'], default_outcome='shutdown', child_termination_cb=child_term_cb, outcome_cb=out_cb, output_keys=['nav_dist']) with sm_nav_approach_state: approach_goal = ApproachGoal() approach_goal.forward_vel = 0.05 approach_goal.forward_mult = 0.50 smach.Concurrence.add( 'MOVE_FORWARD', SimpleActionState('/approach_table/move_forward_act', ApproachAction, goal=approach_goal)) smach.Concurrence.add( 'DETECT_FORWARD_DISTANCE', DetectForwardDistance(self.get_transform, self.nav_approach_dist)) return sm_nav_approach_state
def main(): rospy.init_node('move_gripper_client') parser = argparse.ArgumentParser( description='Client for Korus` move gripper action server') parser.add_argument('gripper_angle', type=float, help='gripper angle') args = parser.parse_args() sm = StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) sm.userdata.gripper_angle = args.gripper_angle with sm: def MoveGripperResultCb(userdata, status, result): rospy.loginfo('action finished in state ' + str(status)) rospy.loginfo('action result: ') rospy.loginfo(str(result)) smach.StateMachine.add( 'MoveGripper', SimpleActionState('/korus/move_gripper', MoveGripperAction, goal_slots=['gripper_angle'], result_cb=MoveGripperResultCb)) sm.execute()
def PatrolStateMachine(listOfWaypoints): tempWaypoint = () waypoints = [] #convert waypoints to ros topic format #waypoints.append([str(0), (listOfWaypoints[-1][1],listOfWaypoints[-1][0]), (0.0, 0.0, 0.0, 1.0)]) for i in range(0, len(listOfWaypoints), 1): tempWaypoint = (listOfWaypoints[i][1], listOfWaypoints[i][0]) waypoints.append([str(i), tempWaypoint, (0.0, 0.0, 0.0, 1.0)]) #start state machine patrol = StateMachine(['succeeded', 'aborted', 'preempted']) with patrol: for i, w in enumerate(waypoints): goal_pose = MoveBaseGoal() goal_pose.target_pose.header.frame_id = 'map' goal_pose.target_pose.pose.position.x = w[1][0] goal_pose.target_pose.pose.position.y = w[1][1] goal_pose.target_pose.pose.position.z = 0.0 goal_pose.target_pose.pose.orientation.x = w[2][0] goal_pose.target_pose.pose.orientation.y = w[2][1] goal_pose.target_pose.pose.orientation.z = w[2][2] goal_pose.target_pose.pose.orientation.w = w[2][3] StateMachine.add(w[0], SimpleActionState('move_base', MoveBaseAction, goal=goal_pose), transitions={'succeeded': waypoints[(i + 1) % \ len(waypoints)][0]}) patrol.execute()
def main(): rospy.init_node('smach_example_state_machine') # Create a SMACH state machine sm = smach.StateMachine(outcomes=['outcome4', 'outcome5']) # Open the container with sm: # Add states to the container smach.StateMachine.add('FOO', Foo(), transitions={ 'outcome1': 'BAR', 'outcome2': 'outcome4' }) smach.StateMachine.add('BAR', Bar(), transitions={'outcome2': 'FOO'}) def gripper_result_cb(userdata, status, result): return 'my_outcome' smach.StateMachine.add('TRIGGER_GRIPPER', SimpleActionState( 'action_server_namespace', GetMap, result_cb=gripper_result_cb, output_keys=['gripper_output']), transitions={'succeeded': 'APPROACH_PLUG'}, remapping={'gripper_output': 'userdata_output'}) # Execute SMACH plan outcome = sm.execute()
def main(): rospy.init_node('smach_example_state_machine') sm = StateMachine(['exit']) with sm: for key, (x, y, next_point) in WAYPOINTS.items(): goal = MoveBaseGoal() goal.target_pose.header.frame_id = 'map' goal.target_pose.pose.position.x = x goal.target_pose.pose.position.y = y goal.target_pose.pose.orientation.w = 1. StateMachine.add(key, SimpleActionState('move_base', MoveBaseAction, goal=goal), transitions={'succeeded': next_point, 'aborted': 'exit', 'preempted': 'exit'}) # Create and start the introspection server sis = IntrospectionServer('strat', sm, '/SM_ROOT') sis.start() # Execute the state machine outcome = sm.execute() # Wait for ctrl-c to stop the application rospy.spin() sis.stop()
def __init__(self, text=None, wait_before_speak=None): ''' If text is used, it will be that text which the robot will say. ''' self._text = text input_keys = [] if not text: input_keys = ['text'] self._wait_before_speak = wait_before_speak # Method to define the goal def tts_request_cb(ud, goal): tts_goal = SpeechWithFeedbackGoal() if (not self._text): tts_goal.say = ud.text else: tts_goal.say = self._text return tts_goal SimpleActionState.__init__(self, '/speech_action', SpeechWithFeedbackAction, input_keys=input_keys, goal_cb=tts_request_cb)
def arms_out_of_self_colision(self): self._print_title("ARMS OUT OF SELF COLISION") if self.using_the_robot: if self.__check_action(ARMS_ACTION_NAME) == aborted: return aborted else: self._print_warning("Not checking. ROS_MASTER_URI nor COMPUTER_NAME contains %s" % ROBOTS_NAME) return aborted robot = os.environ.get('PAL_ROBOT') ros_master_uri = os.environ.get('ROS_MASTER_URI') remotelly_executing = ros_master_uri.rfind('localhost') rospy.loginfo('remotelly_executing: %s' % remotelly_executing) # FIXME: Remove this line after test in the robot MOTION_FOLDER_PATH = '' plan = False checkSafety = False if robot == 'rh2c' or robot == 'rh2m' or robot == 'reemh3c' or robot == 'reemh3m' or robot == 'reemh3' or remotelly_executing == -1: check_for_door = True # FIXME: This variable is not being used. MOTION_FOLDER_PATH = "/mnt_flash/stacks/reem_alive/alive_engine/config/database/Stopped/interact_1.xml" if remotelly_executing == -1: MOTION_FOLDER_PATH = packages.get_pkg_dir('pal_smach_utils') + '/config/interact_1.xml' plan = True checkSafety = False motion_goal = MotionManagerGoal() motion_goal.plan = plan motion_goal.filename = MOTION_FOLDER_PATH motion_goal.checkSafety = checkSafety motion_goal.repeat = False motion_manager_action = SimpleActionState(ARMS_ACTION_NAME, MotionManagerAction, goal=motion_goal) userdata_hacked = {} status = motion_manager_action.execute(userdata_hacked) if status == succeeded: rospy.loginfo(self.colors.GREEN_BOLD + "Arms out of self colisin: OK " + self.colors.NATIVE_COLOR) else: self.ALL_OK = False rospy.loginfo(self.colors.RED_BOLD + "Arms out of self colisin: Failed " + self.colors.NATIVE_COLOR)
def __init__(self, path=None, frame_id='base_footprint'): # Private attributes self._path = path self._frame_id = frame_id # Method to define the goal def walk_path_goal_cb(userdata, goal): walk_path_goal = FollowPathGoal() path = Path() path.header.frame_id = self._frame_id path.header.stamp = rospy.Time.now() for sub in self._path: subgoal = PoseStamped() subgoal.header.frame_id = self._frame_id subgoal.header.stamp = rospy.Time.now() subgoal.pose.position = Point(sub[0], sub[1], 0.0) q = tf.transformations.quaternion_from_euler(0, 0, sub[2]) subgoal.pose.orientation = Quaternion(*q) path.poses.append(subgoal) walk_path_goal.path = path return walk_path_goal # Class constructor SimpleActionState.__init__(self, '/walk_path', FollowPathAction, goal_cb=walk_path_goal_cb)
def __init__(self, frame_id="/base_link", move_base_action_name=MOVE_BASE_ACTION_NAME, pose=None, goal_cb=None, goal_key=None, **kwargs): """Constructor for MoveActionState @type frame_id: string @param frame_id: The fame_id of the pose object. By default is "/base_link" @type move_base_action_name: string @param move_base_action_name: The action name. By default is the action name defined on the MOVE_BASE_ACTION_NAME variable. @type pose: geometry_msgs.Pose @param pose: The pose where the robot should move to on frame_id link. @type goal_cb: callable @param goal_cb: The function that will be called to get the pose. You can set only one of 'pose, goal_cb and goal_key' variables. @type goal_key: input_key on userdata. @param goal_key: The key on userdata variable that contains the pose. """ if (pose and goal_cb) or (pose and goal_key) or (goal_cb and goal_key): raise ValueError("You've set more than one of `pose', " \ "`goal_cb' and `goal_key'!") if not goal_cb and not goal_key and not pose: raise ValueError("Neither `pose' nor `goal_cb' nor `goal_key' were set!") if goal_key: if not 'input_keys' in kwargs: kwargs['input_keys'] = [] kwargs['input_keys'].append(goal_key) def generic_goal_cb(userdata, old_goal): """ Send a goal to the action """ self.nav_goal = MoveBaseGoal() self.nav_goal.target_pose.header.stamp = rospy.Time.now() self.nav_goal.target_pose.header.frame_id = frame_id if pose: self.nav_goal.target_pose.pose = pose return self.nav_goal elif goal_key: self.nav_goal.target_pose.pose = getattr(userdata, goal_key) return self.nav_goal else: self.nav_goal.target_pose.pose = Pose() return goal_cb(userdata, self.nav_goal) def _result_cb(userdata, status, result): """ This function will analize the status of the response If the status is aborted, will call the action again with the same orientation, but with the distance -0,5. If the status now is succeeded, then call the move_by with x=0.5 in front. :type status: actionlib.GoalStatus :type result: MoveBaseResult """ if status != GoalStatus.SUCCEEDED: rospy.loginfo(C.BACKGROUND_YELLOW + "Retrying go to the target goal" + C.NATIVE_COLOR) new_pose = pose_at_distance(self.nav_goal.target_pose.pose, DISTANCE_TO_RETRY ) self.nav_goal.target_pose.pose = new_pose self.nav_goal.target_pose.header.stamp = rospy.Time.now() move_action = SimpleActionState(move_base_action_name, MoveBaseAction, goal=self.nav_goal) result_status = move_action.execute(userdata) if result_status == succeeded: new_goal = MoveBaseGoal() new_goal.target_pose.header.frame_id = FRAME_BASE_LINK new_goal.target_pose.pose.position.x = DISTANCE_TO_RETRY new_goal.target_pose.header.stamp = rospy.Time.now() move_action = SimpleActionState(MOVE_BY_ACTION_NAME , MoveBaseAction, goal=new_goal) return move_action.execute(userdata) # THE PART BELOW WAS 'REMOVED' BECAUSE FINAL_APPROACH IS NOT RUNNING BY DEFAULT. # final_approach_goal = FinalApproachPoseRequest() # final_approach_goal.pose.position.x = DISTANCE_TO_RETRY - 0.1 # -0.1 Just because final_approach don't check if is secure to move # final_approach_goal.pose.orientation.w = 0.0 # rospy.loginfo("Calling /approachToGoal with message:\n" + str(final_approach_goal)) # final_approach = ServiceState('/approachToGoal', FinalApproachPose, request=final_approach_goal) # return final_approach.execute(userdata) SimpleActionState.__init__(self, move_base_action_name, MoveBaseAction, goal_cb=generic_goal_cb, result_cb=_result_cb, **kwargs)
def __init__(self, follow_joint_trajectory_topic = '/right_hand_controller/follow_joint_trajectory'): SimpleActionState.__init__(self, follow_joint_trajectory_topic, FollowJointTrajectoryAction, goal_cb=grasp_open_hand_goal_cb2, result_cb=grasp_hand_result_cb)
def __init__(self, frame="/map"): SimpleActionState.__init__( self, "move_base", MoveBaseAction, input_keys=["x", "y", "yaw"], goal_cb=self.__goal_cb ) self.frame = frame
def __init__(self, input_keys=[]): SimpleActionState.__init__(self, 'recharge', RechargeAction, goal_cb=self.goal_cb, input_keys=input_keys)
def __init__(self): SimpleActionState.__init__(self, 'move_SchunkArm', MoveArmAction, input_keys=['x', 'y', 'z', 'phi', 'parent_frame'])