class ApplyPlanningSceneState(EventState): ''' State to apply planning scene using ApplyPlanningSceneAction. -- timeout double How long to wait for clearing map confirmation (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 ) <= done Planning scene was successfully applied. <= failed Applying planning scene failed. ''' def __init__(self, timeout=5.0, wait_duration=5, action_topic=None): ''' Constructor ''' super(ApplyPlanningSceneState, self).__init__( input_keys=['action_topic'], outcomes=['done', 'failed']) self.client = None self.timeout_duration = rospy.Duration(timeout) self.wait_duration = wait_duration self.action_topic = action_topic if (self.action_topic is not None) and (len(self.action_topic) > 0): # If topic is defined, set the client up on startup self.client = ProxyActionClient({self.action_topic: ApplyPlanningSceneAction}, self.wait_duration) else: self.action_topic = None # override "" self.moveit_client = None try: self.moveit_client = ProxyMoveItClient(None) except Exception as e: Logger.logerr(" %s - exception on initialization of ProxyMoveItClient \n%s"% (self.name, str(e))) self.success = None 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 return self.return_code if self.client.has_result(self.action_topic): result = self.client.get_result(self.action_topic) Logger.loginfo('ApplyPlanningScene - %s' % str(self.client.get_goal_status_text(self.action_topic))) if result.success: Logger.loginfo('ApplyPlanningScene success! ') self.return_code = 'done' return self.return_code else: Logger.logwarn('ApplyPlanningScene failed ' ) self.return_code = 'failed' return self.return_code elif self.client.get_state(self.action_topic) == GoalStatus.ABORTED: # No result returned is returned for this action, so go by the client state userdata.success = self.success Logger.logwarn("ApplyPlanningScene - %s request aborted by apply planning scene action server" % (self.name)) self.return_code = 'failed' return self.return_code elif self.client.get_state(self.action_topic) == GoalStatus.REJECTED: # No result returned is returned for this action, so go by the client state userdata.success = self.success Logger.logwarn("ApplyPlanningScene - %s request rejected by apply planning scene action server" % (self.name)) self.return_code = 'failed' return self.return_code elif rospy.Time.now() > self.timeout_target: userdata.success = self.success Logger.logwarn("ApplyPlanningScene - timeout waiting for %s to apply 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 #Logger.logwarn("ApplyPlanningScene - %s (%f > %f (%f))" % (self.name, rospy.Time.now().to_sec(), self.timeout_target.to_sec(), self.timeout_duration.to_sec())) def on_enter(self, userdata): self.return_code = None # Retrieve the relevant data try : if (self.action_topic is None): self.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.action_topic, str(e))) self.return_code = 'failed' return try: if (self.client is None): self.client = ProxyActionClient({self.action_topic: ApplyPlanningSceneAction}, self.wait_duration) except Exception as e: Logger.logwarn('Failed to set up the action client for %s\n%s' % (self.action_topic, str(e))) self.return_code = 'failed' return if self.moveit_client is None: try: self.moveit_client = ProxyMoveItClient(None) except Exception as e: self.moveit_client = None self.return_code = 'param_error' Logger.logerr(str(e)) Logger.logwarn('No MoveItClient configured - state %s does not exist or is currently undefined!' % self.name) return try: # Action Initialization action_goal = ApplyPlanningSceneGoal() action_goal.scene = self.moveit_client._planning_scene self.client.send_goal(self.action_topic, action_goal) self.timeout_target = rospy.Time.now() + self.timeout_duration except Exception as e: Logger.logwarn('Failed to send ApplyPlanningSceneGoal for group - %s\n%s' % (self.name, str(e))) self.return_code = 'failed' def on_stop(self): try: if ( self.client.is_available(self.action_topic) \ and not self.client.has_result(self.action_topic) ): # Cancel any active goals self.client.cancel(self.action_topic) except Exception as e: # client already closed Logger.logwarn('Action client already closed - %s\n%s' % (self.action_topic, str(e))) def on_pause(self): pass def on_resume(self, userdata): # If paused during state execution, then re-send the clear command self.on_enter(userdata)
class ExecuteKnownTrajectoryState(EventState): ''' State to execute known RobotTrajectory from MoveIt planner using ExecuteKnownTrajectoryAction capability. -- timeout float How long to wait for execution completion (default: 5 seconds) -- max_delay float Seconds that a trajectory may be delayed and still be sent to controller (default: -1 ignore time and send trajectory) -- wait_duration float How long to wait for action server (seconds) (0.0 waits indefinitely) -- action_topic string Topic name for the ExecuteKnownTrajectory action server (default: None --> Use user data) ># action_topic string Topic name for the ExecuteKnownTrajectory action server (if None ) ># trajectory RobotTrajectory The trajectory to execute. #> status_text string Status text reflecting outcome #> goal_names string[] List of joint names for goal state #> goal_values float[] List of joint values for goal state <= done Successful. <= failed Failed. <= param_error Invalid parameters - no trajectory sent ''' def __init__(self, timeout=5.0, max_delay=-1.0, wait_duration=5, action_topic=None): ''' Constructor ''' super(ExecuteKnownTrajectoryState, self).__init__( input_keys=['action_topic', 'trajectory'], outcomes=['done', 'failed', 'param_error'], output_keys=['status_text', 'goal_names', 'goal_values'] ) self.action_client = None self.goal_names = None self.goal_values = None self.timeout_duration = rospy.Duration(timeout) self.timeout_target = None self.wait_duration = wait_duration self.given_action_topic = action_topic self.max_delay = None if max_delay > 0.0: self.max_delay = rospy.Duration(max_delay) try: 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.action_client = ProxyActionClient({self.given_action_topic: ExecuteKnownTrajectoryAction}, self.wait_duration) else: self.given_action_topic = None # override "" except Exception as e: Logger.logerr(" %s - exception on initialization of ProxyMoveItClient \n%s"% (self.name, str(e))) self.current_action_topic = self.given_action_topic self.status_text = None 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.status_text = self.status_text userdata.goal_names = self.goal_names userdata.goal_values = self.goal_values return self.return_code if self.action_client.has_result(self.current_action_topic): result = self.action_client.get_result(self.current_action_topic) if result.error_code.val == MoveItErrorCodes.SUCCESS: self.status_text = 'ExecuteKnownTrajectory %s success! ' % self.name self.return_code = 'done' elif result.error_code.val == MoveItErrorCodes.PREEMPTED: self.status_text = 'ExecuteKnownTrajectory %s preempted '% self.name self.return_code = 'failed' elif result.error_code.val == MoveItErrorCodes.TIMED_OUT: self.status_text = 'ExecuteKnownTrajectory %s timed out '% self.name self.return_code = 'failed' elif result.error_code.val == MoveItErrorCodes.CONTROL_FAILED: self.status_text = 'ExecuteKnownTrajectory %s control failed '% self.name self.return_code = 'failed' else: self.status_text = 'ExecuteKnownTrajectory %s failed ' % self.name self.return_code = 'failed' Logger.loginfo('ExecuteKnownTrajectory result for %s - %s\n%s' % (self.name, self.status_text, str(self.action_client.get_goal_status_text(self.current_action_topic)))) userdata.status_text = self.status_text userdata.goal_names = self.goal_names userdata.goal_values = self.goal_values return self.return_code elif self.action_client.get_state(self.current_action_topic) == GoalStatus.ABORTED: # No result returned is returned for this action, so go by the client state self.status_text = "ExecuteKnownTrajectory- %s request aborted by execute known trajectory action server" % (self.name) self.return_code = 'failed' elif self.action_client.get_state(self.current_action_topic) == GoalStatus.REJECTED: # No result returned is returned for this action, so go by the client state self.status_text = "ExecuteKnownTrajectory - %s request rejected by execute known trajectory action server" % (self.name) self.return_code = 'failed' elif (self.timeout_target is not None) and (rospy.Time.now() > self.timeout_target): self.status_text = "ExecuteKnownTrajectory - timeout waiting for %s to execute known trajectory(%f > %f (%f))" % (self.name, rospy.Time.now().to_sec(), self.timeout_target.to_sec(), self.timeout_duration.to_sec()) self.return_code = 'failed' #Logger.logwarn("ExecuteKnownTrajectory - %s (%f > %f (%f))" % (self.name, rospy.Time.now().to_sec(), self.timeout_target.to_sec(), self.timeout_duration.to_sec())) if (self.return_code is not None): Logger.logwarn( self.status_text ) userdata.status_text = self.status_text userdata.goal_names = self.goal_names userdata.goal_values = self.goal_values return self.return_code def on_enter(self, userdata): self.return_code = None self.status_text = None self.goal_names = None self.goal_values = None # Retrieve the relevant data try : if (self.given_action_topic is None): self.current_action_topic = userdata.action_topic except Exception as e: self.status_text = 'Failed to set up the action client for %s - invalid user data parameters\n%s' % (self.current_action_topic, str(e)) self.return_code = 'param_error' Logger.logerr(self.status_text) return try: if (self.action_client is None): self.action_client = ProxyActionClient({self.current_action_topic: ExecuteKnownTrajectoryAction}, self.wait_duration) if not self.action_client.is_available(self.current_action_topic): # Try to re-connect if not available self.action_client.setupClient(self.current_action_topic, ExecuteKnownTrajectoryAction, self.wait_duration) if not self.action_client.is_available(self.current_action_topic): self.status_text = '%s - no connection to %s available!' % (self.name, self.current_action_topic) self.return_code = 'param_error' Logger.loginfo(self.status_text) return except Exception as e: self.status_text = 'Failed to set up the action client for %s\n%s' % (self.current_action_topic, str(e)) self.return_code = 'param_error' Logger.logerr(self.status_text) return try: # Action Initialization action_goal = ExecuteKnownTrajectoryGoal() action_goal.trajectory = userdata.trajectory # Check that our trajectory is still fresh elapsed = rospy.Time.now() - action_goal.trajectory.joint_trajectory.header.stamp if self.max_delay is not None and elapsed > self.max_delay: self.status_text = '%s - Stale trajectory - %f > %f - do not send action for %s' % (self.name, elapsed.to_sec(), self.max_delay.to_sec(), self.current_action_topic ) self.return_code = 'param_error' Logger.logerr(self.status_text) return elif elapsed.to_sec() > 2.0: Logger.logwarn("%s - trajectory older than %f - send anyway!" % (self.name, elapsed.to_sec())) # Reset the timestamp before sending as we assume execute from begining in this state action_goal.trajectory.joint_trajectory.header.stamp = rospy.Time(0) self.action_client.send_goal(self.current_action_topic, action_goal) if (self.timeout_duration > rospy.Duration(0.0)): self.timeout_target = rospy.Time.now() + action_goal.trajectory.joint_trajectory.points[-1].time_from_start + self.timeout_duration else: self.timeout_target = None self.goal_names = action_goal.trajectory.joint_trajectory.joint_names self.goal_values = action_goal.trajectory.joint_trajectory.points[-1].positions except Exception as e: self.status_text = 'Failed to send ExecuteKnownTrajectoryGoal for %s\n%s' % (self.name, str(e)) self.return_code = 'param_error' Logger.logerr(self.status_text) def on_stop(self): try: if ( self.action_client.is_available(self.current_action_topic) \ and not self.action_client.has_result(self.current_action_topic) ): # Cancel any active goals self.action_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_pause(self): try: self.action_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 command self.on_enter(userdata)
class JointValuesToMoveActionState(EventState): ''' State to send a specified joint configuration to a specified MoveGroup move action. This state attempts a plan and execute move to a specified move group action interface using joint names and values specified in parameters; the number of joint values must match the given joint names, and the joint names should be consistent with the specified move group This state uses the basic MoveGroupAction interface with minimal configuration. -- action_topic string Topic on which MoveIt is listening for action calls. (default: None - use userdata ) -- move_group string Name of the move group to be used for planning. (default: None - use userdata ) -- joint_names string[] List of joint names Does not need to set all joints in group. Same order as their corresponding names in joint_values. -- joint_values float[] List of joint values Same order as their corresponding names in joint_names. -- joint_tolerance float Joint tolerance (above and below) (default: 0.0 use srdf limits) -- constraint_weight float How much to weight joint constraint relative to other constraints -- allowed_planning_time float Allowable planning time (seconds) -- wait_duration float How long to wait for action server (seconds) (default: 0 seconds) -- timeout float How long to wait for execution completion after planning (default: 10 seconds) ># action_topic string Topic on which MoveIt is listening for action calls. ># move_group string Name of the move group to be used for planning. Pass the configuration data along for potential re-use later #> 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. #> joint_names string[] Names of the target joints. #> joint_values float[] Target configuration of the joints. <= reached Target joint configuration has been reached. <= param_error Invalid user data - cannot send plan request <= planning_failed Failed to find a plan to the given joint configuration. <= control_failed Failed to move the arm along the planned trajectory. <= failed Failed to send goal ''' def __init__(self, joint_names, joint_values, move_group=None, action_topic=None, joint_tolerance=0.0, constraint_weight=1.0, allowed_planning_time=2.0, wait_duration=0, timeout=10.0): ''' Constructor ''' super(JointValuesToMoveActionState, self).__init__(outcomes=[ 'reached', 'param_error', 'planning_failed', 'control_failed', 'failed' ], input_keys=['move_group', 'action_topic'], output_keys=[ 'status_text', 'move_group', 'action_topic', 'joint_names', 'joint_values' ]) self.client = None self.given_move_group = move_group self.given_action_topic = action_topic self.joint_names = joint_names self.joint_values = joint_values self.joint_tolerance = joint_tolerance self.constraint_weight = constraint_weight self.allowed_planning_time = allowed_planning_time self.wait_duration = wait_duration self.return_code = None self.status_text = None self.timeout_duration = rospy.Duration(timeout) self.timeout_target = None if (self.given_action_topic is not None) and (len(self.given_action_topic) > 0): self.client = ProxyActionClient( {self.given_action_topic: MoveGroupAction}, self.wait_duration) else: self.given_action_topic = None if (self.given_move_group is None) or (len(self.given_move_group) < 1): self.given_move_group = None self.current_action_topic = self.given_action_topic self.current_move_group = self.given_move_group def execute(self, userdata): ''' Execute this state ''' if self.return_code is not None: userdata.status_text = self.status_text return self.return_code if self.client.has_result(self.current_action_topic): result = self.client.get_result(self.current_action_topic) Logger.loginfo('Move Group - %s' % str( self.client.get_goal_status_text(self.current_action_topic))) if result.error_code.val == MoveItErrorCodes.SUCCESS: self.status_text = 'Move Group success! - %s' % str( result.error_code) self.return_code = 'reached' elif result.error_code.val == MoveItErrorCodes.PLANNING_FAILED: self.status_text = 'Move Group planning failed with result error code - %s' % str( result.error_code) self.return_code = 'planning_failed' elif result.error_code.val == MoveItErrorCodes.INVALID_MOTION_PLAN: self.status_text = 'Move Group invalid motion plan with result error code - %s' % str( result.error_code) self.return_code = 'planning_failed' elif result.error_code.val == MoveItErrorCodes.MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE: self.status_text = 'Move Group invalid motion plan due to environment change with result error code - %s' % str( result.error_code) self.return_code = 'control_failed' elif result.error_code.val == MoveItErrorCodes.CONTROL_FAILED: self.status_text = 'Move Group control failed for move action of group - %s (error code - %s)' % ( self.current_move_group, str(result.error_code)) self.return_code = 'control_failed' elif result.error_code.val == MoveItErrorCodes.UNABLE_TO_AQUIRE_SENSOR_DATA: self.status_text = 'Move Group cannot acquire sensor data with result error code - %s' % str( result.error_code) self.return_code = 'planning_failed' elif result.error_code.val == MoveItErrorCodes.TIMED_OUT: self.status_text = 'Move Group timeout with result error code - %s' % str( result.error_code) self.return_code = 'control_failed' elif result.error_code.val == MoveItErrorCodes.PREEMPTED: self.status_text = 'Move Group preempted with result error code - %s' % str( result.error_code) self.return_code = 'control_failed' elif result.error_code.val == MoveItErrorCodes.START_STATE_IN_COLLISION: self.status_text = 'Move Group start state in collision with result error code - %s' % str( result.error_code) self.return_code = 'planning_failed' elif result.error_code.val == MoveItErrorCodes.START_STATE_VIOLATES_PATH_CONSTRAINTS: self.status_text = 'Move Group start state violates path constraints with result error code - %s' % str( result.error_code) self.return_code = 'planning_failed' elif ( (result.error_code.val >= MoveItErrorCodes.INVALID_GROUP_NAME) and \ (result.error_code.val <= MoveItErrorCodes.START_STATE_VIOLATES_PATH_CONSTRAINTS) ): self.status_text = 'Move Group invalid request data with result error code - %s' % str( result.error_code) self.return_code = 'planning_failed' elif ( (result.error_code.val >= MoveItErrorCodes.SENSOR_INFO_STALE) and \ (result.error_code.val <= MoveItErrorCodes.FRAME_TRANSFORM_FAILURE) ): self.status_text = 'Move Group - system data failure result error code - %s' % str( result.error_code) self.return_code = 'planning_failed' elif result.error_code.val == MoveItErrorCodes.NO_IK_SOLUTION: self.status_text = 'Move Group no IK solution with result error code - %s' % str( result.error_code) self.return_code = 'planning_failed' elif result.error_code.val == MoveItErrorCodes.FAILURE: self.status_text = 'Move Group unknown failure with result error code - %s' % str( result.error_code) self.return_code = 'planning_failed' else: self.status_text = 'Move Group - failed with unknown error code - %s' % str( result.error_code) self.return_code = 'planning_failed' Logger.loginfo(self.status_text) userdata.status_text = self.status_text 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 self.status_text = "MoveGroup- %s request aborted by action server" % ( self.name) self.return_code = 'failed' 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 self.status_text = "MoveGroup - %s request rejected by action server" % ( self.name) self.return_code = 'failed' elif (self.timeout_target is not None) and (rospy.Time.now() > self.timeout_target): self.status_text = "MoveGroup - timeout waiting for %s response (%f > %f (%f))" % ( self.name, rospy.Time.now().to_sec(), self.timeout_target.to_sec(), self.timeout_duration.to_sec()) self.return_code = 'failed' #Logger.logwarn("ExecuteKnownTrajectory - %s (%f > %f (%f))" % (self.name, rospy.Time.now().to_sec(), self.timeout_target.to_sec(), self.timeout_duration.to_sec())) if (self.return_code is not None): Logger.logwarn(self.status_text) userdata.status_text = self.status_text return self.return_code def on_enter(self, userdata): self.return_code = None # Retrieve the relevant data try: if (len(self.joint_values) != len(self.joint_names)): Logger.logwarn( 'Mismatch in joint names and values (%d vs. %d) -' % (len(self.joint_values), len(self.joint_names))) self.return_code = 'param_error' return except Exception as e: Logger.logwarn('Failed to get relevant user data -\n%s' % (str(e))) self.return_code = 'param_error' return try: if (self.given_move_group is None): self.current_move_group = userdata.move_group if (self.given_action_topic is None): self.current_action_topic = userdata.action_topic if (self.client is None): self.client = ProxyActionClient( {self.current_action_topic: MoveGroupAction}, self.wait_duration) if (not self.client.is_available(self.current_action_topic)): Logger.logwarn('Action client is not available for %s' % (self.current_action_topic)) self.return_code = 'param_error' return 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 = 'param_error' return try: # Action Initialization action_goal = MoveGroupGoal() action_goal.request.start_state.is_diff = True # Flags start state as empty to use current state on server action_goal.request.group_name = self.current_move_group action_goal.request.allowed_planning_time = self.allowed_planning_time action_goal.planning_options.planning_scene_diff.robot_state.is_diff = True # Flags start state as empty to use current state on server 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_values[i], tolerance_above=self.joint_tolerance, tolerance_below=self.joint_tolerance, weight=self.constraint_weight)) action_goal.request.goal_constraints.append(goal_constraints) self.client.send_goal(self.current_action_topic, action_goal) if (self.timeout_duration > rospy.Duration(0.0)): self.timeout_target = rospy.Time.now() + rospy.Duration( self.allowed_planning_time) + self.timeout_duration else: self.timeout_target = None # Pass the configuration data along as user data userdata.move_group = self.current_move_group userdata.action_topic = self.current_action_topic userdata.joint_names = self.joint_names userdata.joint_values = self.joint_values except Exception as e: Logger.logwarn('Failed to send action goal for group - %s\n%s' % (self.current_move_group, str(e))) self.return_code = 'planning_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_pause(self): try: 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): self.on_enter(userdata)