def _operatable_execute(self, *args, **kwargs): outcome = self.__execute(*args, **kwargs) if self._is_controlled: log_requested_outcome = outcome # request outcome because autonomy level is too low if not self._force_transition and (self.autonomy.has_key(outcome) and self.autonomy[outcome] >= OperatableStateMachine.autonomy_level): if self._sent_outcome_requests.count(outcome) == 0: self._pub.publish(self._request_topic, OutcomeRequest(outcome=self._outcome_list.index(outcome), target=self._parent._get_path() + "/" + self.name)) rospy.loginfo("<-- Want result: %s > %s", self.name, outcome) StateLogger.log_state_execution(self._get_path(), self.__class__.__name__, outcome, not self._force_transition, False) self._sent_outcome_requests.append(outcome) outcome = OperatableState._loopback_name # autonomy level is high enough, report the executed transition elif outcome != OperatableState._loopback_name: self._sent_outcome_requests = [] rospy.loginfo("State result: %s > %s", self.name, outcome) self._pub.publish(self._outcome_topic, UInt8(self._outcome_list.index(outcome))) StateLogger.log_state_execution(self._get_path(), self.__class__.__name__, outcome, not self._force_transition, True) self._force_transition = False return outcome
def _operatable_execute(self, *args, **kwargs): outcome = self.__execute(*args, **kwargs) if self._is_controlled: # reset previously requested outcome if applicable if self._last_requested_outcome is not None and outcome == OperatableState._loopback_name: self._pub.publish(self._request_topic, OutcomeRequest(outcome=255, target=self._parent._get_path() + "/" + self.name)) self._last_requested_outcome = None # request outcome because autonomy level is too low if not self._force_transition and (self.autonomy.has_key(outcome) and self.autonomy[outcome] >= OperatableStateMachine.autonomy_level or outcome != OperatableState._loopback_name and self._get_path() in rospy.get_param('/flexbe/breakpoints', [])): if outcome != self._last_requested_outcome: self._pub.publish(self._request_topic, OutcomeRequest(outcome=self._outcome_list.index(outcome), target=self._parent._get_path() + "/" + self.name)) rospy.loginfo("<-- Want result: %s > %s", self.name, outcome) StateLogger.log_state_execution(self._get_path(), self.__class__.__name__, outcome, not self._force_transition, False) self._last_requested_outcome = outcome outcome = OperatableState._loopback_name # autonomy level is high enough, report the executed transition elif outcome != OperatableState._loopback_name: self._sent_outcome_requests = [] rospy.loginfo("State result: %s > %s", self.name, outcome) self._pub.publish(self._outcome_topic, UInt8(self._outcome_list.index(outcome))) self._pub.publish(self._debug_topic, String("%s > %s" % (self._get_path(), outcome))) StateLogger.log_state_execution(self._get_path(), self.__class__.__name__, outcome, not self._force_transition, True) self._force_transition = False return outcome
def _operatable_execute(self, *args, **kwargs): outcome = self.__execute(*args, **kwargs) if self._is_controlled: log_requested_outcome = outcome # request outcome because autonomy level is too low if not self._force_transition and ( self.autonomy.has_key(outcome) and self.autonomy[outcome] >= OperatableStateMachine.autonomy_level): if self._sent_outcome_requests.count(outcome) == 0: self._pub.publish( self._request_topic, OutcomeRequest( outcome=self._outcome_list.index(outcome), target=self._parent._get_path() + "/" + self.name)) rospy.loginfo("<-- Want result: %s > %s", self.name, outcome) StateLogger.log_state_execution(self._get_path(), self.__class__.__name__, outcome, not self._force_transition, False) self._sent_outcome_requests.append(outcome) outcome = OperatableState._loopback_name # autonomy level is high enough, report the executed transition elif outcome != OperatableState._loopback_name: self._sent_outcome_requests = [] rospy.loginfo("State result: %s > %s", self.name, outcome) self._pub.publish(self._outcome_topic, UInt8(self._outcome_list.index(outcome))) self._pub.publish( self._debug_topic, String("%s > %s" % (self._get_path(), outcome))) StateLogger.log_state_execution(self._get_path(), self.__class__.__name__, outcome, not self._force_transition, True) self._force_transition = False return outcome