def _monitoring_execute(self, *args, **kwargs):
        new_status = None
        had_warning = False
        if (self._force_monitoring or self._is_controlled) and self._sub.has_buffered(self._diagnostics_topic):
            new_status = ""
            diag_msg = self._sub.get_from_buffer(self._diagnostics_topic)
            for status in diag_msg.status:
                if not status.name in self._monitored_keys.keys(): continue
                if status.level == DiagnosticStatus.WARN:
                    had_warning = True
                    if not status.name + "_warn" in self._sent_keys:
                        self._sent_keys.append(status.name + "_warn")
                        Logger.logwarn("%s: %s" % (status.name, status.message))
                if status.level == DiagnosticStatus.ERROR:
                    if not status.name + "_err" in self._sent_keys:
                        self._sent_keys.append(status.name + "_err")
                        Logger.logerr("%s: %s" % (status.name, status.message))
                    new_status = status.name

        if new_status == "":
            self._current_status = None
            new_status = None
            if not had_warning:
                self._sent_keys = list()

        if new_status is None or self._current_status == new_status:
            return self.__execute(*args, **kwargs)

        self._current_status = new_status

        return self._monitored_keys[self._current_status]
Esempio n. 2
0
 def _mirror_structure_callback(self, msg):
     Logger.localinfo("--> Creating behavior structure for mirror...")
     self._pub.publish('flexbe/mirror/structure',
                       self._build_structure_msg())
     Logger.localinfo("<-- Sent behavior structure for mirror.")
     # enable control of states since a mirror is listening
     self._enable_ros_control()
Esempio n. 3
0
 def _execute_current_state(self):
     # catch any exception and keep state active to let operator intervene
     try:
         outcome = super(OperatableStateMachine,
                         self)._execute_current_state()
         self._last_exception = None
     except Exception as e:
         outcome = None
         self._last_exception = e
         Logger.logerr('Failed to execute state %s:\n%s' %
                       (self.current_state_label, str(e)))
     # provide explicit sync as back-up functionality
     # should be used only if there is no other choice
     # since it requires additional 8 byte + header update bandwith and time to restart mirror
     if self._inner_sync_request and self.get_deep_state() is not None:
         self._inner_sync_request = False
         if self.id is None:
             self.parent._inner_sync_request = True
         else:
             msg = BehaviorSync()
             msg.behavior_id = self.id
             msg.current_state_checksum = zlib.adler32(
                 self.get_deep_state().path.encode()) & 0x7fffffff
             self._pub.publish('flexbe/mirror/sync', msg)
     return outcome
    def _monitoring_execute(self, *args, **kwargs):
        new_status = None
        had_warning = False
        if (self._force_monitoring
                or self._is_controlled) and self._sub.has_buffered(
                    self._diagnostics_topic):
            new_status = ""
            diag_msg = self._sub.get_from_buffer(self._diagnostics_topic)
            for status in diag_msg.status:
                if not status.name in self._monitored_keys.keys(): continue
                if status.level == DiagnosticStatus.WARN:
                    had_warning = True
                    if not status.name + "_warn" in self._sent_keys:
                        self._sent_keys.append(status.name + "_warn")
                        Logger.logwarn("%s: %s" %
                                       (status.name, status.message))
                if status.level == DiagnosticStatus.ERROR:
                    if not status.name + "_err" in self._sent_keys:
                        self._sent_keys.append(status.name + "_err")
                        Logger.logerr("%s: %s" % (status.name, status.message))
                    new_status = status.name

        if new_status == "":
            self._current_status = None
            new_status = None
            if not had_warning:
                self._sent_keys = list()

        if new_status is None or self._current_status == new_status:
            return self.__execute(*args, **kwargs)

        self._current_status = new_status

        return self._monitored_keys[self._current_status]
    def setupClient(self, topic, msg_type, wait_duration=10):
        """
        Tries to set up an action client for calling it later.
        
        @type topic: string
        @param topic: The topic of the action to call.
        
        @type msg_type: msg type
        @param msg_type: The type of messages of this action client.
        
        @type wait_duration: int
        @param wait_duration: Defines how long to wait for the given client if it is not available right now.
        """
        if topic not in ProxyActionClient._clients:
            client = actionlib.SimpleActionClient(topic, msg_type)
            t = Timer(1, self._print_wait_warning, [topic])
            t.start()
            available = client.wait_for_server(
                rospy.Duration.from_sec(wait_duration))
            warning_sent = False
            try:
                t.cancel()
            except Exception as ve:
                # already printed the warning
                warning_sent = True

            if not available:
                Logger.logerr("Action client %s timed out!" % topic)
            else:
                ProxyActionClient._clients[topic] = client
                if warning_sent:
                    Logger.loginfo("Finally found action client %s..." %
                                   (topic))
    def setupClient(self, topic, msg_type, wait_duration=10):
        """
        Tries to set up an action client for calling it later.
        
        @type topic: string
        @param topic: The topic of the action to call.
        
        @type msg_type: msg type
        @param msg_type: The type of messages of this action client.
        
        @type wait_duration: int
        @param wait_duration: Defines how long to wait for the given client if it is not available right now.
        """
        if topic not in ProxyActionClient._clients:
            client = actionlib.SimpleActionClient(topic, msg_type)
            t = Timer(1, self._print_wait_warning, [topic])
            t.start()
            available = client.wait_for_server(rospy.Duration.from_sec(wait_duration))
            warning_sent = False
            try:
                t.cancel()
            except Exception as ve:
                # already printed the warning
                warning_sent = True

            if not available:
                Logger.logerr("Action client %s timed out!" % topic)
            else:
                ProxyActionClient._clients[topic] = client
                if warning_sent:
                    Logger.loginfo("Finally found action client %s..." % (topic))
Esempio n. 7
0
 def _set_autonomy_level(self, msg):
     """ Sets the current autonomy level. """
     if OperatableStateMachine.autonomy_level != msg.data:
         Logger.localinfo('--> Autonomy changed to %d' % msg.data)
     if msg.data < 0:
         self.preempt()
     else:
         OperatableStateMachine.autonomy_level = msg.data
     self._pub.publish('flexbe/command_feedback',
                       CommandFeedback(command="autonomy", args=[]))
    def add_joint_values(self, joint_values):
        joint_names = ProxyMoveitClient._robot.get_joint_names(self._move_group)
        if len(joint_names) != len(joint_values):
            Logger.logwarn("Amount of given joint values (%d) does not match the amount of joints (%d)." % (len(joint_values), len(joint_names)))
            #return

        goal_constraints = Constraints()
        for i in range(len(joint_names)):
            goal_constraints.joint_constraints.append(JointConstraint(joint_name=joint_names[i], position=joint_values[i]))

        self._goal.request.goal_constraints.append(goal_constraints)
    def has_topic(self, topic):
        """
        Determines if the given topic is already subscribed.

        @type topic: string
        @param topic: The topic of interest.
        """
        Logger.logwarn(
            'Deprecated (ProxySubscriberCached): use "is_available(topic)" instead of "has_topic(topic)".'
        )
        return self.is_available(topic)
Esempio n. 10
0
 def _sync_callback(self, msg):
     Logger.localinfo("--> Synchronization requested...")
     msg = BehaviorSync()
     msg.behavior_id = self.id
     # make sure we are already executing
     self.wait(condition=lambda: self.get_deep_state() is not None)
     msg.current_state_checksum = zlib.adler32(
         self.get_deep_state().path.encode()) & 0x7fffffff
     self._pub.publish('flexbe/mirror/sync', msg)
     self._pub.publish('flexbe/command_feedback',
                       CommandFeedback(command="sync", args=[]))
     Logger.localinfo("<-- Sent synchronization message for mirror.")
Esempio n. 11
0
 def _attach_callback(self, msg):
     Logger.localinfo("--> Enabling control...")
     # set autonomy level
     OperatableStateMachine.autonomy_level = msg.data
     # enable control of states
     self._enable_ros_control()
     self._inner_sync_request = True
     # send command feedback
     cfb = CommandFeedback(command="attach")
     cfb.args.append(self.name)
     self._pub.publish('flexbe/command_feedback', cfb)
     Logger.localinfo("<-- Sent attach confirm.")
    def __init__(self):
        if not ProxyMoveitClient._is_initialized:
            ProxyMoveitClient._is_initialized = True
            Logger.loginfo("Initializing proxy MoveIt client...")

            moveit_commander.roscpp_initialize(sys.argv)
            ProxyMoveitClient._robot = moveit_commander.RobotCommander()
            ProxyMoveitClient._client = ProxyActionClient({ProxyMoveitClient._action_topic: MoveAction})

        self._goal = None
        self._move_group = ""
        self._result = None
        self._planner_id = "RRTConnectkConfigDefault"
        self._target_link_axis = []
Esempio n. 13
0
    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 is None:
                self._pub.publish(
                    self._request_topic,
                    OutcomeRequest(outcome=255, target=self.path))
                self._last_requested_outcome = None

            # request outcome because autonomy level is too low
            if not self._force_transition and (
                    not self.parent.is_transition_allowed(self.name, outcome)
                    or outcome is not None and self.is_breakpoint):
                if outcome != self._last_requested_outcome:
                    self._pub.publish(
                        self._request_topic,
                        OutcomeRequest(outcome=self.outcomes.index(outcome),
                                       target=self.path))
                    Logger.localinfo("<-- Want result: %s > %s" %
                                     (self.name, outcome))
                    StateLogger.log(
                        'flexbe.operator',
                        self,
                        type='request',
                        request=outcome,
                        autonomy=self.parent.autonomy_level,
                        required=self.parent.get_required_autonomy(outcome))
                    self._last_requested_outcome = outcome
                outcome = None

            # autonomy level is high enough, report the executed transition
            elif outcome is not None and outcome in self.outcomes:
                Logger.localinfo("State result: %s > %s" %
                                 (self.name, outcome))
                self._pub.publish(self._outcome_topic,
                                  UInt8(self.outcomes.index(outcome)))
                self._pub.publish(self._debug_topic,
                                  String("%s > %s" % (self.path, outcome)))
                if self._force_transition:
                    StateLogger.log('flexbe.operator',
                                    self,
                                    type='forced',
                                    forced=outcome,
                                    requested=self._last_requested_outcome)
                self._last_requested_outcome = None

        self._force_transition = False
        return outcome
    def _preemptable_execute(self, *args, **kwargs):
        if self._is_controlled and self._sub.has_msg(self._preempt_topic):
            self._sub.remove_last_msg(self._preempt_topic)
            self._pub.publish(self._feedback_topic,
                              CommandFeedback(command="preempt"))
            PreemptableState.preempt = True
            Logger.localinfo("--> Behavior will be preempted")

        if PreemptableState.preempt:
            if not self._is_controlled:
                Logger.localinfo("Behavior will be preempted")
            self._force_transition = True
            return self._preempted_name

        return self.__execute(*args, **kwargs)
    def __init__(self):
        if not ProxyMoveitClient._is_initialized:
            ProxyMoveitClient._is_initialized = True
            Logger.loginfo("Initializing proxy MoveIt client...")

            moveit_commander.roscpp_initialize(sys.argv)
            ProxyMoveitClient._robot = moveit_commander.RobotCommander()
            ProxyMoveitClient._client = ProxyActionClient(
                {ProxyMoveitClient._action_topic: MoveAction})

        self._goal = None
        self._move_group = ""
        self._result = None
        self._planner_id = "RRTConnectkConfigDefault"
        self._target_link_axis = []
    def get_behavior(self, be_id):
        """
        Provides the library entry corresponding to the given ID.

        @type be_id: int
        @param be_id: Behavior ID to look up.

        @return Corresponding library entry or None if not found.
        """
        try:
            return self._behavior_lib[be_id]
        except KeyError:
            Logger.logwarn("Did not find ID %d in libary, updating..." % be_id)
            self.parse_packages()
            return self._behavior_lib.get(be_id, None)
    def add_joint_values(self, joint_values):
        joint_names = ProxyMoveitClient._robot.get_joint_names(
            self._move_group)
        if len(joint_names) != len(joint_values):
            Logger.logwarn(
                "Amount of given joint values (%d) does not match the amount of joints (%d)."
                % (len(joint_values), len(joint_names)))
            #return

        goal_constraints = Constraints()
        for i in range(len(joint_names)):
            goal_constraints.joint_constraints.append(
                JointConstraint(joint_name=joint_names[i],
                                position=joint_values[i]))

        self._goal.request.goal_constraints.append(goal_constraints)
    def add_behavior(self, behavior_class, behavior_id):
        """
        Adds another behavior as part of this behavior.
        This other behavior should be declared as contained in the behavior manifest.

        @type behavior_class: class
        @param behavior_class: The class implementing the other behavior.

        @type behavior_id: string
        @param behavior_id: Unique identifier for this behavior instance.
        """
        if not hasattr(self, 'contains'):
            Logger.logerr(
                'Behavior was not initialized! Please call superclass constructor.'
            )
        instance = behavior_class()
        self.contains[behavior_id] = instance
    def get_from_buffer(self, topic):
        """
        Pops the oldest buffered message of the given topic.

        @type topic: string
        @param topic: The topic of interest.
        """
        if not ProxySubscriberCached._topics[topic]['buffered']:
            Logger.logwarn('Attempted to access buffer of non-buffered topic!')
            return None
        if len(ProxySubscriberCached._topics[topic]['msg_queue']) == 0:
            return None
        msg = ProxySubscriberCached._topics[topic]['msg_queue'][0]
        ProxySubscriberCached._topics[topic][
            'msg_queue'] = ProxySubscriberCached._topics[topic]['msg_queue'][
                1:]
        return msg
Esempio n. 20
0
 def _execute_single_state(self, state, force_exit=False):
     result = None
     try:
         with UserData(reference=self._userdata,
                       remap=self._remappings[state.name],
                       input_keys=state.input_keys,
                       output_keys=state.output_keys) as userdata:
             if force_exit:
                 state._entering = True
                 state.on_exit(userdata)
             else:
                 result = state.execute(userdata)
     except Exception as e:
         result = None
         self._last_exception = e
         Logger.logerr('Failed to execute state %s:\n%s' %
                       (self.current_state_label, str(e)))
     return result
 def _execute_unlock(self, target):
     if target == self.path or (self._locked and target == ''):
         target = self.path
         found_target = True
         self._locked = False
     else:
         found_target = self._parent.unlock(target)
     # provide feedback about unlock
     self._pub.publish(
         self._feedback_topic,
         CommandFeedback(command="unlock",
                         args=[target, target if found_target else ""]))
     if not found_target:
         Logger.logwarn(
             "Wanted to unlock %s, but could not find it in current path %s."
             % (target, self.path))
     else:
         Logger.localinfo("--> Unlocking in state %s" % target)
Esempio n. 22
0
    def reset_joint_constraints(self, move_group=None):
        '''
        Reset the planning options to match the defaults
        @param move_group     : string or list specifying a particular move group(s) (default: None - change all relevant )
        '''

        flag = False
        if move_group is None:
            for group in self._move_group_list:
                try:
                    ret = self.reset_joint_constraints(group)
                    if ret:
                        flag=ret
                except:
                    pass
            # Check to see that something was reset
            if flag:
                return flag
            else:
                raise Exception(" Failed to reset joint constraints for any relevant topic for move_group=(%s) !" % (str(move_group)))

        elif bool(move_group) and all([isinstance(elem,basestring) for elem in move_group]):
            # List of strings
            for group in move_group:
                try:
                    ret = self.reset_joint_constraints(group)
                    if ret:
                        flag=ret
                except:
                    pass
            # Check to see that something was reset
            if flag:
                return flag
            else:
                raise Exception(" Failed to reset joint constraints for any relevant topic for move_group=(%s) !" % (str(move_group)))
        else:
            # Base case to reset specified motion plan requests options
            try:
                # Reset the planning options for a given move group
                ProxyMoveItClient._joint_constraints[move_group] = copy.deepcopy(ProxyMoveItClient._default_joint_constraints[move_group])
                return True
            except Exception as e:
                Logger.logerr(" Invalid move group %s for joint constraints - not configured yet!" % (str(move_group)))
                return False
    def __init__(self, verbose=False):
        if not ProxyTrajectoryClient._is_initialized:
            ProxyTrajectoryClient._is_initialized = True
            Logger.loginfo(
                "Initializing proxy FollowJointTrajectory client...")

            ProxyTrajectoryClient._client = ProxyActionClient({
                ProxyTrajectoryClient._action_topic:
                FollowJointTrajectoryAction
            })

            ProxyTrajectoryClient._verbose = verbose

            # enable the IK Service
            ik_ns = "ExternalTools/" + ProxyTrajectoryClient._limb + "/PositionKinematicsNode/IKService"
            ProxyTrajectoryClient._iksvc = rospy.ServiceProxy(
                ik_ns, SolvePositionIK)
            rospy.wait_for_service(ik_ns, 5.0)

            # enable the Baxter
            ProxyTrajectoryClient._rs = baxter_interface.RobotEnable(
                baxter_interface.CHECK_VERSION)
            ProxyTrajectoryClient._init_state = ProxyTrajectoryClient._rs.state(
            ).enabled
            print("Enabling robot... ")
            ProxyTrajectoryClient._rs.enable()

            #  enable baxter limb interface
            ProxyTrajectoryClient._baxter_limb_interface = baxter_interface.limb.Limb(
                ProxyTrajectoryClient._limb)

            #  Get the names from joints
            ProxyTrajectoryClient._joint_name = [
                joint for joint in
                ProxyTrajectoryClient._baxter_limb_interface.joint_names()
            ]

            ProxyTrajectoryClient._gripper = baxter_interface.Gripper(
                ProxyTrajectoryClient._limb)

        self._goal = FollowJointTrajectoryGoal()
        self._result = None
        self._goal_time_tolerance = rospy.Time(0.1)
        self._goal.goal_time_tolerance = self._goal_time_tolerance
    def publish(self, topic, msg):
        """
        Publishes a message on the specified topic.

        @type topic: string
        @param topic: The topic to publish on.

        @type msg: message class (defined when created publisher)
        @param msg: The message to publish.
        """
        if topic not in ProxyPublisher._topics:
            Logger.logwarn('ProxyPublisher: topic %s not yet registered!' %
                           topic)
            return
        try:
            ProxyPublisher._topics[topic].publish(msg)
        except Exception as e:
            Logger.logwarn('Something went wrong when publishing to %s!\n%s' %
                           (topic, str(e)))
    def use_behavior(self,
                     behavior_class,
                     behavior_id,
                     default_keys=None,
                     parameters=None):
        """
        Creates a state machine implementing the given behavior to use it in the behavior state machine.
        Behavior has to be added first.

        @type behavior_class: class
        @param behavior_class: The class implementing the other behavior.

        @type behavior_id: string
        @param behavior_id: Same identifier as used for adding.

        @type default_keys: list
        @param default_keys: List of input keys of the behavior which should be ignored
                             and instead use the default values as given by the behavior.

        @type parameters: dict
        @param parameters: Optional assignment of values to behavior parameters.
                           Any assigned parameter will be ignored for runtime customization,
                           i.e., cannot be overwritten by a user who runs the behavior.
        """
        if behavior_id not in self.contains:
            Logger.logerr('Tried to use a behavior without adding it!')
            return None

        if parameters is not None:
            for parameter, value in parameters.items():
                setattr(self.contains[behavior_id], parameter, value)

        state_machine = self.contains[behavior_id]._get_state_machine()

        if default_keys is not None:
            state_machine._input_keys = list(
                set(state_machine._input_keys) - set(default_keys))
        for key in state_machine._input_keys:
            state_machine._own_userdata(remove_key=key)

        return state_machine
Esempio n. 26
0
    def setup_action_client(self, action_topic, action_type, wait_duration):
        """
        Tries to set up a MoveIt MoveGroup action client for calling it later.

        @param action_topic  : string - The topic of the action to call.
        @param action_type   : string - class definition name for interface
        @param wait_duration : Defines how long to wait for the given client if it is not available right now.
        """

        if not isinstance(action_topic,str):
            raise Exception(" ProxyMoveItClient - Invalid action topic %s " % ( action_topic ))

        if action_topic not in ProxyMoveItClient._action_clients:
            # We have not initialized this client yet
            Logger.loginfo("Initializing proxy MoveIt client for "+action_topic+" ...")

            try:
                ProxyMoveItClient._action_clients[action_topic] = ProxyActionClient({action_topic: eval(action_type) }, wait_duration)
            except Exception as e:
                Logger.logwarn("ProxyMoveItClient setup error - %s"% (str(e)))
                raise e
Esempio n. 27
0
    def _check_topic_available(self, topic, wait_duration=1):
        """
        Checks whether a topic is available.

        @type topic: string
        @param topic: The topic of the action.

        @type wait_duration: int
        @param wait_duration: Defines how long to wait for the given client if it is not available right now.
        """
        client = ProxyActionClient._clients.get(topic)
        if client is None:
            Logger.logerr(
                "Action client %s not yet registered, need to add it first!" %
                topic)
            return False
        t = Timer(.5, self._print_wait_warning, [topic])
        t.start()
        available = client.wait_for_server(
            rospy.Duration.from_sec(wait_duration))
        warning_sent = False
        try:
            t.cancel()
        except Exception:
            # already printed the warning
            warning_sent = True

        if not available:
            Logger.logerr("Action client %s timed out!" % topic)
            return False
        else:
            if warning_sent:
                Logger.loginfo("Finally found action client %s..." % (topic))
        return True
Esempio n. 28
0
    def get_group_state(self, config_name, group_name, robot_name=None ):
        '''
        Get named configuration from semantic description
        @param config_name  string name of configuration
        @param group_name  string   move group name that the configuration belongs to
        @param robot_name   string  robot (default uses current or first robot in SRDF)
        '''
        config = None
        try:
            # Find the designated robot (or first in file)

            if (robot_name is not None and robot_name != ProxyMoveItClient._robot_name):
                for r in ProxyMoveItClient._srdf_ET.iter('robot'):
                    if robot_name == r.attrib['name']:
                        ProxyMoveItClient._robot_name = r.attrib['name']
                        ProxyMoveItClient._robot_semantics = r
                        Logger.loginfo("   Switching proxy to robot %s " % (ProxyMoveItClient._robot_name) )

            if ProxyMoveItClient._robot_semantics is None:
                Logger.logwarn('Did not find robot name in SRDF - %s' % robot_name)
                return None


            # Find the desired configuration
            for c in ProxyMoveItClient._robot_semantics.iter('group_state'):
                if (group_name is None or group_name == '' or group_name == c.attrib['group']) \
                    and c.attrib['name'] == config_name:
                    config = c
                    break

            return config

        except Exception as e:
            Logger.logwarn('Unable to find named configuration in SRDF -\n%s' % str(e))
            return None
    def wait_for_any(self, topic, timeout=5.0):
        """
        Blocks until there are any subscribers to the given topic.

        @type topic: string
        @param topic: The topic to publish on.

        @type timeout: float
        @param timeout: How many seconds should be the maximum blocked time.
        """
        pub = ProxyPublisher._topics.get(topic)
        if pub is None:
            Logger.logerr(
                "Publisher %s not yet registered, need to add it first!" %
                topic)
            return False
        t = Timer(.5, self._print_wait_warning, [topic])
        t.start()
        available = self._wait_for_subscribers(pub, timeout)
        warning_sent = False
        try:
            t.cancel()
        except Exception:
            # already printed the warning
            warning_sent = True

        if not available:
            Logger.logerr("Waiting for subscribers on %s timed out!" % topic)
            return False
        else:
            if warning_sent:
                Logger.loginfo("Finally found subscriber on %s..." % (topic))
        return True
 def _manually_transitionable_execute(self, *args, **kwargs):
     if self._is_controlled and self._sub.has_buffered(
             self._transition_topic):
         command_msg = self._sub.get_from_buffer(self._transition_topic)
         self._pub.publish(
             self._feedback_topic,
             CommandFeedback(command="transition",
                             args=[command_msg.target, self.name]))
         if command_msg.target != self.name:
             Logger.logwarn(
                 "Requested outcome for state %s but active state is %s" %
                 (command_msg.target, self.name))
         else:
             self._force_transition = True
             outcome = self.outcomes[command_msg.outcome]
             Logger.localinfo(
                 "--> Manually triggered outcome %s of state %s" %
                 (outcome, self.name))
             return outcome
     # otherwise, return the normal outcome
     self._force_transition = False
     return self.__execute(*args, **kwargs)
    def setupService(self, topic, msg_type, persistent=False, wait_duration=10):
        """
        Tries to set up a service caller for calling it later.
        
        @type topic: string
        @param topic: The topic of the service to call.
        
        @type msg_type: service class
        @param msg_type: The type of messages of this service.
        
        @type persistent: bool
        @param persistent: Defines if this service caller is persistent.
        
        @type wait_duration: int
        @param wait_duration: Defines how long to wait for the given service if it is not available right now.
        """
        if topic not in ProxyServiceCaller._services:
            warning_sent = False
            available = False
            try:
                t = Timer(1, self._print_wait_warning, [topic])
                t.start()
                rospy.wait_for_service(topic, wait_duration)
                available = True
            except rospy.exceptions.ROSException, e:
                available = False

            try:
                t.cancel()
            except Exception as ve:
                # already printed the warning
                warning_sent = True

            if not available:
                Logger.logerr("Service client %s timed out!" % topic)
            else:
                ProxyServiceCaller._services[topic] = rospy.ServiceProxy(topic, msg_type, persistent)
                if warning_sent:
                    Logger.loginfo("Finally found action client %s..." % (topic))
    def setupService(self, topic, msg_type, persistent=False, wait_duration=10):
        """
        Tries to set up a service caller for calling it later.
        
        @type topic: string
        @param topic: The topic of the service to call.
        
        @type msg_type: service class
        @param msg_type: The type of messages of this service.
        
        @type persistent: bool
        @param persistent: Defines if this service caller is persistent.
        
        @type wait_duration: int
        @param wait_duration: Defines how long to wait for the given service if it is not available right now.
        """
        if topic not in ProxyServiceCaller._services:
            warning_sent = False
            available = False
            try:
                t = Timer(1, self._print_warning, [topic])
                t.start()
                rospy.wait_for_service(topic, wait_duration)
                available = True
            except rospy.exceptions.ROSException, e:
                available = False

            try:
                t.cancel()
            except Exception as ve:
                # already printed the warning
                warning_sent = True

            if not available:
                Logger.logerr("Service client %s timed out!" % topic)
            else:
                ProxyServiceCaller._services[topic] = rospy.ServiceProxy(topic, msg_type, persistent)
                if warning_sent:
                    Logger.loginfo("Finally found action client %s..." % (topic))
    def find_behavior(self, be_name):
        """
        Searches for a behavior with the given name and returns it along with its ID.

        @type be_name: string
        @param be_name: Behavior ID to look up.

        @return Tuple (be_id, be_entry) corresponding to the name or (None, None) if not found.
        """
        find = lambda: next((id, be) for (id, be)  # noqa: E731 (allow lambda, only used locally here)
                            in self._behavior_lib.items()
                            if be["name"] == be_name)
        try:
            return find()
        except StopIteration:
            Logger.logwarn("Did not find behavior '%s' in libary, updating..." % be_name)
            self.parse_packages()
            try:
                return find()
            except StopIteration:
                Logger.logerr("Still cannot find behavior '%s' in libary after update, giving up!" % be_name)
                return None, None
    def get_sourcecode_filepath(self, be_id, add_tmp=False):
        """
        Constructs a file path to the source code of corresponding to the given ID.

        @type be_id: int
        @param be_id: Behavior ID to look up.

        @type add_tmp: bool
        @param add_tmp: Append "_tmp" to the file to consider a temporary version.

        @return String containing the absolute path to the source code file.
        """
        be_entry = self.get_behavior(be_id)
        if be_entry is None:
            # rely on get_behavior to handle/log missing package
            return None
        try:
            module_path = __import__(be_entry["package"]).__path__[-1]
        except ImportError:
            Logger.logwarn("Cannot import behavior package '%s', using 'rospack find' instead" % be_entry["package"])
            # rp can find it because otherwise, the above entry would not exist
            module_path = os.path.join(self._rp.get_path(be_entry["package"]), "src", be_entry["package"])
        filename = be_entry["file"] + '.py' if not add_tmp else '_tmp.py'
        return os.path.join(module_path, filename)
Esempio n. 35
0
    def get_goal_joint_constraints(self, move_group, joint_values, joint_names=None):
        '''
        Creates a JointConstraint for  list of goal constraints, and creates a new goal based on list
        of joint constraints for the current plan request specification

        @param move_group   : string specifying a particular move group
        @param joint_values : list of joint values (must match the given joint names)
        @param joint_names  : list of string joint names (default: None uses all joints in group)
        '''

        if joint_names is None:
            joint_names = ProxyMoveItClient._joint_names[move_group]

        if len(joint_names) != len(joint_values):
            Logger.logerr("Amount of given joint values (%d) does not match the amount of joints (%d) in group %s." % (len(joint_values), len(joint_names), move_group))
            raise Exception("Amount of given joint values (%d) does not match the amount of joints (%d) in group %s." % (len(joint_values), len(joint_names), move_group))

        joint_constraints = []
        for i, name in enumerate(joint_names):
            constraint = copy.deepcopy(ProxyMoveItClient._joint_constraints[move_group][name])
            constraint.position = joint_values[i]
            joint_constraints.append( constraint )

        return joint_constraints
 def _print_wait_warning(self, topic):
     Logger.logwarn("Waiting for action client %s..." % (topic))
 def _print_warning(self, topic):
     Logger.logwarn("Waiting for service client %s..." % (topic))