class SceneGeometryActionClient(EventState): ''' Actionlib_comm_state is an actionlib state that will make a call to an action server. This is intended to serve as a template for future states that need to communicate with action server. Currently this is set up to communicate with the data collection action server and doesnt do anything other than return true or false. TODO: Proper error checking <= continue All actions completed, data collection started <= failed Data control failed to initialize or call something TODO: Proper error checking '''e def __init__(self, instruction): # See example_state.py for basic explanations. super(SceneGeometryActionClient, self).__init__(outcomes = ['completed', 'failed']) self._topic = 'scene_constraint_as' self._client = ProxyActionClient({self._topic: SceneConstraintAction}) self._instruction = instruction # It may happen that the action client fails to send the action goal. self._error = False def execute(self, userdata): # Check if the client failed to send the goal. if self._error: return 'failed' #otherwise get the result and perform next steps if(self._client.has_result(self._topic)): result = self._client.get_result(self._topic) status = result.constraint_result if(status == True): print("returned True") return 'completed' else: print("returned False") return 'failed' def on_enter(self, userdata): #Creating the goal to send goal = SceneConstraintGoal() goal.constraint_request = self._instruction #error checking in case communication cant be established try: self._client.send_goal(self._topic, goal) except Exception as e: Logger.logwarn('Failed to send the start data collection command:\n%s' % str(e)) self._error = True def on_exit(self, userdata): # Makes sure that the action is not running when leaving this state. if not self._client.has_result(self._topic): self._client.cancel(self._topic) Logger.loginfo('Cancelled active action goal.')
class StageActionClient(EventState): ''' stage_ac_state is an action client state that will make a call to an action server. This is intended to serve as a template for future states that need to communicate with action server. The topic given should correspond to an action server and it right now only works for StageAction action messages but in the future it will have any custom messages needed once they are more finalized. Currently just sends true or false to an action server and either returns complete or failed based on the response. TODO: Proper error checking other custom messages <= continue All actions completed, data collection started <= failed Data control failed to initialize or call something TODO: Proper error checking ''' def __init__(self, topic): # See example_state.py for basic explanations. super(StageActionClient, self).__init__(outcomes=['completed', 'failed']) self._topic = topic self._client = ProxyActionClient({self._topic: StageAction}) # It may happen that the action client fails to send the action goal. self._error = False def execute(self, userdata): # Check if the client failed to send the goal. if self._error: return 'failed' #otherwise get the result and perform next steps if (self._client.has_result(self._topic)): result = self._client.get_result(self._topic) status = result.result if (status == 0): print("Completed Succesfully") return 'completed' else: print("Error Thrown") return 'failed' def on_enter(self, userdata): #Creating the goal to send goal = StageGoal() goal.start = 0 #error checking in case communication cant be established try: self._client.send_goal(self._topic, goal) except Exception as e: Logger.logwarn( 'Failed to send the start data collection command:\n%s' % str(e)) self._error = True def on_exit(self, userdata): # Makes sure that the action is not running when leaving this state. if not self._client.has_result(self._topic): self._client.cancel(self._topic) Logger.loginfo('Cancelled active action goal.')
class TestRendezvousState(EventState): ''' This is a test actionlib state used for the rendezvous behavior -- goal_distance double The distance ># start Determin if the state should start from the beginning r if it needs to wait. <= continue If a result is recieved from the actionlib server <= failed Not really used at this point ''' def __init__(self, goal_distance): # Declaring outcomes, input keys by calling the super constructor with the corresponding arguments. super(TestRendezvousState, self).__init__(outcomes=['continue', 'failed']) # Pass the topic the actionlib server is listening on self._topic = 'test_rendezvous' self._client = ProxyActionClient({self._topic: rendezvousAction}) self._error = False def execute(self, userdata): # Check if the client failed to send the goal. if self._error: return 'failed' # Check if the action has been completed if self._client.has_result(self._topic): result = self._client.get_result(self._topic) Logger.loginfo('Rendezvous has talked') return 'continue' # If the action has not yet finished, no outcome will ne returned and will stay active. def on_enter(self, userdata): # When entering this state, we send the actoionlin server the goal once and then sit back and hope for the best # Create the goal msg. goal = rendezvousGoal() goal.goal_dist = 1 # Send the goal msg self._error = False try: self._client.send_goal(self._topic, goal) except Exception as e: Logger.logwarn('Failed to send the goal msg to server:\n%s' % str(e)) def on_exit(self, userdata): # Make sure that the action is not running when leabing this state. # A situation where the action would still be active is for example when the operator manually triggers an outcome. if not self._client.has_result(self._topic): self._client.cancel(self._topic) Loger.loginfo('Cancelled active action goal.')
class MetricSweepState(EventState): ''' Robot performs a metric sweep at its current location. -- sweep_type string Type of the sweep to do (select one of the provided options). <= sweeped Sweep has been completed. <= failed There has been an error when sweeping. ''' COMPLETE = 'complete' MEDIUM = 'medium' SHORT = 'short' SHORTEST = 'shortest' def __init__(self, sweep_type): super(MetricSweepState, self).__init__(outcomes = ['sweeped', 'failed']) self._sweep_type = sweep_type self._topic = '/do_sweep' self._client = ProxyActionClient({self._topic: SweepAction}) self._error = False def execute(self, userdata): if self._error: return 'failed' if self._client.has_result(self._topic): result = self._client.get_result(self._topic) if result.success: return 'sweeped' else: return 'failed' def on_enter(self, userdata): goal = SweepGoal() goal.type = self._sweep_type self._error = False try: self._client.send_goal(self._topic, goal) except Exception as e: Logger.logwarn('Failed to send the Sweep command:\n%s' % str(e)) self._error = True def on_exit(self, userdata): if not self._client.has_result(self._topic): self._client.cancel(self._topic) Logger.loginfo('Cancelled active action goal.')
class MoveCameraState(EventState): ''' Moves the camera. ># pan float Pan angle [-180, 180) ># tilt float Tilt angle [-41, 41] <= done Moved the camera. <= failed Cannot send the action goal. ''' def __init__(self): # See example_state.py for basic explanations. super(MoveCameraState, self).__init__(outcomes = ['done', 'failed'], input_keys = ['pan', 'tilt']) self._topic = 'SetPTUState' self._client = ProxyActionClient({self._topic: PtuGotoAction}) self._error = False def execute(self, userdata): if self._error: return 'failed' if self._client.has_result(self._topic): return 'done' def on_enter(self, userdata): goal = PtuGotoGoal() goal.pan = userdata.pan goal.tilt = userdata.tilt goal.pan_vel = 60 goal.tilt_vel = 60 self._error = False try: self._client.send_goal(self._topic, goal) except Exception as e: Logger.logwarn('Failed to send the camera movement command:\n%s' % str(e)) self._error = True def on_exit(self, userdata): # Make sure that the action is not running when leaving this state. # A situation where the action would still be active is for example when the operator manually triggers an outcome. if not self._client.has_result(self._topic): self._client.cancel(self._topic) Logger.loginfo('Cancelled active action goal.')
class MetricSweepState(EventState): ''' Robot performs a metric sweep at its current location. -- sweep_type string Type of the sweep to do (select one of the provided options). <= sweeped Sweep has been completed. <= failed There has been an error when sweeping. ''' COMPLETE = 'complete' MEDIUM = 'medium' SHORT = 'short' SHORTEST = 'shortest' def __init__(self, sweep_type): super(MetricSweepState, self).__init__(outcomes=['sweeped', 'failed']) self._sweep_type = sweep_type self._topic = '/do_sweep' self._client = ProxyActionClient({self._topic: SweepAction}) self._error = False def execute(self, userdata): if self._error: return 'failed' if self._client.has_result(self._topic): result = self._client.get_result(self._topic) if result.success: return 'sweeped' else: return 'failed' def on_enter(self, userdata): goal = SweepGoal() goal.type = self._sweep_type self._error = False try: self._client.send_goal(self._topic, goal) except Exception as e: Logger.logwarn('Failed to send the Sweep command:\n%s' % str(e)) self._error = True def on_exit(self, userdata): if not self._client.has_result(self._topic): self._client.cancel(self._topic) Logger.loginfo('Cancelled active action goal.')
class SpeechOutputEnglishParamTextState(EventState): ''' Lets the robot speak the string, set in Behaviour Parameters variable or inplace in the state GUI. -- text_to_speak string The text to be spoken. <= done Speaking has finished. <= failed Failed to execute speaking. ''' def __init__(self, text_to_speak="Hello World!"): super(SpeechOutputEnglishParamTextState, self).__init__(outcomes=['done', 'failed']) self._topic = '/sound_play' self._text_to_speak = text_to_speak self._client = ProxyActionClient({self._topic: SoundRequestAction}) self._error = False def execute(self, userdata): if self._error: return 'failed' if self._client.has_result(self._topic): return 'done' def on_enter(self, userdata): Logger.loginfo('Speech output, talking') goal = SoundRequestGoal() goal.sound_request.arg = self._text_to_speak goal.sound_request.command = SoundRequest.PLAY_ONCE goal.sound_request.sound = SoundRequest.SAY goal.sound_request.volume = 1.0 self._error = False try: self._client.send_goal(self._topic, goal) # Logger.logwarn('V try') except Exception as e: Logger.logwarn('Failed to send the Speech command:\n%s' % str(e)) self._error = True def on_exit(self, userdata): if not self._client.has_result(self._topic): self._client.cancel(self._topic) Logger.loginfo('Cancelled active action goal.')
class SpeechOutputState(EventState): ''' Lets the robot speak the given input string. ># text string Text to output. <= done Speaking has finished. <= failed Failed to execute speaking. ''' def __init__(self): super(SpeechOutputState, self).__init__(outcomes = ['done', 'failed'], input_keys = ['text']) self._topic = '/speak' self._client = ProxyActionClient({self._topic: maryttsAction}) self._error = False def execute(self, userdata): if self._error: return 'failed' if self._client.has_result(self._topic): return 'done' def on_enter(self, userdata): goal = maryttsGoal() goal.text = userdata.text print 'speech_output_state: say : ', goal.text self._error = False try: self._client.send_goal(self._topic, goal) except Exception as e: Logger.logwarn('Failed to send the Sweep command:\n%s' % str(e)) self._error = True def on_exit(self, userdata): if not self._client.has_result(self._topic): self._client.cancel(self._topic) Logger.loginfo('Cancelled active action goal.')
class SpeechOutputState(EventState): ''' Lets the robot speak the given input string. ># text string Text to output. <= done Speaking has finished. <= failed Failed to execute speaking. ''' def __init__(self): super(SpeechOutputState, self).__init__(outcomes = ['done', 'failed'], input_keys = ['text']) self._topic = '/speak' self._client = ProxyActionClient({self._topic: maryttsAction}) self._error = False def execute(self, userdata): if self._error: return 'failed' if self._client.has_result(self._topic): return 'done' def on_enter(self, userdata): goal = maryttsGoal() goal.text = userdata.text self._error = False try: self._client.send_goal(self._topic, goal) except Exception as e: Logger.logwarn('Failed to send the Sweep command:\n%s' % str(e)) self._error = True def on_exit(self, userdata): if not self._client.has_result(self._topic): self._client.cancel(self._topic) Logger.loginfo('Cancelled active action goal.')
class SpeechOutputViaSoundPlayState(EventState): ''' Lets the robot speak the given input string. ># text_IN string Text to output. <= done Speaking has finished. <= failed Failed to execute speaking. ''' def __init__(self): super(SpeechOutputViaSoundPlayState, self).__init__(outcomes=['done', 'failed'], input_keys=['text_IN']) self._topic = '/sound_play' self._client = ProxyActionClient({self._topic: SoundRequestAction}) self._error = False def execute(self, userdata): if self._error: return 'failed' if self._client.has_result(self._topic): return 'done' def on_enter(self, userdata): Logger.loginfo('Speech output, talking') goal = SoundRequestGoal() goal.sound_request.arg = userdata.text_IN goal.sound_request.command = SoundRequest.PLAY_ONCE goal.sound_request.sound = SoundRequest.SAY goal.sound_request.volume = 1.0 self._error = False try: self._client.send_goal(self._topic, goal) # Logger.logwarn('V try') except Exception as e: Logger.logwarn('Failed to send the Speech command:\n%s' % str(e)) self._error = True def on_exit(self, userdata): if not self._client.has_result(self._topic): self._client.cancel(self._topic) Logger.loginfo('Cancelled active action goal.')
class NavigateState(EventState): ''' Navigate to waypoints. ># target_pose Dict[str,float] Pose to navigate to, a dictionary with the keys 'x', 'y', and 'frame_id' <= ok Navigation complete with no errors <= err Navigation errors were encountered ''' def __init__(self): super(NavigateState, self).__init__(outcomes=['ok', 'err'], input_keys=['target_pose']) self._topic = 'move_base' self._client = ProxyActionClient({self._topic: MoveBaseAction}) self._error = False def execute(self, ud): if self._error: return 'err' if self._client.has_result(self._topic): status = self._client.get_state(self._topic) # type: GoalStatus return 'ok' if status == GoalStatus.SUCCEEDED else 'err' def on_enter(self, userdata): goal = MoveBaseGoal() goal.target_pose = PoseStamped() goal.target_pose.header.frame_id = userdata.target_pose['frame_id'] goal.target_pose.pose.position.x = userdata.target_pose['x'] goal.target_pose.pose.position.y = userdata.target_pose['y'] goal.target_pose.pose.orientation.w = 1.0 self._error = False try: self._client.send_goal(self._topic, goal) except Exception as err: Logger.logwarn( 'Could not send nav goal to move_base:\n{}'.format(err)) self._error = True def on_exit(self, userdata): if not self._client.has_result(self._topic): self._client.cancel(self._topic)
class SpeechOutputState(EventState): """ Lets the robot speak the given input string. ># text string Text to output. <= done Speaking has finished. <= failed Failed to execute speaking. """ def __init__(self): super(SpeechOutputState, self).__init__(outcomes=["done", "failed"], input_keys=["text"]) self._topic = "/speak" self._client = ProxyActionClient({self._topic: maryttsAction}) self._error = False def execute(self, userdata): if self._error: return "failed" if self._client.has_result(self._topic): return "done" def on_enter(self, userdata): goal = maryttsGoal() goal.text = userdata.text Logger.loginfo("Speech output, saying:\n%s" % goal.text) self._error = False try: self._client.send_goal(self._topic, goal) except Exception as e: Logger.logwarn("Failed to send the Sweep command:\n%s" % str(e)) self._error = True def on_exit(self, userdata): if not self._client.has_result(self._topic): self._client.cancel(self._topic) Logger.loginfo("Cancelled active action goal.")
def test_action_client(self): t1 = '/action_1' server = None def execute_cb(goal): rospy.sleep(.5) if server.is_preempt_requested(): server.set_preempted() else: server.set_succeeded(BehaviorExecutionResult(outcome='ok')) server = actionlib.SimpleActionServer(t1, BehaviorExecutionAction, execute_cb, auto_start=False) server.start() client = ProxyActionClient({t1: BehaviorExecutionAction}) self.assertFalse(client.has_result(t1)) client.send_goal(t1, BehaviorExecutionGoal()) rate = rospy.Rate(20) for i in range(20): self.assertTrue(client.is_active(t1) or client.has_result(t1)) rate.sleep() self.assertTrue(client.has_result(t1)) result = client.get_result(t1) self.assertEqual(result.outcome, 'ok') client.send_goal(t1, BehaviorExecutionGoal()) rospy.sleep(.1) client.cancel(t1) rospy.sleep(.9) self.assertFalse(client.is_active(t1)) self.assertFalse(client.is_available('/not_there')) client = ProxyActionClient({'/invalid': BehaviorExecutionAction}, wait_duration=.1) self.assertFalse(client.is_available('/invalid'))
class BrickGraspingActionState(EventState): """ brick grasping results """ RESULT_SUCCESS = 0 RESULT_STOPPED_BY_COMMAND = 1 RESULT_OBJECT_NOT_RELIABLE = 2 RESULT_OBJECT_NOT_VISIBLE = 3 RESULT_GRASPING_TIMOUT = 4 RESULT_GRASPING_FAILED = 5 """ brick grasping object types """ OBJECT_RED = 1 OBJECT_GREEN = 2 OBJECT_BLUE = 3 OBJECT_ORANGE = 4 WALL = 5 """ result ids """ RESULT_SUCCESS = 0 RESULT_STOPPED_BY_COMMAND = 1 RESULT_OBJECT_NOT_RELIABLE = 2 RESULT_OBJECT_NOT_VISIBLE = 3 RESULT_GRASPING_TIMOUT = 4 RESULT_GRASPING_FAILED = 5 def __init__(self, return_altitude): # See example_state.py for basic explanations. super(BrickGraspingActionState, self).__init__(outcomes=['grasped', 'grasping_error'], input_keys=['brick_type']) self._topic = 'brick_grasping' self._client = ProxyActionClient( {self._topic: GraspingAction}) # pass required clients as dict (topic: type) self._return_altitude = return_altitude Logger.loginfo('[GraspingAction]: Client initialized') # It may happen that the action client fails to send the action goal. self._error = False def execute(self, userdata): # While this state is active, check if the action has been finished and evaluate the result. # Check if the client failed to send the goal. if self._error: return 'grasping_error' # Check if the action has been finished if self._client.has_result(self._topic): result = self._client.get_result(self._topic) Logger.loginfo("[GraspingAction]: ended %s" % str(result.message)) # In this example, we also provide the amount of cleaned dishes as output key. if result.success: return 'grasped' else: return 'grasping_error' """feedback params are""" feedback = self._client.get_feedback(self._topic) if feedback is not None: rospy.loginfo_throttle( 5, "[GraspingAction]: Current feedback msg: %s" % str(feedback.message)) def on_enter(self, userdata): # When entering this state, we send the action goal once to let the robot start its work. brick_type = userdata.brick_type # Create the goal. goal = GraspingGoal() #fill the goal Logger.loginfo("[GraspingAction]: want to grasp brick type %s" % (str(brick_type))) goal.goal = brick_type goal.return_altitude = self._return_altitude #where to return after grasping # Send the goal. self._error = False # make sure to reset the error state since a previous state execution might have failed try: Logger.loginfo("[GraspingAction]: sending goal %s to topic %s" % (str(goal), str(self._topic))) self._client.send_goal(self._topic, goal) except Exception as e: # Since a state failure not necessarily causes a behavior failure, it is recommended to only print warnings, not errors. # Using a linebreak before appending the error log enables the operator to collapse details in the GUI. Logger.logwarn( '[GraspingAction]: Failed to send the GraspingAction command:\n%s' % str(e)) self._error = True def on_exit(self, userdata): # Make sure that the action is not running when leaving this state. # A situation where the action would still be active is for example when the operator manually triggers an outcome. if not self._client.has_result(self._topic): self._client.cancel(self._topic) Logger.loginfo('[GraspingAction]: Cancelled active action goal.')
class TweetPictureState(EventState): ''' State for tweeting a picture and text ''' def __init__(self): # See example_state.py for basic explanations. super(TweetPictureState, self).__init__( outcomes=['picture_tweeted', 'tweeting_failed', 'command_error'], input_keys=['picture_path', 'tweet_text']) # Create the action client when building the behavior. # This will cause the behavior to wait for the client before starting execution # and will trigger a timeout error if it is not available. # Using the proxy client provides asynchronous access to the result and status # and makes sure only one client is used, no matter how often this state is used in a behavior. self._topic = 'strands_tweets' self._client = ProxyActionClient( {self._topic: SendTweetAction}) # pass required clients as dict (topic: type) # It may happen that the action client fails to send the action goal. self._error = False def execute(self, userdata): # While this state is active, check if the action has been finished and evaluate the result. # Check if the client failed to send the goal. if self._error: return 'command_error' # Check if the action has been finished if self._client.has_result(self._topic): result = self._client.get_result(self._topic) if result.success: return 'picture_tweeted' else: return 'tweeting_failed' # If the action has not yet finished, no outcome will be returned and the state stays active. def on_enter(self, userdata): # When entering this state, we send the action goal once to let the robot start its work. # As documented above, we get the specification of which dishwasher to use as input key. # This enables a previous state to make this decision during runtime and provide the ID as its own output key. # Create the goal. tweet = SendTweetGoal() tweet.text = userdata.tweet_text if len(userdata.tweet_text) > 140: tweet.text = '#LAMoR15 #ECMR15 I just told a too looong joke, stupid tweet length!' tweet.with_photo = True img = cv2.imread(userdata.picture_path) bridge = CvBridge() image_message = bridge.cv2_to_imgmsg(img, encoding="bgr8") tweet.photo = image_message try: self._client.send_goal(self._topic, tweet) except Exception as e: # Since a state failure not necessarily causes a behavior failure, it is recommended to only print warnings, not errors. # Using a linebreak before appending the error log enables the operator to collapse details in the GUI. Logger.logwarn( 'Failed to send the TweetPictureState command:\n%s' % str(e)) self._error = True def on_exit(self, userdata): # Make sure that the action is not running when leaving this state. # A situation where the action would still be active is for example when the operator manually triggers an outcome. if not self._client.has_result(self._topic): self._client.cancel(self._topic) Logger.loginfo('Cancelled active action goal.')
class TestActionState(EventState): ''' This is a test actionlib state to see how to actually make our own actionlib states that call an actionlib server using the state as a client. I will try to keep documentation but no promises. This state takes doesn't take any input and just goes to finish once it recieves the goal from the server. <= communicated_with_server Was able to connect and communicate with the actionlib server. <= no_communication_with_server Was not able to connect and communicate with the actionlib server. ''' def __init__(self): # See example_state.py for basic explanations. super(TestActionState, self).__init__(outcomes=[ 'communicated_with_server', 'no_communication_with_server' ]) # Create the action client when building the behavior. # This will cause the behavior to wait for the client before starting execution # and will trigger a timeout error if it is not available. # Using the proxy client provides asynchronous access to the result and status # and makes sure only one client is used, no matter how often this state is used in a behavior. self._topic = 'test_action' self._client = ProxyActionClient( {self._topic: testAction}) # pass required clients as dict (topic: type) # It may happen that the action client fails to send the action goal. self._error = False def execute(self, userdata): # Check if the client failed to send the goal. if self._error: return 'no_communication_with_server' # Check if the action has been finished if self._client.has_result(self._topic): result = self._client.get_result(self._topic) Logger.loginfo('the result is %s' % result) return 'communicated_with_server' # If the action has not yet finished, no outcome will be returned and the state stays active. def on_enter(self, userdata): # When entering this state, we send the action goal once to let the robot start its work. # Create the goal. goal = testGoal() goal.test_goal = "test test" # Send the goal. self._error = False # make sure to reset the error state since a previous state execution might have failed try: self._client.send_goal(self._topic, goal) except Exception as e: # Since a state failure not necessarily causes a behavior failure, it is recommended to only print warnings, not errors. # Using a linebreak before appending the error log enables the operator to collapse details in the GUI. Logger.logwarn('Failed to send the test_goal command:\n%s' % str(e)) self._error = True def on_exit(self, userdata): # Make sure that the action is not running when leaving this state. # A situation where the action would still be active is for example when the operator manually triggers an outcome. if not self._client.has_result(self._topic): self._client.cancel(self._topic) Logger.loginfo('Cancelled active action goal.')
class MoveBaseState(EventState): """ Navigates a robot to a desired position and orientation using move_base. ># waypoint Pose2D Target waypoint for navigation. <= arrived Navigation to target pose succeeded. <= failed Navigation to target pose failed. """ def __init__(self): """Constructor""" super(MoveBaseState, self).__init__(outcomes=['arrived', 'failed'], input_keys=['waypoint']) self._action_topic = "/move_base" self._client = ProxyActionClient({self._action_topic: MoveBaseAction}) self._arrived = False self._failed = False def execute(self, userdata): """Wait for action result and return outcome accordingly""" if self._arrived: return 'arrived' if self._failed: return 'failed' if self._client.has_result(self._action_topic): status = self._client.get_state(self._action_topic) if status == GoalStatus.SUCCEEDED: self._arrived = True return 'arrived' elif status in [ GoalStatus.PREEMPTED, GoalStatus.REJECTED, GoalStatus.RECALLED, GoalStatus.ABORTED ]: Logger.logwarn('Navigation failed: %s' % str(status)) self._failed = True return 'failed' def on_enter(self, userdata): """Create and send action goal""" self._arrived = False self._failed = False # Create and populate action goal goal = MoveBaseGoal() pt = Point(x=userdata.waypoint.x, y=userdata.waypoint.y) qt = transformations.quaternion_from_euler(0, 0, userdata.waypoint.theta) goal.target_pose.pose = Pose(position=pt, orientation=Quaternion(*qt)) goal.target_pose.header.frame_id = "odom" # goal.target_pose.header.stamp.secs = 5.0 # Send the action goal for execution try: self._client.send_goal(self._action_topic, goal) except Exception as e: Logger.logwarn("Unable to send navigation action goal:\n%s" % str(e)) self._failed = True def cancel_active_goals(self): if self._client.is_available(self._action_topic): if self._client.is_active(self._action_topic): if not self._client.has_result(self._action_topic): self._client.cancel(self._action_topic) Logger.loginfo('Cancelled move_base active action goal.') def on_exit(self, userdata): self.cancel_active_goals() def on_stop(self): self.cancel_active_goals()
class SaraFollow(EventState): """ Make Sara follow an entity using move_base. -- Distance float distance to keep between sara and the entity -- ReplanPeriod float Time in seconds between each replanning for movebase ># ID int entity ID. """ def __init__(self, distance=1, ReplanPeriod=0.5): """Constructor""" super(SaraFollow, self).__init__(outcomes=['failed'], input_keys=['ID']) # Prepare the action client for move_base self._action_topic = "/move_base" self._client = ProxyActionClient({self._action_topic: MoveBaseAction}) # Initialise the subscribers self.entities_topic = "/entities" self._pose_topic = "/robot_pose" self._sub = ProxySubscriberCached({ self._pose_topic: Pose, self.entities_topic: Entities }) # Get the parameters self.targetDistance = distance self.ReplanPeriod = ReplanPeriod # Initialise the status variables self.MyPose = None self.lastPlanTime = 0 def execute(self, userdata): """Wait for action result and return outcome accordingly""" # Initialise the Entity Entity = None # Get my last position if self._sub.has_msg(self._pose_topic): self.MyPose = self._sub.get_last_msg(self._pose_topic) # Get the entity's position if self._sub.has_msg(self.entities_topic): message = self._sub.get_last_msg(self.entities_topic) for entity in message.entities: if entity.ID == userdata.ID: Entity = entity # If both my pose and the entyties are known, we start following. if self.MyPose and Entity: # Calculate a new goal if the desired period has passed. if self.lastPlanTime + self.ReplanPeriod < rospy.get_time(): self.lastPlanTime = rospy.get_time() self.setGoal( self.generateGoal(self.MyPose.position, Entity.position)) # Check if movebase can reach the goal if self._client.has_result(self._action_topic): status = self._client.get_state(self._action_topic) if status in [ GoalStatus.PREEMPTED, GoalStatus.REJECTED, GoalStatus.RECALLED, GoalStatus.ABORTED ]: Logger.logwarn('Navigation failed: %s' % str(status)) return 'failed' def on_enter(self, userdata): """Clean the costmap""" rospy.wait_for_service('/move_base/clear_costmaps') serv = rospy.ServiceProxy('/move_base/clear_costmaps', Empty) serv() self.MyPose = None self.Entity = None self.Counter = 0 def on_exit(self, userdata): if not self._client.has_result(self._action_topic): self._client.cancel(self._action_topic) Logger.loginfo('Cancelled active action goal.') def on_pause(self): pose = self._client.get_feedback(self._action_topic) self.setGoal(pose) def generateGoal(self, MyPosition, EntityPosition): # Calculate the safe distance to reach GoalPose = Pose() # Calculate the diference in position dx = EntityPosition.x - MyPosition.x dy = EntityPosition.y - MyPosition.y # Calculate the desired distance to the person. # If the person is too close, the robot must not go backward. distanceToPerson = max(self.targetDistance, (dx**2 + dy**2)**0.5) # Calculate the desired position to reach. GoalPose.position.x = EntityPosition.x - dx / distanceToPerson * self.targetDistance GoalPose.position.y = EntityPosition.y - dy / distanceToPerson * self.targetDistance # Calculate the desired direction. angle = math.atan2(dy, dx) qt = quaternion_from_euler(0, 0, angle) GoalPose.orientation.w = qt[3] GoalPose.orientation.x = qt[0] GoalPose.orientation.y = qt[1] GoalPose.orientation.z = qt[2] return GoalPose def setGoal(self, pose): # Generate the goal goal = MoveBaseGoal() goal.target_pose.pose = pose goal.target_pose.header.frame_id = "map" # Send the action goal for execution try: Logger.loginfo("sending goal" + str(goal)) self._client.send_goal(self._action_topic, goal) except Exception as e: Logger.logwarn("Unable to send navigation action goal:\n%s" % str(e)) self._failed = True
class MoveToWaypointState(EventState): ''' Lets the robot move to a given waypoint. ># waypoint PoseStamped Specifies the waypoint to which the robot should move. ># victim string object_id of detected object ># speed Speed of the robot <= reached Robot is now located at the specified waypoint. <= failed Failed to send a motion request to the action server. <= update Update the pose of current waypoint ''' def __init__(self): ''' Constructor ''' super(MoveToWaypointState, self).__init__(outcomes=['reached', 'failed', 'update'], input_keys=['waypoint','victim','speed']) self._action_topic = '/move_base' self._move_client = ProxyActionClient({self._action_topic: MoveBaseAction}) self.set_tolerance = rospy.ServiceProxy('/controller/set_alternative_tolerances', SetAlternativeTolerance) self._failed = False self._reached = False def execute(self, userdata): ''' Execute this state ''' if self._failed: return 'failed' if self._reached: return 'reached' if self._move_client.has_result(self._action_topic): result = self._move_client.get_result(self._action_topic) if result.result == 1: self._reached = True return 'reached' else: self._failed = True Logger.logwarn('Failed to reach waypoint!') return 'failed' self._currentTime = Time.now() if self._currentTime.secs - self._startTime.secs >= 10: return 'update' def on_enter(self, userdata): self._startTime = Time.now() self._failed = False self._reached = False goal_id = GoalID() goal_id.id = 'abcd' goal_id.stamp = Time.now() action_goal = MoveBaseGoal() action_goal.target_pose = userdata.waypoint action_goal.speed = userdata.speed if action_goal.target_pose.header.frame_id == "": action_goal.target_pose.header.frame_id = "world" try: self._move_client.send_goal(self._action_topic, action_goal) resp = self.set_tolerance(goal_id, 0.2, 1.55) except Exception as e: Logger.logwarn('Failed to send motion request to waypoint (%(x).3f, %(y).3f):\n%(err)s' % { 'err': str(e), 'x': userdata.waypoint.pose.position.x, 'y': userdata.waypoint.pose.position.y }) self._failed = True Logger.loginfo('Driving to next waypoint') def on_stop(self): try: if self._move_client.is_available(self._action_topic) \ and not self._move_client.has_result(self._action_topic): self._move_client.cancel(self._action_topic) except: # client already closed pass def on_exit(self, userdata): try: if self._move_client.is_available(self._action_topic) \ and not self._move_client.has_result(self._action_topic): self._move_client.cancel(self._action_topic) except: # client already closed pass def on_pause(self): self._move_client.cancel(self._action_topic) def on_resume(self, userdata): self.on_enter(userdata)
class SrdfStateToMoveit(EventState): ''' State to look up a pre-defined joint configuration from the SRDF file loaded in the parameter server (/robot_description_semantic) and send it to MoveIt to plan and move. -- config_name string Name of the joint configuration of interest. -- move_group string Name of the move group to be used for planning. -- action_topic string Topic on which MoveIt is listening for action calls. -- robot_name string Optional name of the robot to be used. If left empty, the first one found will be used (only required if multiple robots are specified in the same file). ># joint_values float[] Target configuration of the joints. Same order as their corresponding names in joint_names. <= reached Target joint configuration has been reached. <= planning_failed Failed to find a plan to the given joint configuration. <= control_failed Failed to move the arm along the planned trajectory. ''' def __init__(self, config_name, move_group="", action_topic='/move_group', robot_name=""): ''' Constructor ''' super(SrdfStateToMoveit, self).__init__(outcomes=[ 'reached', 'planning_failed', 'control_failed', 'param_error' ], output_keys=[ 'config_name', 'move_group', 'robot_name', 'action_topic', 'joint_values', 'joint_names' ]) self._config_name = config_name self._move_group = move_group self._robot_name = robot_name self._action_topic = action_topic self._client = ProxyActionClient({self._action_topic: MoveGroupAction}) self._planning_failed = False self._control_failed = False self._success = False self._srdf_param = None if rospy.has_param("/robot_description_semantic"): self._srdf_param = rospy.get_param("/robot_description_semantic") else: Logger.logerr( 'Unable to get parameter: /robot_description_semantic') self._param_error = False self._srdf = None def execute(self, userdata): if self._param_error: return 'param_error' ''' Execute this state ''' if self._planning_failed: return 'planning_failed' if self._control_failed: return 'control_failed' if self._success: return 'reached' if self._client.has_result(self._action_topic): result = self._client.get_result(self._action_topic) if result.error_code.val == MoveItErrorCodes.CONTROL_FAILED: Logger.logwarn( 'Control failed for move action of group: %s (error code: %s)' % (self._move_group, str(result.error_code))) self._control_failed = True return 'control_failed' elif result.error_code.val != MoveItErrorCodes.SUCCESS: Logger.logwarn( 'Move action failed with result error code: %s' % str(result.error_code)) self._planning_failed = True return 'planning_failed' else: self._success = True return 'reached' def on_enter(self, userdata): self._param_error = False self._planning_failed = False self._control_failed = False self._success = False #Parameter check if self._srdf_param is None: self._param_error = True return try: self._srdf = ET.fromstring(self._srdf_param) except Exception as e: Logger.logwarn( 'Unable to parse given SRDF parameter: /robot_description_semantic' ) self._param_error = True if not self._param_error: robot = None for r in self._srdf.iter('robot'): if self._robot_name == '' or self._robot_name == r.attrib[ 'name']: robot = r userdata.robot_name = robot # Save robot name to output key break if robot is None: Logger.logwarn('Did not find robot name in SRDF: %s' % self._robot_name) return 'param_error' config = None for c in robot.iter('group_state'): if (self._move_group == '' or self._move_group == c.attrib['group']) \ and c.attrib['name'] == self._config_name: config = c self._move_group = c.attrib[ 'group'] # Set move group name in case it was not defined userdata.config_name = config # Save configuration name to output key userdata.move_group = self._move_group # Save move_group to output key break if config is None: Logger.logwarn('Did not find config name in SRDF: %s' % self._config_name) return 'param_error' try: self._joint_config = [ float(j.attrib['value']) for j in config.iter('joint') ] self._joint_names = [ str(j.attrib['name']) for j in config.iter('joint') ] userdata.joint_values = self._joint_config # Save joint configuration to output key userdata.joint_names = self._joint_names # Save joint names to output key except Exception as e: Logger.logwarn('Unable to parse joint values from SRDF:\n%s' % str(e)) return 'param_error' # Action Initialization action_goal = MoveGroupGoal() action_goal.request.group_name = self._move_group action_goal.request.allowed_planning_time = 1.0 goal_constraints = Constraints() for i in range(len(self._joint_names)): goal_constraints.joint_constraints.append( JointConstraint(joint_name=self._joint_names[i], position=self._joint_config[i], weight=1.0)) action_goal.request.goal_constraints.append(goal_constraints) try: self._client.send_goal(self._action_topic, action_goal) userdata.action_topic = self._action_topic # Save action topic to output key except Exception as e: Logger.logwarn('Failed to send action goal for group: %s\n%s' % (self._move_group, str(e))) self._planning_failed = True def on_stop(self): try: if self._client.is_available(self._action_topic) \ and not self._client.has_result(self._action_topic): self._client.cancel(self._action_topic) except: # client already closed pass def on_pause(self): self._client.cancel(self._action_topic) def on_resume(self, userdata): self.on_enter(userdata)
class MotionServiceState(EventState): """Implements a state where a certain motion is performed. This state can be used to execute motions created by the motion editor. -- motion_key string Reference to the motion to be executed. -- time_factor float Factor to multiply with the default execution time of the specified motion. For example, 2 will result in a motion twice as long, thus executed at half speed. <= done Indicates that the expected execution time of the motion has passed. The motion service provides no way to check if a motion has actually finished. <= failed Indicates that the requested motion doesn't exist and therefore wasn't executed """ def __init__(self, motion_key, time_factor=1): """ Constructor """ super(MotionServiceState, self).__init__(outcomes=['done', 'failed']) self.motion_key = motion_key self.time_factor = time_factor self._finish_time = None self._motion_goal_ns = '/motion_service/motion_goal' self._client = ProxyActionClient({self._motion_goal_ns: ExecuteMotionAction}) self._failed = False self._done = False def execute(self, userdata): """ Execute this state """ if self._failed: return 'failed' if self._done: return 'done' if self._client.has_result(self._motion_goal_ns): result = self._client.get_result(self._motion_goal_ns) if result is None: # Bug in actionlib, sometimes returns None instead of result # Operator decision needed Logger.logwarn("Failed to execute the motion '%s':\nAction result is None" % self.motion_key) self._failed = True return 'failed' if result.error_string is None or len(result.error_code) == 0: Logger.logwarn("Failed to execute the motion '%s':\nAction result is None" % self.motion_key) self._failed = True return 'failed' success = all(lambda x: x == 0, result.error_code) if success: rospy.loginfo('Trajectory finished successfully.') # dont need to send this to the operator self._done = True return 'done' else: Logger.logwarn("Failed to execute the motion '%s':\n%s" % (self.motion_key, str(result.error_code))) self._failed = True return 'failed' def on_enter(self, userdata): self._failed = False self._done = False # build goal goal = ExecuteMotionGoal() goal.motion_name = self.motion_key goal.time_factor = self.time_factor try: self._client.send_goal(self._motion_goal_ns, goal) except Exception as e: Logger.logwarn("Failed sending motion goal for '%s':\n%s" % (self.motion_key, str(e))) def on_exit(self, userdata): if not self._client.has_result(self._motion_goal_ns): self._client.cancel(self._motion_goal_ns) Logger.loginfo("Cancelled active motion goal.")
class MoveBaseState(EventState): """ Navigates a robot to a desired position and orientation using move_base. ># waypoint Pose2D Target waypoint for navigation. <= arrived Navigation to target pose succeeded. <= failed Navigation to target pose failed. """ def __init__(self): """Constructor""" super(MoveBaseState, self).__init__(outcomes = ['arrived', 'failed'], input_keys = ['waypoint']) self._action_topic = "/move_base" self._client = ProxyActionClient({self._action_topic: MoveBaseAction}) self._arrived = False self._failed = False def execute(self, userdata): """Wait for action result and return outcome accordingly""" if self._arrived: return 'arrived' if self._failed: return 'failed' if self._client.has_result(self._action_topic): status = self._client.get_state(self._action_topic) if status == GoalStatus.SUCCEEDED: self._arrived = True return 'arrived' elif status in [GoalStatus.PREEMPTED, GoalStatus.REJECTED, GoalStatus.RECALLED, GoalStatus.ABORTED]: Logger.logwarn('Navigation failed: %s' % str(status)) self._failed = True return 'failed' def on_enter(self, userdata): """Create and send action goal""" self._arrived = False self._failed = False # Create and populate action goal goal = MoveBaseGoal() pt = Point(x = userdata.waypoint.x, y = userdata.waypoint.y) qt = transformations.quaternion_from_euler(0, 0, userdata.waypoint.theta) goal.target_pose.pose = Pose(position = pt, orientation = Quaternion(*qt)) goal.target_pose.header.frame_id = "odom" # goal.target_pose.header.stamp.secs = 5.0 # Send the action goal for execution try: self._client.send_goal(self._action_topic, goal) except Exception as e: Logger.logwarn("Unable to send navigation action goal:\n%s" % str(e)) self._failed = True def on_exit(self, userdata): if not self._client.has_result(self._action_topic): self._client.cancel(self._action_topic) Logger.loginfo('Cancelled active action goal.')
class ExampleActionState(EventState): """ Actionlib actions are the most common basis for state implementations since they provide a non-blocking, high-level interface for robot capabilities. The example is based on the DoDishes-example of actionlib (see http://wiki.ros.org/actionlib). This time we have input and output keys in order to specify the goal and possibly further evaluate the result in a later state. -- dishes_to_do int Expected amount of dishes to be cleaned. ># dishwasher int ID of the dishwasher to be used. #> cleaned int Amount of cleaned dishes. <= cleaned_some Only a few dishes have been cleaned. <= cleaned_enough Cleaned a lot of dishes. <= command_error Cannot send the action goal. """ def __init__(self, dishes_to_do): # See example_state.py for basic explanations. super(ExampleActionState, self).__init__( outcomes=["cleaned_some", "cleaned_enough", "command_error"], input_keys=["dishwasher"], output_keys=["cleaned"], ) self._dishes_to_do = dishes_to_do # Create the action client when building the behavior. # This will cause the behavior to wait for the client before starting execution # and will trigger a timeout error if it is not available. # Using the proxy client provides asynchronous access to the result and status # and makes sure only one client is used, no matter how often this state is used in a behavior. self._topic = "do_dishes" self._client = ProxyActionClient({self._topic: DoDishesAction}) # pass required clients as dict (topic: type) # It may happen that the action client fails to send the action goal. self._error = False def execute(self, userdata): # While this state is active, check if the action has been finished and evaluate the result. # Check if the client failed to send the goal. if self._error: return "command_error" # Check if the action has been finished if self._client.has_result(self._topic): result = self._client.get_result(self._topic) dishes_cleaned = result.total_dishes_cleaned # In this example, we also provide the amount of cleaned dishes as output key. userdata.cleaned = dishes_cleaned # Based on the result, decide which outcome to trigger. if dishes_cleaned > self._dishes_to_do: return "cleaned_enough" else: return "cleaned_some" # If the action has not yet finished, no outcome will be returned and the state stays active. def on_enter(self, userdata): # When entering this state, we send the action goal once to let the robot start its work. # As documented above, we get the specification of which dishwasher to use as input key. # This enables a previous state to make this decision during runtime and provide the ID as its own output key. dishwasher_id = userdata.dishwasher # Create the goal. goal = DoDishesGoal() goal.dishwasher_id = dishwasher_id # Send the goal. self._error = False # make sure to reset the error state since a previous state execution might have failed try: self._client.send_goal(self._topic, goal) except Exception as e: # Since a state failure not necessarily causes a behavior failure, it is recommended to only print warnings, not errors. # Using a linebreak before appending the error log enables the operator to collapse details in the GUI. Logger.logwarn("Failed to send the DoDishes command:\n%s" % str(e)) self._error = True def on_exit(self, userdata): # Make sure that the action is not running when leaving this state. # A situation where the action would still be active is for example when the operator manually triggers an outcome. if not self._client.has_result(self._topic): self._client.cancel(self._topic) Logger.loginfo("Cancelled active action goal.")
class GripperCommandState(EventState): """ Executes a custom trajectory. -- gripper_cmd int Identifier of the pre-defined commands. The state's class variables can be used here. -- time float Relative time in seconds from starting the execution to when the corresponding target gripper state should be reached. <= done Gripper command was successfully executed. <= failed Failed to send or execute gripper command. """ CLOSE_GRIPPER = 0 OPEN_GRIPPER = 1 def __init__(self, gripper_cmd, time=5.0): """Constructor""" super(GripperCommandState, self).__init__(outcomes=["done", "failed"]) self._joint_names = ["gripper_finger_joint_l", "gripper_finger_joint_r"] self._gripper_joint_positions = { # gripper_finger_joint_l, gripper_finger_joint_r 0: [0.001, 0.001], 1: [0.010, 0.010], # NOTE: 0.011 results in goal tolerance violation } self._gripper_cmd = gripper_cmd self._time = time self._action_topic = "/arm_1/gripper_controller/follow_joint_trajectory" self._client = ProxyActionClient({self._action_topic: FollowJointTrajectoryAction}) self._done = False self._failed = False def execute(self, userdata): """Wait for action result and return outcome accordingly""" if self._done: return "done" if self._failed: return "failed" if self._client.has_result(self._action_topic): result = self._client.get_result(self._action_topic) if result.error_code == FollowJointTrajectoryResult.SUCCESSFUL: self._done = True return "done" elif result.error_code == FollowJointTrajectoryResult.GOAL_TOLERANCE_VIOLATED: Logger.logwarn("Probably done, but goal tolerances violated (%d)" % result.error_code) self._done = True return "done" else: Logger.logwarn( "Gripper trajectory failed to execute: (%d) %s" % (result.error_code, result.error_string) ) self._failed = True return "failed" def on_enter(self, userdata): """Create and send action goal""" self._done = False self._failed = False # Create and populate action goal goal = FollowJointTrajectoryGoal() goal.trajectory.joint_names = self._joint_names target_joint_positions = self._gripper_joint_positions[self._gripper_cmd] point = JointTrajectoryPoint() point.positions = target_joint_positions point.time_from_start = rospy.Duration.from_sec(self._time) goal.trajectory.points.append(point) # Send the action goal for execution try: self._client.send_goal(self._action_topic, goal) except Exception as e: Logger.logwarn("Unable to send follow joint trajectory action goal:\n%s" % str(e)) self._failed = True def on_exit(self, userdata): if not self._client.has_result(self._action_topic): self._client.cancel(self._action_topic) Logger.loginfo("Cancelled active action goal.")
class LookAtWaypoint(EventState): """ Lets the robot move to a given waypoint. ># waypoint PoseStamped Specifies the waypoint to which the robot should move. <= reached Robot is now located at the specified waypoint. <= failed Failed to send a motion request to the action server. """ def __init__(self): """ Constructor """ super(LookAtWaypoint, self).__init__(outcomes=["reached", "failed"], input_keys=["waypoint"]) self._action_topic = "/pan_tilt_sensor_head_joint_control/look_at" self._client = ProxyActionClient({self._action_topic: hector_perception_msgs.msg.LookAtAction}) self._failed = False self._reached = False def execute(self, userdata): """ Execute this state """ # if self._failed: # return 'failed' if self._reached: return "reached" # if self._move_client.has_result(self._action_topic): # result = self._move_client.get_result(self._action_topic) # if result.result == 1: # self._reached = True # return 'reached' # else: # self._failed = True # Logger.logwarn('Failed to look at waypoint!') # return 'failed' def on_enter(self, userdata): self._failed = False self._reached = False action_goal = hector_perception_msgs.msg.LookAtGoal() action_goal.look_at_target.target_point.point = userdata.waypoint.pose.position if action_goal.look_at_target.target_point.header.frame_id == "": action_goal.look_at_target.target_point.header.frame_id = "map" try: self._client.send_goal(self._action_topic, action_goal) except Exception as e: Logger.logwarn( "Failed to send LookAt request to waypoint (%(x).3f, %(y).3f):\n%(err)s" % {"err": str(e), "x": userdata.waypoint.pose.position.x, "y": userdata.waypoint.pose.position.y} ) self._failed = True Logger.loginfo("Looking at next waypoint") self._reached = True def on_stop(self): try: if self._client.is_available(self._action_topic) and not self._client.has_result(self._action_topic): self._client.cancel(self._action_topic) except: # client already closed pass def on_pause(self): self._client.cancel(self._action_topic) def on_resume(self, userdata): self.on_enter(userdata)
class PipeDetectionState(EventState): """ Triggers pipe detection to find the location of the pipes -- max_attempts int Maximum number of re-running attempts. 0 means unlimitied. <= found Found the pipe location. <= unknown Did not detect any pipes. """ def __init__(self, max_attempts=1): """ Constructor """ super(PipeDetectionState, self).__init__(outcomes=["found", "unknown"]) self._action_topic = "/hector_five_pipes_detection_node/detect" self._client = ProxyActionClient({self._action_topic: DetectObjectAction}) self._max_attempts = max_attempts self._success = False self._failed = False def execute(self, userdata): """ Execute this state """ if self._failed: return "unknown" if self._client.has_result(self._action_topic): result = self._client.get_result(self._action_topic) if result.detection_success: return "found" elif self._max_attempts == 0: Logger.loginfo("Did not detect pipes, trying again (unlimited)...") self.on_enter(userdata) elif self._max_attempts > 1: self._max_attempts -= 1 Logger.loginfo("Did not detect pipes, trying again (%d more times)..." % self._max_attempts) self.on_enter(userdata) else: return "unknown" def on_enter(self, userdata): self._failed = False action_goal = DetectObjectGoal() try: self._client.send_goal(self._action_topic, action_goal) except Exception as e: Logger.logwarn("Failed to send pipe detection request:\n%s" % str(e)) self._failed = True def on_stop(self): try: if self._client.is_available(self._action_topic) and not self._client.has_result(self._action_topic): self._client.cancel(self._action_topic) except: # client already closed pass def on_pause(self): self._client.cancel(self._action_topic) def on_resume(self, userdata): self.on_enter(userdata)
class MoveToFixedWaypoint(EventState): ''' Lets the robot move to a given waypoint. -- allow_backwards Boolean Allow the robot to drive backwards when approaching the given waypoint. ># waypoint PoseStamped Specifies the waypoint to which the robot should move. ># speed Speed of the robot <= reached Robot is now located at the specified waypoint. <= failed Failed to send a motion request to the action server. ''' def __init__(self, allow_backwards = False): ''' Constructor ''' super(MoveToFixedWaypoint, self).__init__(outcomes=['reached', 'failed'], input_keys=['waypoint','speed']) self._action_topic = '/move_base' self._move_client = ProxyActionClient({self._action_topic: MoveBaseAction}) #self.set_tolerance = rospy.ServiceProxy('/controller/set_alternative_tolerances', SetAlternativeTolerance) self._dynrec = Client("/vehicle_controller", timeout = 10) self._defaultspeed = 0.1 self._allow_backwards = allow_backwards self._failed = False self._reached = False def execute(self, userdata): ''' Execute this state ''' if self._failed: return 'failed' if self._reached: return 'reached' if self._move_client.has_result(self._action_topic): result = self._move_client.get_result(self._action_topic) self._dynrec.update_configuration({'speed':self._defaultspeed}) if result.result == 1: self._reached = True return 'reached' else: self._failed = True Logger.logwarn('Failed to reach waypoint!') return 'failed' def on_enter(self, userdata): speedValue = self._dynrec.get_configuration(timeout = 0.5) if speedValue is not None: self._defaultspeed = speedValue['speed'] self._dynrec.update_configuration({'speed':userdata.speed}) self._startTime = Time.now() self._failed = False self._reached = False #goal_id = GoalID() #goal_id.id = 'abcd' #goal_id.stamp = Time.now() action_goal = MoveBaseGoal() action_goal.target_pose = userdata.waypoint action_goal.speed = userdata.speed action_goal.reverse_allowed = self._allow_backwards if action_goal.target_pose.header.frame_id == "": action_goal.target_pose.header.frame_id = "world" try: self._move_client.send_goal(self._action_topic, action_goal) #resp = self.set_tolerance(goal_id, 0.2, 1.55) except Exception as e: Logger.logwarn('Failed to send motion request to waypoint (%(x).3f, %(y).3f):\n%(err)s' % { 'err': str(e), 'x': userdata.waypoint.pose.position.x, 'y': userdata.waypoint.pose.position.y }) self._failed = True Logger.loginfo('Driving to next waypoint') def on_stop(self): try: if self._move_client.is_available(self._action_topic) \ and not self._move_client.has_result(self._action_topic): self._move_client.cancel(self._action_topic) except: # client already closed pass def on_exit(self, userdata): try: if self._move_client.is_available(self._action_topic) \ and not self._move_client.has_result(self._action_topic): self._move_client.cancel(self._action_topic) except: # client already closed pass def on_pause(self): self._move_client.cancel(self._action_topic) def on_resume(self, userdata): self.on_enter(userdata)
class CreateStepGoalState(EventState): ''' Creates a footstep goal from a desired pose. -- pose_is_pelvis boolean Set this to True if the pose is given as pelvis pose and not on the ground. ># target_pose PoseStamped Pose to which the robot should walk. #> step_goal Feet Desired feet pose. <= done Successfully created a step goal. <= failed Failed to create a plan. ''' def __init__(self, pose_is_pelvis=False): '''Constructor''' super(CreateStepGoalState, self).__init__(outcomes=['done', 'failed'], input_keys=['target_pose'], output_keys=['step_goal']) self._action_topic = "/vigir/footstep_manager/generate_feet_pose" self._client = ProxyActionClient( {self._action_topic: GenerateFeetPoseAction}) self._pose_is_pelvis = pose_is_pelvis self._done = False self._failed = False def execute(self, userdata): '''Execute this state''' if self._failed: return 'failed' if self._done: return 'done' if self._client.has_result(self._action_topic): result = self._client.get_result(self._action_topic) if result.status.error == ErrorStatus.NO_ERROR: userdata.step_goal = result.feet self._done = True return 'done' else: Logger.logwarn('Step goal creation failed:\n%s' % result.status.error_msg) self._failed = True return 'failed' def on_enter(self, userdata): '''Upon entering the state, request the feet pose.''' self._done = False self._failed = False # Create request msg and action goal request = FeetPoseRequest() request.header = userdata.target_pose.header request.header.stamp = rospy.Time.now() # timestamp used to track goal request.pose = userdata.target_pose.pose request.flags = FeetPoseRequest.FLAG_PELVIS_FRAME if self._pose_is_pelvis else 0 action_goal = GenerateFeetPoseGoal(request=request) # Send action goal try: self._client.send_goal(self._action_topic, action_goal) except Exception as e: Logger.logwarn('Failed to send step goal request') rospy.logwarn(str(e)) self._failed = True def on_exit(self, userdata): '''Destructor''' if not self._client.has_result(self._action_topic): self._client.cancel(self._action_topic) Logger.loginfo("Cancelled active action goal.")
class MoveArmState(EventState): ''' Lets the robot move its arm. ># joint_config float[] Target joint configuration of the arm. <= reached Target joint configuration has been reached. <= planning_failed Failed to find a plan to the given joint configuration. <= control_failed Failed to move the arm along the planned trajectory. ''' def __init__(self): ''' Constructor ''' super(MoveArmState, self).__init__(outcomes=['reached', 'planning_failed', 'control_failed'], input_keys=['joint_config', 'group_name']) self._action_topic = '/move_group' self._client = ProxyActionClient({self._action_topic: MoveGroupAction}) self._joint_names = ['arm_joint_0', 'arm_joint_1', 'arm_joint_2', 'arm_joint_3'] self._planning_failed = False self._control_failed = False self._success = False def execute(self, userdata): ''' Execute this state ''' if self._planning_failed: return 'planning_failed' if self._control_failed: return 'control_failed' if self._success: return 'reached' if self._client.has_result(self._action_topic): result = self._client.get_result(self._action_topic) if result.error_code.val == MoveItErrorCodes.CONTROL_FAILED: Logger.logwarn('Move Action failed with result error code: %s' % str(result.error_code)) self._control_failed = True return 'control_failed' elif result.error_code.val != MoveItErrorCodes.SUCCESS: Logger.logwarn('Move Action failed with result error code: %s' % str(result.error_code)) self._planning_failed = True return 'planning_failed' else: self._success = True return 'reached' def on_enter(self, userdata): self._planning_failed = False self._control_failed = False self._success = False action_goal = MoveGroupGoal() action_goal.request.group_name = userdata.group_name goal_constraints = Constraints() for i in range(len(self._joint_names)): goal_constraints.joint_constraints.append(JointConstraint(joint_name=self._joint_names[i], position=userdata.joint_config[i])) action_goal.request.goal_constraints.append(goal_constraints) try: self._client.send_goal(self._action_topic, action_goal) except Exception as e: Logger.logwarn('Failed to send move group goal for arm motion:\n%s' % str(e)) self._planning_failed = True def on_stop(self): try: if self._client.is_available(self._action_topic) \ and not self._client.has_result(self._action_topic): self._client.cancel(self._action_topic) except: # client already closed pass def on_pause(self): self._client.cancel(self._action_topic) def on_resume(self, userdata): self.on_enter(userdata)
class Explore(EventState): ''' Starts the Exploration Task via /move_base ># speed Speed of the robot <= succeeded Exploration Task was successful <= failed Exploration Task failed ''' def __init__(self): super(Explore, self).__init__(outcomes = ['succeeded', 'failed'], input_keys =['speed']) self._action_topic = '/move_base' self._move_client = ProxyActionClient({self._action_topic: MoveBaseAction}) self._dynrec = Client("/vehicle_controller", timeout = 10) self._defaultspeed = 0.1 self._succeeded = False self._failed = False def execute(self, userdata): if self._move_client.has_result(self._action_topic): result = self._move_client.get_result(self._action_topic) self._dynrec.update_configuration({'speed':self._defaultspeed}) if result.result == 1: self._reached = True Logger.loginfo('Exploration succeeded') return 'succeeded' else: self._failed = True Logger.logwarn('Exploration failed!') return 'failed' def on_enter(self, userdata): speedValue = self._dynrec.get_configuration(timeout = 0.5) if speedValue is not None: self._defaultspeed = speedValue['speed'] self._dynrec.update_configuration({'speed':userdata.speed}) self._succeeded = False self._failed = False action_goal = MoveBaseGoal() action_goal.exploration = True action_goal.speed = userdata.speed if action_goal.target_pose.header.frame_id == "": action_goal.target_pose.header.frame_id = "world" try: if self._move_client.is_active(self._action_topic): self._move_client.cancel(self._action_topic) self._move_client.send_goal(self._action_topic, action_goal) except Exception as e: Logger.logwarn('Failed to start Exploration' % { 'err': str(e), 'x': userdata.waypoint.pose.position.x, 'y': userdata.waypoint.pose.position.y }) self._failed = True def on_exit(self, userdata): self._move_client.cancel(self._action_topic) def on_start(self): pass def on_stop(self): pass
class MoveArmDynState(EventState): ''' Lets the robot move its arm. ># object_pose PoseStamped Pose of the object to observe. ># object_type string Object type. ># object_id string ID of the object. <= reached Target joint configuration has been reached. <= sampling_failed Failed to find any valid and promising camera pose. <= planning_failed Failed to find a plan to move the arm to a desired camera pose. <= control_failed Failed to move the arm along the planned trajectory. ''' def __init__(self): ''' Constructor ''' super(MoveArmDynState, self).__init__(outcomes=['reached', 'sampling_failed', 'planning_failed', 'control_failed'], input_keys=['object_pose', 'object_type', 'object_id']) self._action_topic = '/combined_planner' self._client = ProxyActionClient({self._action_topic: ArgoCombinedPlanAction}) # rotations for different object types to get z-axis aligned with optical axis self._rot = tf.transformations.quaternion_about_axis(-0.5*math.pi, (0,1,0)) self._sampling_failed = False self._planning_failed = False self._control_failed = False self._success = False def execute(self, userdata): ''' Execute this state ''' if self._sampling_failed: return 'sampling_failed' if self._planning_failed: return 'planning_failed' if self._control_failed: return 'control_failed' if self._success: return 'reached' if self._client.has_result(self._action_topic): result = self._client.get_result(self._action_topic) if result.success.val == ErrorCodes.SUCCESS: self._success = True return 'reached' elif result.success.val == ErrorCodes.SAMPLING_FAILED: Logger.logwarn('Sampling failed when attempting to move the arm') self._sampling_failed = True return 'sampling_failed' elif result.success.val == ErrorCodes.PLANNING_FAILED: Logger.logwarn('Planning failed when attempting to move the arm') self._planning_failed = True return 'planning_failed' else: Logger.logwarn('Control failed when attempting to move the arm') self._control_failed = True return 'control_failed' def on_enter(self, userdata): self._sampling_failed = False self._planning_failed = False self._control_failed = False self._success = False action_goal = ArgoCombinedPlanGoal() action_goal.target = deepcopy(userdata.object_pose) type_map = { 'dial_gauge': ObjectTypes.DIAL_GAUGE, 'level_gauge': ObjectTypes.LEVEL_GAUGE, 'valve': ObjectTypes.VALVE, 'hotspot': ObjectTypes.HOTSPOT, 'pipes' : ObjectTypes.PIPES, 'victim' : ObjectTypes.VICTIM } object_type = type_map.get(userdata.object_type, ObjectTypes.UNKNOWN) q = [ action_goal.target.pose.orientation.x, action_goal.target.pose.orientation.y, action_goal.target.pose.orientation.z, action_goal.target.pose.orientation.w ] q = tf.transformations.quaternion_multiply(q, self._rot) action_goal.target.pose.orientation = Quaternion(*q) action_goal.object_id.data = userdata.object_id action_goal.object_type.val = object_type action_goal.action_type.val = ActionCodes.SAMPLE_MOVE_ARM Logger.loginfo('Position arm to look at %s object' % str(userdata.object_type)) try: self._client.send_goal(self._action_topic, action_goal) except Exception as e: Logger.logwarn('Failed to send move group goal for arm motion:\n%s' % str(e)) self._control_failed = True def on_stop(self): try: if self._client.is_available(self._action_topic) \ and not self._client.has_result(self._action_topic): self._client.cancel(self._action_topic) except: # client already closed pass def on_pause(self): self._client.cancel(self._action_topic) def on_resume(self, userdata): self.on_enter(userdata)
class ParameterActionClient(EventState): ''' parameter_ac is an action client state that will make a call to an action server. This is intended to serve as a template for future states that need to communicate with action server. The topic given should correspond to an action server and it right now only works for StageAction action messages but in the future it will have any custom messages needed once they are more finalized. Currently just sends true or false to an action server and either returns complete or failed based on the response. TODO: Proper error checking other custom messages <= continue All actions completed, data collection started <= failed Data control failed to initialize or call something TODO: Proper error checking ''' def __init__(self, topic): # See example_state.py for basic explanations. super(ParameterActionClient, self).__init__(outcomes=['completed', 'failed'], input_keys=["trial_params"]) self._topic = topic self._client = ProxyActionClient({self._topic: TestParametersAction}) self._apparatus = None # It may happen that the action client fails to send the action goal. self._error = False #temporary user interface for inputting test parameters # self.trial_count = 0 # self.test_count = 0 # self.read_csv = False # self.tests = [] # self.params = { # "object" : 0, # "angle" : 0, # "trials" : 0 # } def execute(self, userdata): # Check if the client failed to send the goal. if self._error: return 'failed' #otherwise get the result and perform next steps if (self._client.has_result(self._topic)): result = self._client.get_result(self._topic) status = result.result if (status == 0): print("Completed Succesfully") return 'completed' else: print("Error Thrown") return 'failed' def on_enter(self, userdata): rospy.loginfo(" --------------------- {}".format(userdata)) #Creating the goal to send for testing goal = TestParametersGoal() goal.parameters = userdata.trial_params # find which apparatus is being used # if self._apparatus == None: # if rosnode.rosnode_ping("testbed_controller", max_count=10): # self._apparatus = "testbed" # else: # self._apparatus = "door/drawer" # #for testbed. Name of file to be read: parameters.csv # self.trial_count = self.trial_count + 1 # if self._apparatus == "testbed": # if(not self.read_csv): # with open('/home/testbed-tower/infrastructure_system/parameters.csv', mode='r') as f: # reader = csv.reader(f, delimiter=' ', quotechar='|') # for row in reader: # parsed_row = row[0].split(",") # self.params["object"] = float(parsed_row[0]) # self.params["angle"] = float(parsed_row[1]) # self.params["trials"] = float(parsed_row[2]) # self.tests.append(copy.deepcopy(self.params)) # self.read_csv = True # current_test = self.tests[self.test_count] # if(self.trial_count == current_test["trials"]): # self.test_count = self.test_count + 1 # self.trial_count = 0 # goal.parameters = [current_test["object"], current_test["angle"]] # #for door/drawer # else: # goal.parameters = [1,2,3] #error checking in case communication cant be established try: self._client.send_goal(self._topic, goal) except Exception as e: Logger.logwarn( 'Failed to send the start data collection command:\n%s' % str(e)) self._error = True def on_exit(self, userdata): # Makes sure that the action is not running when leaving this state. if not self._client.has_result(self._topic): self._client.cancel(self._topic) Logger.loginfo('Cancelled active action goal.')
class TweetPictureState(EventState): ''' State for tweeting a picture and text ''' def __init__(self): # See example_state.py for basic explanations. super(TweetPictureState, self).__init__(outcomes = ['picture_tweeted', 'tweeting_failed','command_error'], input_keys = ['picture_path', 'tweet_text']) # Create the action client when building the behavior. # This will cause the behavior to wait for the client before starting execution # and will trigger a timeout error if it is not available. # Using the proxy client provides asynchronous access to the result and status # and makes sure only one client is used, no matter how often this state is used in a behavior. self._topic = 'strands_tweets' self._client = ProxyActionClient({self._topic: SendTweetAction}) # pass required clients as dict (topic: type) # It may happen that the action client fails to send the action goal. self._error = False def execute(self, userdata): # While this state is active, check if the action has been finished and evaluate the result. # Check if the client failed to send the goal. if self._error: return 'command_error' # Check if the action has been finished if self._client.has_result(self._topic): result = self._client.get_result(self._topic) if result.success: return 'picture_tweeted' else: return 'tweeting_failed' # If the action has not yet finished, no outcome will be returned and the state stays active. def on_enter(self, userdata): # When entering this state, we send the action goal once to let the robot start its work. # As documented above, we get the specification of which dishwasher to use as input key. # This enables a previous state to make this decision during runtime and provide the ID as its own output key. # Create the goal. tweet = SendTweetGoal() tweet.text = userdata.tweet_text if len(userdata.tweet_text) > 140: tweet.text = '#LAMoR15 #ECMR15 I just told a too looong joke, stupid tweet length!' tweet.with_photo = True img = cv2.imread(userdata.picture_path) bridge = CvBridge() image_message = bridge.cv2_to_imgmsg(img, encoding="bgr8") tweet.photo = image_message try: self._client.send_goal(self._topic, tweet) except Exception as e: # Since a state failure not necessarily causes a behavior failure, it is recommended to only print warnings, not errors. # Using a linebreak before appending the error log enables the operator to collapse details in the GUI. Logger.logwarn('Failed to send the TweetPictureState command:\n%s' % str(e)) self._error = True def on_exit(self, userdata): # Make sure that the action is not running when leaving this state. # A situation where the action would still be active is for example when the operator manually triggers an outcome. if not self._client.has_result(self._topic): self._client.cancel(self._topic) Logger.loginfo('Cancelled active action goal.')
class GotoSingleArmJointConfigState(EventState): """ Directly commands the trajectory/joint controllers to move a single joint to the desired configuration. -- target_config int Identifier of the pre-defined pose to be used. -- arm_side string Arm side {left, right} ># current_config dict The current arm joint positions joint_names string[] : joint_values[] <= done Successfully executed the motion. <= failed Failed to execute the motion. """ # Wrists WRIST_CCW = 11 WRIST_CW = 12 # Forearms # ... def __init__(self, arm_side, target_config, time=2.0): """Constructor""" super(GotoSingleArmJointConfigState, self).__init__(outcomes=["done", "failed"], input_keys=["current_config"]) if not rospy.has_param("behavior/robot_namespace"): Logger.logerr("Need to specify parameter behavior/robot_namespace at the parameter server") return self._robot = rospy.get_param("behavior/robot_namespace") if not rospy.has_param("behavior/joint_controllers_name"): Logger.logerr("Need to specify parameter behavior/joint_controllers_name at the parameter server") return controller_namespace = rospy.get_param("behavior/joint_controllers_name") ################################ ATLAS ################################ self._configs = dict() self._configs["flor"] = dict() self._configs["flor"]["left"] = { 11: {"joint_name": "l_arm_wry2", "joint_value": -2.5}, 12: {"joint_name": "l_arm_wry2", "joint_value": +2.5}, } self._configs["flor"]["right"] = { 11: {"joint_name": "r_arm_wry2", "joint_value": +2.5}, 12: {"joint_name": "r_arm_wry2", "joint_value": -2.5}, } ################################ THOR ################################# self._configs["thor_mang"] = dict() self._configs["thor_mang"]["left"] = { 11: {"joint_name": "l_wrist_yaw2", "joint_value": 3.84}, 12: {"joint_name": "l_wrist_yaw2", "joint_value": -3.84}, } self._configs["thor_mang"]["right"] = { 11: {"joint_name": "r_wrist_yaw2", "joint_value": -3.84}, 12: {"joint_name": "r_wrist_yaw2", "joint_value": 3.84}, } ####################################################################### self._joint_name = self._configs[self._robot][arm_side][target_config]["joint_name"] self._joint_value = self._configs[self._robot][arm_side][target_config]["joint_value"] self._time = time self._action_topic = ( "/" + controller_namespace + "/" + arm_side + "_arm_traj_controller" + "/follow_joint_trajectory" ) self._client = ProxyActionClient({self._action_topic: FollowJointTrajectoryAction}) self._failed = False def execute(self, userdata): """Execute this state""" if self._failed: return "failed" if self._client.has_result(self._action_topic): result = self._client.get_result(self._action_topic) if result: if result.error_code == FollowJointTrajectoryResult.SUCCESSFUL: return "done" else: Logger.logwarn( "Joint trajectory failed to execute (%d). Reason: %s" % (result.error_code, result.error_string) ) self._failed = True return "failed" else: Logger.logwarn("Wait for result returned True even though the result is %s" % str(result)) self._failed = True return "failed" def on_enter(self, userdata): """On enter, create and send the follow joint trajectory action goal.""" self._failed = False current_config = userdata.current_config # Get the index of the joint whose value will be replaced index_of_joint = current_config["joint_names"].index(self._joint_name) # Replace the old joint value with the target_config's one new_values = current_config["joint_values"] new_values[index_of_joint] = self._joint_value # Create trajectory point out of the revised joint values point = JointTrajectoryPoint(positions=new_values, time_from_start=rospy.Duration.from_sec(self._time)) # Create trajectory message trajectory = JointTrajectory(joint_names=current_config["joint_names"], points=[point]) action_goal = FollowJointTrajectoryGoal(trajectory=trajectory) # execute the motion try: self._client.send_goal(self._action_topic, action_goal) except Exception as e: Logger.logwarn("Failed to send trajectory action goal:\n%s" % str(e)) self._failed = True def on_exit(self, userdata): """Destructor""" if not self._client.has_result(self._action_topic): self._client.cancel(self._action_topic) Logger.loginfo("Cancelled active action goal.")
class MoveToWaypointState(EventState): ''' Lets the robot move to a given waypoint. ># waypoint PoseStamped Specifies the waypoint to which the robot should move. <= reached Robot is now located at the specified waypoint. <= failed Failed to send a motion request to the action server. ''' def __init__(self): ''' Constructor ''' super(MoveToWaypointState, self).__init__(outcomes=['reached', 'failed'], input_keys=['waypoint']) self._action_topic = '/move_base' self._client = ProxyActionClient({self._action_topic: MoveBaseAction}) self._failed = False self._reached = False def execute(self, userdata): ''' Execute this state ''' if self._failed: return 'failed' if self._reached: return 'reached' if self._client.has_result(self._action_topic): result = self._client.get_result(self._action_topic) if result.result == 1: self._reached = True return 'reached' else: self._failed = True Logger.logwarn('Failed to reach waypoint!') return 'failed' def on_enter(self, userdata): self._failed = False self._reached = False action_goal = MoveBaseGoal() action_goal.target_pose = userdata.waypoint if action_goal.target_pose.header.frame_id == "": action_goal.target_pose.header.frame_id = "world" try: self._client.send_goal(self._action_topic, action_goal) except Exception as e: Logger.logwarn('Failed to send motion request to waypoint (%(x).3f, %(y).3f):\n%(err)s' % { 'err': str(e), 'x': userdata.waypoint.pose.position.x, 'y': userdata.waypoint.pose.position.y }) self._failed = True Logger.loginfo('Driving to next waypoint') def on_stop(self): try: if self._client.is_available(self._action_topic) \ and not self._client.has_result(self._action_topic): self._client.cancel(self._action_topic) except: # client already closed pass def on_pause(self): self._client.cancel(self._action_topic) def on_resume(self, userdata): self.on_enter(userdata)
class ApproachPersonState(EventState): ''' Actionlib actions are the most common basis for state implementations since they provide a non-blocking, high-level interface for robot capabilities. The example is based on the DoDishes-example of actionlib (see http://wiki.ros.org/actionlib). This time we have input and output keys in order to specify the goal and possibly further evaluate the result in a later state. -- dishes_to_do int Expected amount of dishes to be cleaned. ># dishwasher int ID of the dishwasher to be used. #> cleaned int Amount of cleaned dishes. <= cleaned_some Only a few dishes have been cleaned. <= cleaned_enough Cleaned a lot of dishes. <= command_error Cannot send the action goal. ''' def __init__(self): # See example_state.py for basic explanations. super(ApproachPersonState, self).__init__(outcomes = ['goal_reached', 'goal_failed','command_error'], input_keys = ['person_pose']) # Create the action client when building the behavior. # This will cause the behavior to wait for the client before starting execution # and will trigger a timeout error if it is not available. # Using the proxy client provides asynchronous access to the result and status # and makes sure only one client is used, no matter how often this state is used in a behavior. self._topic = 'stalk_pose' self._client = ProxyActionClient({self._topic: StalkPoseAction}) # pass required clients as dict (topic: type) self._timeoutTime = None self._personTopic = '/upper_body_detector/closest_bounding_box_centre' # It may happen that the action client fails to send the action goal. self._error = False def execute(self, userdata): # While this state is active, check if the action has been finished and evaluate the result. print 'approach_person_state: returning goal_reached' return 'goal_reached' # Check if the client failed to send the goal. if self._error: return 'command_error' if rospy.Time.now() > self._timeoutTime: return 'goal_failed' # Check if the action has been finished if self._client.has_result(self._topic): result = self._client.get_result(self._topic) if result.success: return 'goal_reached' else: return 'goal_failed' # If the action has not yet finished, no outcome will be returned and the state stays active. def on_enter(self, userdata): # When entering this state, we send the action goal once to let the robot start its work. # As documented above, we get the specification of which dishwasher to use as input key. # This enables a previous state to make this decision during runtime and provide the ID as its own output key. # Create the goal. goal = StalkPoseGoal() goal.runtime_sec = 60 goal.topic_name = self._personTopic goal.target = userdata.person_pose self._error = False self._timeoutTime = rospy.Time.now() + rospy.Duration(20) #try: # self._client.send_goal(self._topic, goal) #except Exception as e: # Since a state failure not necessarily causes a behavior failure, it is recommended to only print warnings, not errors. # Using a linebreak before appending the error log enables the operator to collapse details in the GUI. # Logger.logwarn('Failed to send the ApproachPerson command:\n%s' % str(e)) # self._error = True def on_exit(self, userdata): # Make sure that the action is not running when leaving this state. # A situation where the action would still be active is for example when the operator manually triggers an outcome. if not self._client.has_result(self._topic): self._client.cancel(self._topic) Logger.loginfo('Cancelled active action goal.')
class LookAtPattern(EventState): ''' Specify a pattern for the robots camera to follow ># pattern string The pattern to follow <= succeeded Camera follows the pattern <= failed Failed to set pattern ''' def __init__(self): ''' Constructor ''' super(LookAtPattern, self).__init__(outcomes=['succeeded', 'failed'], input_keys=['pattern']) self._action_topic = '/pan_tilt_sensor_head_joint_control/look_at' self._client = ProxyActionClient({self._action_topic: hector_perception_msgs.msg.LookAtAction}) self._failed = False self._succeeded = False def execute(self, userdata): ''' Execute this state ''' if self._failed: return 'failed' if self._succeeded: return 'succeeded' def on_enter(self, userdata): self._failed = False self._succeeded = False action_goal = hector_perception_msgs.msg.LookAtGoal() action_goal.look_at_target.pattern = userdata.pattern try: self._client.send_goal(self._action_topic, action_goal) except Exception as e: Logger.logwarn('Failed to send LookAt request with pattern %(pat)s\n%(err)s' % { 'pat': userdata.pattern, 'err': str(e) }) self._failed = True Logger.loginfo('Pattern %(pat)s loaded' % { 'pat': userdata.pattern }) self._succeeded = True def on_stop(self): try: if self._client.is_available(self._action_topic) \ and not self._client.has_result(self._action_topic): self._client.cancel(self._action_topic) except: # client already closed pass def on_pause(self): self._client.cancel(self._action_topic) def on_resume(self, userdata): self.on_enter(userdata)
class GripperRollState(EventState): """ Open or close the gripper. -- rotation float Rotation of the joint, use HORIZONTAL for horizontal pose (0). -- duration float Time (sec) for executing the motion. <= done Trajectory has successfully finished its execution. <= failed Failed to execute trajectory. """ HORIZONTAL = 0 def __init__(self, rotation, duration=1.0): """ Constructor """ super(GripperRollState, self).__init__(outcomes=["done", "failed"]) self._action_topic = "/gripper_control/gripper_roll_traj_controller/follow_joint_trajectory" self._client = ProxyActionClient({self._action_topic: FollowJointTrajectoryAction}) self._failed = False self._duration = duration self._target_joint_value = rotation def execute(self, userdata): if self._failed: return "failed" if self._client.has_result(self._action_topic): result = self._client.get_result(self._action_topic) if result: if result.error_code == FollowJointTrajectoryResult.SUCCESSFUL: return "done" else: Logger.logwarn( "Joint trajectory request failed to execute (%d). Reason: %s" % (result.error_code, result.error_string) ) self._failed = True return "failed" else: Logger.logwarn("Wait for result returned True even though the result is %s" % str(result)) self._failed = True return "failed" def on_enter(self, userdata): self._failed = False # create point endpoint = JointTrajectoryPoint() endpoint.positions.append(self._target_joint_value) endpoint.time_from_start = rospy.Duration(0.2 + self._duration) # create goal goal = FollowJointTrajectoryGoal() goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.2) goal.trajectory.joint_names.append("arm_joint_4") goal.trajectory.points.append(endpoint) # execute the motion try: self._client.send_goal(self._action_topic, goal) except Exception as e: Logger.logwarn("Was unable to execute joint trajectory:\n%s" % str(e)) self._failed = True def on_exit(self, userdata): if not self._client.has_result(self._action_topic): self._client.cancel(self._action_topic) Logger.loginfo("Cancelled active action goal.")
class GripperState(EventState): ''' Open or close the gripper. -- close_fraction float OPEN, CLOSE or any value in [0,1]. -- duration float Time (sec) for executing the motion. <= done Trajectory has successfully finished its execution. <= failed Failed to execute trajectory. ''' OPEN = 0 CLOSE = 1 def __init__(self, action, duration = 1.0): ''' Constructor ''' super(GripperState, self).__init__(outcomes=['done', 'failed']) self._action_topic = "/gripper_control/gripper_grasp_traj_controller/follow_joint_trajectory" self._client = ProxyActionClient({self._action_topic: FollowJointTrajectoryAction}) self._failed = False self._duration = duration self._target_joint_value = action * 1.89 def execute(self, userdata): if self._failed: return 'failed' if self._client.has_result(self._action_topic): result = self._client.get_result(self._action_topic) if result: if result.error_code == FollowJointTrajectoryResult.SUCCESSFUL: return 'done' else: Logger.logwarn('Joint trajectory request failed to execute (%d). Reason: %s' % (result.error_code, result.error_string)) self._failed = True return 'failed' else: Logger.logwarn('Wait for result returned True even though the result is %s' % str(result)) self._failed = True return 'failed' def on_enter(self, userdata): self._failed = False # create point endpoint = JointTrajectoryPoint() endpoint.positions.append(self._target_joint_value) endpoint.time_from_start = rospy.Duration(0.2 + self._duration) # create goal goal = FollowJointTrajectoryGoal() goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.2) goal.trajectory.joint_names.append('gripper_joint_0') goal.trajectory.points.append(endpoint) # execute the motion try: self._client.send_goal(self._action_topic, goal) except Exception as e: Logger.logwarn('Was unable to execute joint trajectory:\n%s' % str(e)) self._failed = True def on_exit(self, userdata): if not self._client.has_result(self._action_topic): self._client.cancel(self._action_topic) Logger.loginfo("Cancelled active action goal.")
class PlanFootstepsState(EventState): ''' Creates a footstep plan to reach the desired feet pose via the OBFSM. -- mode string One of the available planning modes (class constants). ># step_goal Feet Desired feet pose. #> plan_header Header The header of the footstep plan. <= planned Successfully created a plan. <= failed Failed to create a plan. ''' MODE_STEP_NO_COLLISION = 'drc_step_no_collision' MODE_STEP_2D = 'drc_step_2D' MODE_STEP_3D = 'drc_step_3D' MODE_WALK = 'drc_walk' def __init__(self, mode): ''' Constructor ''' super(PlanFootstepsState, self).__init__(outcomes=['planned', 'failed'], input_keys=['step_goal'], output_keys=['plan_header']) self._action_topic = "/vigir/footstep_manager/step_plan_request" self._client = ProxyActionClient( {self._action_topic: StepPlanRequestAction}) self._mode = mode self._done = False self._failed = False def execute(self, userdata): ''' Execute this state ''' if self._failed: userdata.plan_header = None return 'failed' if self._done: return 'planned' if self._client.has_result(self._action_topic): result = self._client.get_result(self._action_topic) if result.status.warning != ErrorStatus.NO_WARNING: Logger.logwarn('Planning footsteps warning:\n%s' % result.status.warning_msg) if result.status.error == ErrorStatus.NO_ERROR: userdata.plan_header = result.step_plan.header num_steps = len(result.step_plan.steps) Logger.loginfo('Received plan with %d steps' % num_steps) self._done = True return 'planned' else: userdata.plan_header = None Logger.logerr('Planning footsteps failed:\n%s' % result.status.error_msg) self._failed = True return 'failed' def on_enter(self, userdata): '''Upon entering the state, send the footstep plan request.''' self._failed = False self._done = False # Create request msg and action goal request = StepPlanRequest() request.header = userdata.step_goal.header request.max_planning_time = 10.0 request.parameter_set_name = String(self._mode) action_goal = StepPlanRequestGoal(plan_request=request) try: self._client.send_goal(self._action_topic, action_goal) except Exception as e: Logger.logwarn('Failed to send footstep plan request:\n%s' % str(e)) self._failed = True def on_exit(self, userdata): '''Destructor''' if not self._client.has_result(self._action_topic): self._client.cancel(self._action_topic) Logger.loginfo("Cancelled active action goal.")
class ExecuteTrajectoryState(EventState): ''' Executes a given joint trajectory message. -- controller string Namespace of the joint trajectory controller. The part of the topic name before /follow_joint_trajectory. ># joint_trajectory JointTrajectory Trajectory to be executed, containing all required information. <= done Trajectory has successfully finished its execution. <= failed Failed to execute trajectory. ''' def __init__(self, controller): ''' Constructor ''' super(ExecuteTrajectoryState, self).__init__(outcomes=['done', 'failed'], input_keys=['joint_trajectory']) self._action_topic = controller + "/follow_joint_trajectory" self._client = ProxyActionClient({self._action_topic: FollowJointTrajectoryAction}) self._failed = False def execute(self, userdata): if self._failed: return 'failed' if self._client.has_result(self._action_topic): result = self._client.get_result(self._action_topic) if result: if result.error_code == FollowJointTrajectoryResult.SUCCESSFUL: return 'done' else: Logger.logwarn('Joint trajectory request failed to execute (%d). Reason: %s' % (result.error_code, result.error_string)) self._failed = True return 'failed' else: Logger.logwarn('Wait for result returned True even though the result is %s' % str(result)) self._failed = True return 'failed' def on_enter(self, userdata): self._failed = False # create goal goal = FollowJointTrajectoryGoal() goal.trajectory = userdata.joint_trajectory goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.2) # execute the motion try: self._client.send_goal(self._action_topic, goal) except Exception as e: Logger.logwarn('Was unable to execute joint trajectory:\n%s' % str(e)) self._failed = True def on_exit(self, userdata): if not self._client.has_result(self._action_topic): self._client.cancel(self._action_topic) Logger.loginfo("Cancelled active action goal.")
class ExampleActionState(EventState): ''' Actionlib actions are the most common basis for state implementations since they provide a non-blocking, high-level interface for robot capabilities. The example is based on the DoDishes-example of actionlib (see http://wiki.ros.org/actionlib). This time we have input and output keys in order to specify the goal and possibly further evaluate the result in a later state. -- dishes_to_do int Expected amount of dishes to be cleaned. ># dishwasher int ID of the dishwasher to be used. #> cleaned int Amount of cleaned dishes. <= cleaned_some Only a few dishes have been cleaned. <= cleaned_enough Cleaned a lot of dishes. <= command_error Cannot send the action goal. ''' def __init__(self, dishes_to_do): # See example_state.py for basic explanations. super(ExampleActionState, self).__init__( outcomes=['cleaned_some', 'cleaned_enough', 'command_error'], input_keys=['dishwasher'], output_keys=['cleaned']) self._dishes_to_do = dishes_to_do # Create the action client when building the behavior. # This will cause the behavior to wait for the client before starting execution # and will trigger a timeout error if it is not available. # Using the proxy client provides asynchronous access to the result and status # and makes sure only one client is used, no matter how often this state is used in a behavior. self._topic = 'do_dishes' self._client = ProxyActionClient( {self._topic: DoDishesAction}) # pass required clients as dict (topic: type) # It may happen that the action client fails to send the action goal. self._error = False def execute(self, userdata): # While this state is active, check if the action has been finished and evaluate the result. # Check if the client failed to send the goal. if self._error: return 'command_error' # Check if the action has been finished if self._client.has_result(self._topic): result = self._client.get_result(self._topic) dishes_cleaned = result.total_dishes_cleaned # In this example, we also provide the amount of cleaned dishes as output key. userdata.cleaned = dishes_cleaned # Based on the result, decide which outcome to trigger. if dishes_cleaned > self._dishes_to_do: return 'cleaned_enough' else: return 'cleaned_some' # If the action has not yet finished, no outcome will be returned and the state stays active. def on_enter(self, userdata): # When entering this state, we send the action goal once to let the robot start its work. # As documented above, we get the specification of which dishwasher to use as input key. # This enables a previous state to make this decision during runtime and provide the ID as its own output key. dishwasher_id = userdata.dishwasher # Create the goal. goal = DoDishesGoal() goal.dishwasher_id = dishwasher_id # Send the goal. self._error = False # make sure to reset the error state since a previous state execution might have failed try: self._client.send_goal(self._topic, goal) except Exception as e: # Since a state failure not necessarily causes a behavior failure, it is recommended to only print warnings, not errors. # Using a linebreak before appending the error log enables the operator to collapse details in the GUI. Logger.logwarn('Failed to send the DoDishes command:\n%s' % str(e)) self._error = True def on_exit(self, userdata): # Make sure that the action is not running when leaving this state. # A situation where the action would still be active is for example when the operator manually triggers an outcome. if not self._client.has_result(self._topic): self._client.cancel(self._topic) Logger.loginfo('Cancelled active action goal.')
class HandTrajectoryState(EventState): ''' Executes a given hand trajectory, i.e., a request to open or close the fingers. -- hand_type string Type of hand (e.g. 'robotiq') ># finger_trajectory JointTrajectory A single joint trajectory for the hand joints. ># hand_side string Which hand side the trajectory refers to (e.g. 'left') <= done The trajectory was successfully executed. <= failed Failed to execute trajectory. ''' def __init__(self, hand_type): '''Constructor''' super(HandTrajectoryState, self).__init__(outcomes=['done', 'failed'], input_keys=['finger_trajectory', 'hand_side']) if not rospy.has_param("behavior/hand_controllers_name"): Logger.logerr("Need to specify parameter behavior/hand_controllers_name at the parameter server") return controller_namespace = rospy.get_param("behavior/hand_controllers_name") if not rospy.has_param("behavior/hand_type_prefix"): Logger.logerr("Need to specify parameter behavior/hand_type_prefix at the parameter server") return hand_type_prefix = rospy.get_param("behavior/hand_type_prefix") # Determine which hand types and sides have been sourced self._hands_in_use = list() LEFT_HAND = os.environ[hand_type_prefix + 'LEFT_HAND_TYPE'] RIGHT_HAND = os.environ[hand_type_prefix + 'RIGHT_HAND_TYPE'] if LEFT_HAND == 'l_' + hand_type: self._hands_in_use.append('left') if RIGHT_HAND == 'r_' + hand_type: self._hands_in_use.append('right') if len(self._hands_in_use) == 0: Logger.logerr('No %s hands seem to be in use:\nLEFT_HAND = %s\nRIGHT_HAND = %s' % (hand_type, LEFT_HAND, RIGHT_HAND)) # Initialize the action clients corresponding to the hands in use self._client_topics = dict() self._active_topic = None self._client = ProxyActionClient() for hand_side in self._hands_in_use: action_topic = ("/%s/%s_hand_traj_controller/follow_joint_trajectory" % (controller_namespace, hand_side)) self._client.setupClient(action_topic, FollowJointTrajectoryAction) self._client_topics[hand_side] = action_topic self._failed = False def execute(self, userdata): '''During execution of the state, keep checking for action servers response''' if self._failed: return 'failed' if self._client.has_result(self._active_topic): result = self._client.get_result(self._active_topic) # Logger.loginfo('Action server says: %s' % result.error_code) if result.error_code == FollowJointTrajectoryResult.SUCCESSFUL: return 'done' else: Logger.logwarn('Hand trajectory request failed to execute: %s (%d)' % (result.error_string, result.error_code)) self._failed = True return 'failed' def on_enter(self, userdata): '''Upon entering the state, create and send the action goal message''' self._failed = False if userdata.hand_side not in self._hands_in_use: Logger.logerr('Hand side from userdata (%s) does not match hands in use: %s' % (userdata.hand_side, self._hands_in_use)) self._failed = True return # Create goal message goal = FollowJointTrajectoryGoal() goal.trajectory = userdata.finger_trajectory # Send goal to action server for execution try: self._active_topic = self._client_topics[userdata.hand_side] self._client.send_goal(self._active_topic, goal) except Exception as e: Logger.logwarn('Failed to send follow (hand) joint trajectory action goal:\n%s' % str(e)) self._failed = True def on_exit(self, userdata): if not self._client.has_result(self._active_topic): self._client.cancel(self._active_topic) Logger.loginfo("Cancelled active action goal.")
class MoveitToJointsState(EventState): ''' Uses MoveIt to plan and move the specified joints to the target configuration. -- move_group string Name of the move group to be used for planning. Specified joint names need to exist in the given group. -- joint_names string[] Names of the joints to set. Does not need to specify all joints. -- action_topic string Topic on which MoveIt is listening for action calls. ># joint_config float[] Target configuration of the joints. Same order as their corresponding names in joint_names. <= reached Target joint configuration has been reached. <= planning_failed Failed to find a plan to the given joint configuration. <= control_failed Failed to move the arm along the planned trajectory. ''' def __init__(self, move_group, joint_names, action_topic='/move_group'): ''' Constructor ''' super(MoveitToJointsState, self).__init__( outcomes=['reached', 'planning_failed', 'control_failed'], input_keys=['joint_config']) self._action_topic = action_topic self._client = ProxyActionClient({self._action_topic: MoveGroupAction}) self._move_group = move_group self._joint_names = joint_names self._planning_failed = False self._control_failed = False self._success = False def execute(self, userdata): ''' Execute this state ''' if self._planning_failed: return 'planning_failed' if self._control_failed: return 'control_failed' if self._success: return 'reached' if self._client.has_result(self._action_topic): result = self._client.get_result(self._action_topic) if result.error_code.val == MoveItErrorCodes.CONTROL_FAILED: Logger.logwarn( 'Control failed for move action of group: %s (error code: %s)' % (self._move_group, str(result.error_code))) self._control_failed = True return 'control_failed' elif result.error_code.val != MoveItErrorCodes.SUCCESS: Logger.logwarn( 'Move action failed with result error code: %s' % str(result.error_code)) self._planning_failed = True return 'planning_failed' else: self._success = True return 'reached' def on_enter(self, userdata): self._planning_failed = False self._control_failed = False self._success = False action_goal = MoveGroupGoal() action_goal.request.group_name = self._move_group goal_constraints = Constraints() for i in range(len(self._joint_names)): goal_constraints.joint_constraints.append( JointConstraint(joint_name=self._joint_names[i], position=userdata.joint_config[i])) action_goal.request.goal_constraints.append(goal_constraints) try: self._client.send_goal(self._action_topic, action_goal) except Exception as e: Logger.logwarn('Failed to send action goal for group: %s\n%s' % (self._move_group, str(e))) self._planning_failed = True def on_stop(self): try: if self._client.is_available(self._action_topic) \ and not self._client.has_result(self._action_topic): self._client.cancel(self._action_topic) except: # client already closed pass def on_pause(self): self._client.cancel(self._action_topic) def on_resume(self, userdata): self.on_enter(userdata)
class MoveitToPose(EventState): ''' Uses MoveIt to plan and move the move group to the target pose. `move_group` node and `robot_description` parameter must present in current namespace. State depends only on `move_group` action interface. NOTE: sometimes lower tolerance values (e.g. 0.01 m and 0.1 rad) may cause planner to fail. -- move_group string Name of the move group to be used for planning. -- plan_only boolean Do not perform any movements. Only plan trajectory. -- position_tolerance float Target positon tolerance in meters (default: 0.001 m). -- orientation_tolerance float Target orientation tolerance in radians (deafult: 0.1 (6 degree)) ># pose object Desired pose as geomery_msgs.msg.PoseStamped message. <= reached Target configuration has been reached (or can be reaced). <= planning_failed Failed to find a plan to the given configuration. <= control_failed Failed to move along the planned trajectory. ''' def __init__(self, move_group, plan_only=False, position_tolerance=0.001, orientation_tolerance=0.001): ''' Constructor ''' super(MoveitToPose, self).__init__( outcomes=['reached', 'planning_failed', 'control_failed'], input_keys=['pose']) # store parameters and get end effector self._move_group = move_group self._position_tolerance = position_tolerance self._orientation_tolerance = orientation_tolerance self._plan_only = plan_only self._client = ProxyActionClient({'move_group': MoveGroupAction}) # get end effector link self._end_effector = None srdf_param = rospy.get_param("robot_description_semantic") srdf = ET.fromstring(srdf_param) for ee in srdf.iter('end_effector'): if ee.attrib['group'] == move_group: self._end_effector = ee.attrib['parent_link'] break if not self._end_effector: Logger.logerr( 'MoveitToPose: Unable to determine end effector for group `%s`.' % self._move_group) raise RuntimeError( 'Unable to determine end effector for group `%s`.' % self._move_group) self._planning_failed = False self._control_failed = False self._success = False def on_enter(self, userdata): self._planning_failed = False self._control_failed = False self._success = False # TODO check userdata # if not isinstance(userdata.pose, PoseStamped): #Logger.logwarn('userdata.pose must be geomery_msgs.msg.PoseStamped. `%s` received' % str(type(userdata.pose))) #self._planning_failed = True #return check_type('pose', 'geometry_msgs/PoseStamped', userdata.pose) # request planing and execution action_goal = MoveGroupGoal() # set palnning options action_goal.planning_options.plan_only = self._plan_only action_goal.planning_options.replan = False # action_goal.planning_options.planning_scene_diff.is_diff = True # action_goal.planning_options.planning_scene_diff.robot_state.is_diff = True # setup request action_goal.request.group_name = self._move_group action_goal.request.num_planning_attempts = 3 action_goal.request.allowed_planning_time = 1.0 action_goal.request.max_velocity_scaling_factor = 1.0 action_goal.request.max_acceleration_scaling_factor = 1.0 # start pose action_goal.request.start_state.is_diff = True # pose constraint goal_constraint = Constraints(name='') # set target position constraint = PositionConstraint() constraint.header = Header(frame_id=userdata.pose.header.frame_id) constraint.link_name = self._end_effector constraint.constraint_region = BoundingVolume() constraint.constraint_region.primitives = [ SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[self._position_tolerance]) ] constraint.constraint_region.primitive_poses = [ Pose(position=userdata.pose.pose.position) ] constraint.weight = 1.0 goal_constraint.position_constraints.append(constraint) # set target orientation constraint = OrientationConstraint() constraint.header = Header(frame_id=userdata.pose.header.frame_id) constraint.link_name = self._end_effector constraint.orientation = userdata.pose.pose.orientation constraint.absolute_x_axis_tolerance = self._orientation_tolerance constraint.absolute_y_axis_tolerance = self._orientation_tolerance constraint.absolute_z_axis_tolerance = self._orientation_tolerance constraint.weight = 0.1 goal_constraint.orientation_constraints.append(constraint) # add goal_constraint action_goal.request.goal_constraints.append(goal_constraint) try: self._client.send_goal('move_group', action_goal) except Exception as e: Logger.logwarn( 'MoveitToPose: Failed to send MoveGroupAction goal for group: %s\n%s' % (self._move_group, str(e))) self._planning_failed = True def execute(self, userdata): ''' Execute this state ''' if self._planning_failed: return 'planning_failed' if self._control_failed: return 'control_failed' if self._success: return 'reached' if self._client.has_result('move_group'): result = self._client.get_result('move_group') if result.error_code.val == MoveItErrorCodes.CONTROL_FAILED: Logger.logwarn( 'MoveitToPose: Control failed for MoveGroupAction of group: %s (error code: %s)' % (self._move_group, str(result.error_code))) self._control_failed = True return 'control_failed' elif result.error_code.val != MoveItErrorCodes.SUCCESS: Logger.logwarn( 'MoveitToPose: Move action failed with result error code: %s' % str(result.error_code)) self._planning_failed = True return 'planning_failed' else: self._success = True return 'reached' def on_stop(self): try: if self._client.is_available(self._action_topic) \ and not self._client.has_result(self._action_topic): self._client.cancel(self._action_topic) except: # client already closed pass def on_pause(self): self._client.cancel(self._action_topic) def on_resume(self, userdata): self.on_enter(userdata)
class TiltHeadState(EventState): ''' Executes a given joint trajectory message. -- desired_tilt float Degrees (positive is down, negative is up). Class variables are available (UP, DOWN, STRAIGHT). <= done The head was successfully tilted. <= failed Failed to execute neck trajectory. ''' UP_40 = -40.0 # max -40 degrees UP_20 = -20.0 STRAIGHT = +0.00 DOWN_30 = +20.0 DOWN_45 = +40.0 DOWN_60 = +60.0 # max +60 degrees # HEAD # STRAIGHT = 0 # UP_40 = 1 # UP_20 = 2 # DOWN_30 = 3 # DOWN_45 = 4 # DOWN_60 = 5 def __init__(self, desired_tilt): '''Constructor''' super(TiltHeadState, self).__init__(outcomes=['done', 'failed']) if not rospy.has_param("behavior/joint_controllers_name"): Logger.logerr("Need to specify parameter behavior/joint_controllers_name at the parameter server") return controller_namespace = rospy.get_param("behavior/joint_controllers_name") # self._configs['flor']['same'] = { # 20: {'joint_name': 'neck_ry', 'joint_values': [+0.00], 'controller': 'neck_traj_controller'}, # max -40 degrees # 21: {'joint_name': 'neck_ry', 'joint_values': [-40.0], 'controller': 'neck_traj_controller'}, # 22: {'joint_name': 'neck_ry', 'joint_values': [-20.0], 'controller': 'neck_traj_controller'}, # 23: {'joint_name': 'neck_ry', 'joint_values': [+20.0], 'controller': 'neck_traj_controller'}, # 24: {'joint_name': 'neck_ry', 'joint_values': [+40.0], 'controller': 'neck_traj_controller'}, # 25: {'joint_name': 'neck_ry', 'joint_values': [+60.0], 'controller': 'neck_traj_controller'} # max +60 degrees # } self._action_topic = "/" + controller_namespace + \ "/neck_traj_controller" + "/follow_joint_trajectory" self._client = ProxyActionClient({self._action_topic: FollowJointTrajectoryAction}) # Convert desired position to radians self._desired_tilt = math.radians(desired_tilt) self._failed = False def execute(self, userdata): if self._failed: return 'failed' if self._client.has_result(self._action_topic): result = self._client.get_result(self._action_topic) if result: if result.error_code == FollowJointTrajectoryResult.SUCCESSFUL: return 'done' else: Logger.logwarn('Joint trajectory request failed to execute: (%d) %s' % (result.error_code, result.error_string)) self._failed = True return 'failed' else: Logger.logwarn('Wait for result returned True even though the result is %s' % str(result)) self._failed = True return 'failed' def on_enter(self, userdata): '''Upon entering, create a neck trajectory and send the action goal.''' self._failed = False # Create neck point neck_point = JointTrajectoryPoint() neck_point.time_from_start = rospy.Duration.from_sec(1.0) neck_point.positions.append(self._desired_tilt) # Create neck trajectory neck_trajectory = JointTrajectory() neck_trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.2) neck_trajectory.joint_names = ['neck_ry'] neck_trajectory.points.append(neck_point) # Create action goal action_goal = FollowJointTrajectoryGoal(trajectory = neck_trajectory) # execute the motion try: self._client.send_goal(self._action_topic, action_goal) except Exception as e: Logger.logwarn('Failed to send neck trajectory goal:\n%s' % str(e)) self._failed = True def on_exit(self, userdata): if not self._client.has_result(self._action_topic): self._client.cancel(self._action_topic) Logger.loginfo("Cancelled active action goal.")
class WallFollowingActionState(EventState): ''' State for calling wall_following action server. ''' def __init__(self, action_server_name, desired_yaw, desired_speed, desired_wall_distance): # See example_state.py for basic explanations. super(WallFollowingActionState, self).__init__( outcomes = ['successed', 'aborted', 'preempted']) self._topic = action_server_name self._desired_yaw = desired_yaw self._desired_speed = desired_speed self._desired_wall_distance = desired_wall_distance self._client = ProxyActionClient({self._topic: FollowingAction}) # pass required clients as dict (topic: type) Logger.loginfo('[WallFollowing]: Action client initialized') # It may happen that the action client fails to send the action goal. self._error = False def execute(self, userdata): # While this state is active, check if the action has been finished and evaluate the result. # Check if the client failed to send the goal. if self._error: return 'aborted' # Check if the action has been finished if self._client.has_result(self._topic): status = self._client.get_state(self._topic) result = self._client.get_result(self._topic) if status == GoalStatus.SUCCEEDED: Logger.loginfo("[WallFollowing]: %s" % str(result.message)) return 'successed' elif status == GoalStatus.PREEMPTED: Logger.logwarn('[WallFollowing]: Action preempted') return 'preempted' elif status in [GoalStatus.REJECTED, GoalStatus.RECALLED, GoalStatus.ABORTED]: Logger.logerr('[WallFollowing]: Action failed: %s' % str(result.message)) return 'aborted' """feedback params are""" feedback = self._client.get_feedback(self._topic) if feedback is not None: Logger.loginfo("[WallFollowing]: Current feedback msg: %s" % feedback.message) def on_enter(self, userdata): # When entering this state, we send the action goal once to let the robot start its work. # Create the goal. goal = FollowingGoal() #fill the goal goal.yaw = self._desired_yaw goal.wall_distance = self._desired_wall_distance goal.speed = self._desired_speed # Send the goal. self._error = False # make sure to reset the error state since a previous state execution might have failed try: self._client.send_goal(self._topic, goal) except Exception as e: # Since a state failure not necessarily causes a behavior failure, it is recommended to only print warnings, not errors. # Using a linebreak before appending the error log enables the operator to collapse details in the GUI. Logger.logerr('[WallFollowing]: Failed to send the goal:\n%s' % str(e)) self._error = True def on_exit(self, userdata): # Make sure that the action is not running when leaving this state. # A situation where the action would still be active is for example when the operator manually triggers an outcome. if not self._client.has_result(self._topic): self._client.cancel(self._topic) Logger.loginfo('[WallFollowing]: Cancelled active action goal.')
class ExecuteStepPlanActionState(EventState): ''' Implements a state to execute a step plan of the footstep planner via the OBFSM. This state will change the control mode of the robot to STEP during execution and expects to be in STAND when entered. ># plan_header Header Header of the footstep plan to be executed. Use one of the footstep planning states to calculate such a plan. <= finished Finished walking. Control mode will be changed back to STAND, but this can take some time. If you need to be in STAND for the next state, add a CheckCurrentControlModeState. <= failed Failed to completely execute the plan. Plan might have been executed partially. ''' def __init__(self): ''' Constructor ''' super(ExecuteStepPlanActionState , self).__init__(outcomes=['finished','failed'], input_keys=['plan_header']) self._action_topic = "/vigir/footstep_manager/execute_step_plan" self._client = ProxyActionClient({self._action_topic: ExecuteStepPlanAction}) self._failed = False def execute(self, userdata): ''' Execute this state ''' if self._failed: return 'failed' if self._client.has_result(self._action_topic): result = self._client.get_result(self._action_topic) if (result is not None and result.status.status == FootstepExecutionStatus.REACHED_GOAL): return 'finished' else: if result is None: Logger.logwarn("Got None as result") else: Logger.logwarn("Result status %d" % result.status.status) self._failed = True return 'failed' def on_enter(self, userdata): '''Upon entering the state, request step plan execution from OBFSM''' execution_request = StepPlan(header = userdata.plan_header) action_goal = ExecuteStepPlanGoal(step_plan = execution_request) try: self._client.send_goal(self._action_topic, action_goal) except Exception as e: Logger.logwarn('Unable to execute step plan') rospy.logwarn(str(e)) self._failed = True def on_exit(self, userdata): if not self._client.has_result(self._action_topic): self._client.cancel(self._action_topic) Logger.loginfo("Cancelled active action goal.")
class MoveitToJointsDynState(EventState): ''' Uses MoveIt to plan and move the specified joints to the target configuration. -- move_group string Name of the move group to be used for planning. Specified joint names need to exist in the given group. -- action_topic string Topic on which MoveIt is listening for action calls. ># joint_names string[] Names of the joints to set. Does not need to specify all joints. ># joint_values float[] Target configuration of the joints. Same order as their corresponding names in joint_names. <= reached Target joint configuration has been reached. <= planning_failed Failed to find a plan to the given joint configuration. <= control_failed Failed to move the arm along the planned trajectory. ''' def __init__(self, move_group, action_topic = '/move_group'): ''' Constructor ''' super(MoveitToJointsDynState, self).__init__( outcomes=['reached', 'planning_failed', 'control_failed'], input_keys=['joint_values', 'joint_names']) self._action_topic = action_topic self._client = ProxyActionClient({self._action_topic: MoveGroupAction}) self._move_group = move_group self._joint_names = None self._planning_failed = False self._control_failed = False self._success = False def execute(self, userdata): ''' Execute this state ''' if self._planning_failed: return 'planning_failed' if self._control_failed: return 'control_failed' if self._success: return 'reached' if self._client.has_result(self._action_topic): result = self._client.get_result(self._action_topic) if result.error_code.val == MoveItErrorCodes.CONTROL_FAILED: Logger.logwarn('Control failed for move action of group: %s (error code: %s)' % (self._move_group, str(result.error_code))) self._control_failed = True return 'control_failed' elif result.error_code.val != MoveItErrorCodes.SUCCESS: Logger.logwarn('Move action failed with result error code: %s' % str(result.error_code)) self._planning_failed = True return 'planning_failed' else: self._success = True return 'reached' def on_enter(self, userdata): self._planning_failed = False self._control_failed = False self._success = False self._joint_names = userdata.joint_names action_goal = MoveGroupGoal() action_goal.request.group_name = self._move_group goal_constraints = Constraints() for i in range(len(self._joint_names)): goal_constraints.joint_constraints.append(JointConstraint(joint_name=self._joint_names[i], position=userdata.joint_values[i])) action_goal.request.goal_constraints.append(goal_constraints) try: self._client.send_goal(self._action_topic, action_goal) except Exception as e: Logger.logwarn('Failed to send action goal for group: %s\n%s' % (self._move_group, str(e))) self._planning_failed = True def on_stop(self): try: if self._client.is_available(self._action_topic) \ and not self._client.has_result(self._action_topic): self._client.cancel(self._action_topic) except: # client already closed pass def on_pause(self): self._client.cancel(self._action_topic) def on_resume(self, userdata): self.on_enter(userdata)
class CreateStepGoalState(EventState): ''' Creates a footstep goal from a desired pose. -- pose_is_pelvis boolean Set this to True if the pose is given as pelvis pose and not on the ground. ># target_pose PoseStamped Pose to which the robot should walk. #> step_goal Feet Desired feet pose. <= done Successfully created a step goal. <= failed Failed to create a plan. ''' def __init__(self, pose_is_pelvis = False): '''Constructor''' super(CreateStepGoalState, self).__init__(outcomes = ['done', 'failed'], input_keys = ['target_pose'], output_keys = ['step_goal']) self._action_topic = "/vigir/footstep_manager/generate_feet_pose" self._client = ProxyActionClient({self._action_topic: GenerateFeetPoseAction}) self._pose_is_pelvis = pose_is_pelvis self._done = False self._failed = False def execute(self, userdata): '''Execute this state''' if self._failed: return 'failed' if self._done: return 'done' if self._client.has_result(self._action_topic): result = self._client.get_result(self._action_topic) if result.status.error == ErrorStatus.NO_ERROR: userdata.step_goal = result.feet self._done = True return 'done' else: Logger.logwarn('Step goal creation failed:\n%s' % result.status.error_msg) self._failed = True return 'failed' def on_enter(self, userdata): '''Upon entering the state, request the feet pose.''' self._done = False self._failed = False # Create request msg and action goal request = FeetPoseRequest() request.header = userdata.target_pose.header request.header.stamp = rospy.Time.now() # timestamp used to track goal request.pose = userdata.target_pose.pose request.flags = FeetPoseRequest.FLAG_PELVIS_FRAME if self._pose_is_pelvis else 0 action_goal = GenerateFeetPoseGoal(request = request) # Send action goal try: self._client.send_goal(self._action_topic, action_goal) except Exception as e: Logger.logwarn('Failed to send step goal request') rospy.logwarn(str(e)) self._failed = True def on_exit(self, userdata): '''Destructor''' if not self._client.has_result(self._action_topic): self._client.cancel(self._action_topic) Logger.loginfo("Cancelled active action goal.")
class MoveitToJointsDynState(EventState): ''' Uses MoveIt to plan and move the specified joints to the target configuration. -- move_group string Name of the move group to be used for planning. Specified joint names need to exist in the given group. -- offset float Some offset -- tool_link string e.g. "vacuum_gripper1_suction_cup" -- action_topic string Topic on which MoveIt is listening for action calls. ># joint_names string[] Names of the joints to set. Does not need to specify all joints. ># joint_values float[] Target configuration of the joints. Same order as their corresponding names in joint_names. <= reached Target joint configuration has been reached. <= planning_failed Failed to find a plan to the given joint configuration. <= control_failed Failed to move the arm along the planned trajectory. ''' def __init__(self, move_group, offset, tool_link, action_topic='/move_group'): ''' Constructor ''' super(MoveitToJointsDynState, self).__init__( outcomes=['reached', 'planning_failed', 'control_failed'], input_keys=['joint_values', 'joint_names']) self._offset = offset self._tool_link = tool_link self._action_topic = action_topic self._fk_srv_topic = '/compute_fk' self._cartesian_srv_topic = '/compute_cartesian_path' self._client = ProxyActionClient({self._action_topic: MoveGroupAction}) self._fk_srv = ProxyServiceCaller({self._fk_srv_topic: GetPositionFK}) self._cartesian_srv = ProxyServiceCaller( {self._cartesian_srv_topic: GetCartesianPath}) self._current_group_name = move_group self._joint_names = None self._planning_failed = False self._control_failed = False self._success = False self._traj_client = actionlib.SimpleActionClient( 'execute_trajectory', moveit_msgs.msg.ExecuteTrajectoryAction) self._traj_client.wait_for_server() self._traj_exec_result = None def execute(self, userdata): ''' Execute this state ''' if self._planning_failed: return 'planning_failed' if self._control_failed: return 'control_failed' if self._success: return 'reached' if self._traj_exec_result: # look at our cached 'execute_trajectory' result to see whether # execution was successful result = self._traj_exec_result if result.error_code.val == MoveItErrorCodes.CONTROL_FAILED: Logger.logwarn( 'Control failed for move action of group: %s (error code: %s)' % (self._current_group_name, str(result.error_code))) self._control_failed = True return 'control_failed' elif result.error_code.val != MoveItErrorCodes.SUCCESS: Logger.logwarn( 'Move action failed with result error code: %s' % str(result.error_code)) self._planning_failed = True return 'planning_failed' else: self._success = True return 'reached' def on_enter(self, userdata): self._planning_failed = False self._control_failed = False self._success = False self._joint_names = userdata.joint_names self._initial_state = RobotState() self._initial_state.joint_state.position = copy.deepcopy( userdata.joint_values) self._initial_state.joint_state.name = copy.deepcopy(self._joint_names) #print self._initial_state.joint_state.name #print self._initial_state.joint_state.position self._srv_req = GetPositionFKRequest() self._srv_req.robot_state = self._initial_state self._srv_req.header.stamp = rospy.Time.now() self._srv_req.header.frame_id = "world" self._srv_req.fk_link_names = [self._tool_link] try: srv_result = self._fk_srv.call(self._fk_srv_topic, self._srv_req) self._failed = False except Exception as e: Logger.logwarn('Could not call FK: ' + str(e)) self._planning_failed = True return grasp_pose = srv_result.pose_stamped[0].pose grasp_pose_stamped = srv_result.pose_stamped[0] # Create a pre-grasp approach pose with an offset of 0.3 pre_grasp_approach_pose = copy.deepcopy(grasp_pose_stamped) pre_grasp_approach_pose.pose.position.z += self._offset + 0.3 # Create an object to MoveGroupInterface for the current robot. self._mgi_active_robot = MoveGroupInterface( self._current_group_name, self._current_group_name + '_base_link') # TODO: clean up in on_exit self._mgi_active_robot.moveToPose(pre_grasp_approach_pose, self._tool_link) # Use cartesian motions to pick the object. cartesian_service_req = GetCartesianPathRequest() cartesian_service_req.start_state.joint_state = rospy.wait_for_message( self._current_group_name + '/joint_states', sensor_msgs.msg.JointState) cartesian_service_req.header.stamp = rospy.Time.now() cartesian_service_req.header.frame_id = "world" cartesian_service_req.link_name = self._tool_link cartesian_service_req.group_name = self._current_group_name cartesian_service_req.max_step = 0.01 cartesian_service_req.jump_threshold = 0 cartesian_service_req.avoid_collisions = True grasp_pose.position.z += self._offset + 0.16 # this is basically the toolframe (with the box as the tool) cartesian_service_req.waypoints.append(grasp_pose) try: cartesian_srv_result = self._cartesian_srv.call( self._cartesian_srv_topic, cartesian_service_req) self._failed = False except Exception as e: Logger.logwarn('Could not call Cartesian: ' + str(e)) self._planning_failed = True return if cartesian_srv_result.fraction < 1.0: Logger.logwarn('Cartesian failed. fraction: ' + str(cartesian_srv_result.fraction)) self._planning_failed = True return traj_goal = moveit_msgs.msg.ExecuteTrajectoryGoal() traj_goal.trajectory = cartesian_srv_result.solution self._traj_client.send_goal(traj_goal) self._traj_client.wait_for_result() self._traj_exec_result = self._traj_client.get_result() def on_exit(self, userdata): pass def on_stop(self): try: if self._client.is_available(self._action_topic) \ and not self._client.has_result(self._action_topic): self._client.cancel(self._action_topic) except: # client already closed pass def on_pause(self): self._client.cancel(self._action_topic) def on_resume(self, userdata): self.on_enter(userdata)
class hsr_MoveitToPoseGoalActionNew(EventState): ''' Uses MoveIt to plan and move the specified joints to the target configuration. -- move_group string Name of the move group to be used for planning.Specified joint names need to exist in the given group. -- action_topic string Topic on which MoveIt is listening for action calls. -- orien_tolerance bool Default: True ># pose_goal pose Target configuration of the joints. ># plan_num int plan_num #> move_status string move_status <= reached Target joint configuration has been reached. <= planning_failed Failed to find a plan to the given joint configuration. <= control_failed Failed to move the arm along the planned trajectory. ''' def __init__(self, move_group='whole_body', action_topic = '/move_group', orien_tolerance=True): ''' Constructor ''' super(hsr_MoveitToPoseGoalActionNew, self).__init__(outcomes=['reached', 'planning_failed', 'control_failed'], output_keys=['move_status'], input_keys=['pose_goal','plan_num']) self._action_topic = action_topic self._client = ProxyActionClient({self._action_topic: MoveGroupAction}) self.orien_tolerance = orien_tolerance self._move_group = move_group self._planning_failed = False self._control_failed = False self._success = False self.count_num = None self._tolerance = 0.01 robot = moveit_commander.RobotCommander() group = moveit_commander.MoveGroupCommander(self._move_group) self.planning_frame = group.get_planning_frame() self.planning_frame = '/map' self.eef_link = group.get_end_effector_link() group_names = robot.get_group_names() scene = moveit_commander.PlanningSceneInterface() def execute(self, userdata): ''' Execute this state ''' if self._planning_failed: userdata.move_status = 'fail_plan' return 'planning_failed' if self._control_failed: return 'control_failed' if self._success: return 'reached' if self._client.has_result(self._action_topic): result = self._client.get_result(self._action_topic) if result.error_code.val == MoveItErrorCodes.CONTROL_FAILED: Logger.logwarn('Control failed for move action of group: %s (error code: %s)' % (self._move_group, str(result.error_code))) self._control_failed = True return 'control_failed' elif result.error_code.val != MoveItErrorCodes.SUCCESS: Logger.logwarn('Move action failed with result error code: %s' % str(result.error_code)) self._planning_failed = True return 'planning_failed' else: self._success = True return 'reached' def on_enter(self, userdata): self.count_num = userdata.plan_num self._planning_failed = False self._control_failed = False self._success = False print('planning_frame:', self.planning_frame) print('eef_link:', self.eef_link) exec_pose = userdata.pose_goal action_goal = make_default_action_goal(self, exec_pose, orientation=self.orien_tolerance) action_goal.planning_options.planning_scene_diff.is_diff = True action_goal.planning_options.planning_scene_diff.robot_state.is_diff = True action_goal.planning_options.plan_only = False try: self._client.send_goal(self._action_topic, action_goal) except Exception as e: Logger.logwarn('Failed to send action goal for group: %s\n%s' % (self._move_group, str(e))) self._planning_failed = True def on_stop(self): try: if self._client.is_available(self._action_topic) \ and not self._client.has_result(self._action_topic): self._client.cancel(self._action_topic) except: # client already closed pass def on_pause(self): self._client.cancel(self._action_topic) def on_resume(self, userdata): self.on_enter(userdata)
class FollowPathState(EventState): """ Follows a plan to reach the desired goal point. -- topic String The node to communicate with ># plan Path Desired Path to follow <= done Successfully reached the goal <= failed Failed to generate or follow a plan <= preempted The state was preempted """ def __init__(self, topic): """ Constructor """ super(FollowPathState, self).__init__(outcomes=['done', 'failed', 'preempted'], input_keys=['plan']) self._action_topic = topic self._client = ProxyActionClient( {self._action_topic: FollowPathAction}) def execute(self, userdata): """ Execute this state """ if self._client.has_result(self._action_topic): result = self._client.get_result(self._action_topic) ProxyActionClient._result[ self. _action_topic] = None # Clear to avoid spam if blocked by low autonomy level if result.code == 0: Logger.loginfo('%s Success!' % (self.name)) self._return = 'done' elif result.code == 1: Logger.logerr('%s Failure' % (self.name)) self._return = 'failed' elif result.code == 2: Logger.logerr('%s Preempted' % (self.name)) self._return = 'preempted' else: Logger.logerr('%s Unknown error' % (self.name)) self._return = 'failed' # Return prior value if blocked return self._return def on_enter(self, userdata): """ On enter, send action goal """ self._return = None result = FollowPathGoal(path=userdata.plan) try: Logger.loginfo('%s Following the path to victory!' % (self.name)) self._client.send_goal(self._action_topic, result) except Exception as e: Logger.logwarn('%s Failed to follow path: %s' % (self.name, str(e))) return 'failed' def on_exit(self, userdata): if self._action_topic in ProxyActionClient._result: ProxyActionClient._result[self._action_topic] = None if self._client.is_active(self._action_topic): Logger.logerr('%s Canceling active goal' % (self.name)) self._client.cancel(self._action_topic)
class GetPlanningSceneState(EventState): ''' State to get planning scene using GetPlanningSceneAction. -- components uint32 Bitfield defining the relevant parts of planning scene that are of interest. (default: 1023 - send everything ) -- timeout double How long to wait for planning scene. (default: 5 seconds) -- wait_duration double How long to wait for action server (seconds) (0.0 waits indefinitely) -- action_topic string Topic name for the ApplyPlanningScene action server. (default: None --> Use user data) ># action_topic string Topic name for the ApplyPlanningScene action server (if None ) #> scene PlanningScene Output scene <= done Planning scene was successfully applied. <= failed Applying planning scene failed. ''' def __init__(self, components=1023, timeout=5.0, wait_duration=2.0, action_topic=None): ''' Constructor ''' super(GetPlanningSceneState, self).__init__( input_keys=['action_topic'], outcomes=['done', 'failed'], output_keys=['scene'] ) self.client = None self.components = components self.timeout_duration = rospy.Duration(timeout) self.wait_duration = wait_duration self.given_action_topic = action_topic if (self.given_action_topic is not None) and (len(self.given_action_topic) > 0): # If topic is defined, set the client up on startup self.client = ProxyActionClient({self.given_action_topic: GetPlanningSceneAction}, self.wait_duration) else: self.given_action_topic = None self.current_action_topic = self.given_action_topic self.scene = list() self.return_code = None def execute(self, userdata): ''' Execute this state ''' if self.return_code is not None: # Return any previously assigned return code if we are in pause userdata.scene = self.scene return self.return_code if self.client.has_result(self.current_action_topic): result = self.client.get_result(self.current_action_topic) self.scene = result.scene self.return_code = 'done' userdata.scene = self.scene Logger.loginfo("GetPlanningScene - %s request succeeded." % (self.name)) return self.return_code elif self.client.get_state(self.current_action_topic) == GoalStatus.ABORTED: # No result returned is returned for this action, so go by the client state Logger.logwarn("GetPlanningScene - %s request aborted by get planning scene action server" % (self.name)) self.return_code = 'failed' return self.return_code elif self.client.get_state(self.current_action_topic) == GoalStatus.REJECTED: # No result returned is returned for this action, so go by the client state Logger.logwarn("GetPlanningScene - %s request rejected by get planning scene action server" % (self.name)) self.return_code = 'failed' return self.return_code elif (self.timeout_target is not None) and (rospy.Time.now() > self.timeout_target): Logger.logwarn("GetPlanningScene - timeout waiting for %s to get planning scene (%f > %f (%f))" % (self.name, rospy.Time.now().to_sec(), self.timeout_target.to_sec(), self.timeout_duration.to_sec())) self.return_code = 'failed' return self.return_code def on_enter(self, userdata): self.return_code = None self.scene = None # Retrieve the relevant data try : if (self.given_action_topic is None): self.current_action_topic = userdata.action_topic except Exception as e: Logger.logwarn('Failed to set up the action client for %s - invalid user data parameters\n%s' % (self.current_action_topic, str(e))) self.return_code = 'failed' return try: if (self.client is None): self.client = ProxyActionClient({self.current_action_topic: GetPlanningSceneAction}, self.wait_duration) except Exception as e: Logger.logwarn('Failed to set up the action client for %s\n%s' % (self.current_action_topic, str(e))) self.return_code = 'failed' return try: if not self.client.is_available(self.current_action_topic): self.client.setup_action_client(self.current_action_topic, GetPlanningSceneAction, self.wait_duration) if not self.setup_action_client.is_available(self.current_action_topic): Logger.logerr( 'Action client is not available for %s' % (self.current_action_topic)) self.return_code = 'failed' return except Exception as e: Logger.logwarn('Failed to setup the action client for %s\n%s' % (self.name, str(e))) self.return_code = 'failed' try: # Action Initialization action_goal = GetPlanningSceneGoal() action_goal.components = PlanningSceneComponents() action_goal.components.components = self.components if (self.timeout_duration > rospy.Duration(0.0)): self.timeout_target = rospy.Time.now() + self.timeout_duration else: self.timeout_target = None self.client.send_goal(self.current_action_topic, action_goal) except Exception as e: Logger.logwarn('Failed to send action goal for group - %s\n%s' % (self.name, str(e))) self.return_code = 'failed' def on_stop(self): try: if ( self.client.is_available(self.current_action_topic) \ and not self.client.has_result(self.current_action_topic) ): # Cancel any active goals self.client.cancel(self.current_action_topic) except Exception as e: # client already closed Logger.logwarn('Action client already closed - %s\n%s' % (self.current_action_topic, str(e))) def on_resume(self, userdata): # If paused during state execution, then re-send the clear command self.on_enter(userdata)
class PlanFootstepsState(EventState): """ Creates a footstep plan to reach the desired feet pose via the OBFSM. -- mode string One of the available planning modes (class constants). ># step_goal Feet Desired feet pose. #> plan_header Header The header of the footstep plan. <= planned Successfully created a plan. <= failed Failed to create a plan. """ MODE_STEP_NO_COLLISION = "drc_step_no_collision" MODE_STEP_2D = "drc_step_2D" MODE_STEP_3D = "drc_step_3D" MODE_WALK = "drc_walk" def __init__(self, mode): """ Constructor """ super(PlanFootstepsState, self).__init__( outcomes=["planned", "failed"], input_keys=["step_goal"], output_keys=["plan_header"] ) self._action_topic = "/vigir/footstep_manager/step_plan_request" self._client = ProxyActionClient({self._action_topic: StepPlanRequestAction}) self._mode = mode self._done = False self._failed = False def execute(self, userdata): """ Execute this state """ if self._failed: userdata.plan_header = None return "failed" if self._done: return "planned" if self._client.has_result(self._action_topic): result = self._client.get_result(self._action_topic) if result.status.warning != ErrorStatus.NO_WARNING: Logger.logwarn("Planning footsteps warning:\n%s" % result.status.warning_msg) if result.status.error == ErrorStatus.NO_ERROR: userdata.plan_header = result.step_plan.header num_steps = len(result.step_plan.steps) Logger.loginfo("Received plan with %d steps" % num_steps) self._done = True return "planned" else: userdata.plan_header = None Logger.logerr("Planning footsteps failed:\n%s" % result.status.error_msg) self._failed = True return "failed" def on_enter(self, userdata): """Upon entering the state, send the footstep plan request.""" self._failed = False self._done = False # Create request msg and action goal request = StepPlanRequest() request.header = userdata.step_goal.header request.max_planning_time = 10.0 request.parameter_set_name = String(self._mode) action_goal = StepPlanRequestGoal(plan_request=request) try: self._client.send_goal(self._action_topic, action_goal) except Exception as e: Logger.logwarn("Failed to send footstep plan request:\n%s" % str(e)) self._failed = True def on_exit(self, userdata): """Destructor""" if not self._client.has_result(self._action_topic): self._client.cancel(self._action_topic) Logger.loginfo("Cancelled active action goal.")
class AlignGateState(EventState): ''' Actionlib actions are the most common basis for state implementations since they provide a non-blocking, high-level interface for robot capabilities. The example is based on the DoDishes-example of actionlib (see http://wiki.ros.org/actionlib). This time we have input and output keys in order to specify the goal and possibly further evaluate the result in a later state. ''' def __init__(self): # See example_state.py for basic explanations. super(AlignGateState, self).__init__(outcomes=['success', 'command_error']) # Create the action client when building the behavior. # This will cause the behavior to wait for the client before starting execution # and will trigger a timeout error if it is not available. # Using the proxy client provides asynchronous access to the result and status # and makes sure only one client is used, no matter how often this state is used in a behavior. self._topic = 'align' self._client = ProxyActionClient( {self._topic: AlignAction}) # pass required clients as dict (topic: type) # It may happen that the action client fails to send the action goal. self._error = False def execute(self, userdata): # While this state is active, check if the action has been finished and evaluate the result. # Check if the client failed to send the goal. if self._error: return 'command_error' # Check if the action has been finished if self._client.has_result(self._topic): self.result = self._client.get_result(self._topic) if self.result: return 'success' # If the action has not yet finished, no outcome will be returned and the state stays active. def on_enter(self, userdata): # When entering this state, we send the action goal once to let the robot start its work. # As documented above, we get the specification of which dishwasher to use as input key. # This enables a previous state to make this decision during runtime and provide the ID as its own output key. objectName = userdata.object # Create the goal. goal = getObjectAlignment() goal.object = objectName # Send the goal. self._error = False # make sure to reset the error state since a previous state execution might have failed try: self._client.send_goal(self._topic, goal) except Exception as e: # Since a state failure not necessarily causes a behavior failure, it is recommended to only print warnings, not errors. # Using a linebreak before appending the error log enables the operator to collapse details in the GUI. Logger.logwarn('Failed to send the Align command:\n%s' % str(e)) self._error = True def on_exit(self, userdata): # Make sure that the action is not running when leaving this state. # A situation where the action would still be active is for example when the operator manually triggers an outcome. if not self._client.has_result(self._topic): self._client.cancel(self._topic) Logger.loginfo('Cancelled active action goal.')
class SrdfStateToMoveit(EventState): ''' State to look up a pre-defined joint configuration from the SRDF file loaded in the parameter server (/robot_description_semantic) and send it to MoveIt to plan and move. -- config_name string Name of the joint configuration of interest. -- move_group string Name of the move group to be used for planning. -- action_topic string Topic on which MoveIt is listening for action calls. -- robot_name string Optional name of the robot to be used. If left empty, the first one found will be used (only required if multiple robots are specified in the same file). ># joint_config float[] Target configuration of the joints. Same order as their corresponding names in joint_names. <= reached Target joint configuration has been reached. <= planning_failed Failed to find a plan to the given joint configuration. <= control_failed Failed to move the arm along the planned trajectory. ''' def __init__(self, config_name, move_group="", action_topic = '/move_group', robot_name=""): ''' Constructor ''' super(SrdfStateToMoveit, self).__init__(outcomes=['reached', 'planning_failed', 'control_failed', 'param_error']) self._config_name = config_name self._move_group = move_group self._robot_name = robot_name self._action_topic = action_topic self._client = ProxyActionClient({self._action_topic: MoveGroupAction}) self._planning_failed = False self._control_failed = False self._success = False self._srdf_param = None if rospy.has_param("/robot_description_semantic"): self._srdf_param = rospy.get_param("/robot_description_semantic") else: Logger.logerror('Unable to get parameter: /robot_description_semantic') self._param_error = False self._srdf = None def execute(self, userdata): if self._param_error: return 'param_error' ''' Execute this state ''' if self._planning_failed: return 'planning_failed' if self._control_failed: return 'control_failed' if self._success: return 'reached' if self._client.has_result(self._action_topic): result = self._client.get_result(self._action_topic) if result.error_code.val == MoveItErrorCodes.CONTROL_FAILED: Logger.logwarn('Control failed for move action of group: %s (error code: %s)' % (self._move_group, str(result.error_code))) self._control_failed = True return 'control_failed' elif result.error_code.val != MoveItErrorCodes.SUCCESS: Logger.logwarn('Move action failed with result error code: %s' % str(result.error_code)) self._planning_failed = True return 'planning_failed' else: self._success = True return 'reached' def on_enter(self, userdata): self._param_error = False self._planning_failed = False self._control_failed = False self._success = False #Parameter check if self._srdf_param is None: self._param_error = True return try: self._srdf = ET.fromstring(self._srdf_param) except Exception as e: Logger.logwarn('Unable to parse given SRDF parameter: /robot_description_semantic') self._param_error = True if not self._param_error: robot = None for r in self._srdf.iter('robot'): if self._robot_name == '' or self._robot_name == r.attrib['name']: robot = r break if robot is None: Logger.logwarn('Did not find robot name in SRDF: %s' % self._robot_name) return 'param_error' config = None for c in robot.iter('group_state'): if (self._move_group == '' or self._move_group == c.attrib['group']) \ and c.attrib['name'] == self._config_name: config = c self._move_group = c.attrib['group'] #Set move group name in case it was not defined break if config is None: Logger.logwarn('Did not find config name in SRDF: %s' % self._config_name) return 'param_error' try: self._joint_config = [float(j.attrib['value']) for j in config.iter('joint')] self._joint_names = [str(j.attrib['name']) for j in config.iter('joint')] except Exception as e: Logger.logwarn('Unable to parse joint values from SRDF:\n%s' % str(e)) return 'param_error' #Action Initialization action_goal = MoveGroupGoal() action_goal.request.group_name = self._move_group goal_constraints = Constraints() for i in range(len(self._joint_names)): goal_constraints.joint_constraints.append(JointConstraint(joint_name=self._joint_names[i], position=self._joint_config[i])) action_goal.request.goal_constraints.append(goal_constraints) try: self._client.send_goal(self._action_topic, action_goal) except Exception as e: Logger.logwarn('Failed to send action goal for group: %s\n%s' % (self._move_group, str(e))) self._planning_failed = True def on_stop(self): try: if self._client.is_available(self._action_topic) \ and not self._client.has_result(self._action_topic): self._client.cancel(self._action_topic) except: # client already closed pass def on_pause(self): self._client.cancel(self._action_topic) def on_resume(self, userdata): self.on_enter(userdata)