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 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 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 _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
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 __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 __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 __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 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
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 _check_service_available(self, topic, wait_duration=1): """ Checks whether a service is available. @type topic: string @param topic: The topic of the service. @type wait_duration: int @param wait_duration: Defines how long to wait for the given service if it is not available right now. """ client = ProxyServiceCaller._services.get(topic) if client is None: Logger.logerr( "Service client %s not yet registered, need to add it first!" % topic) return False 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: available = False try: t.cancel() except Exception: # already printed the warning warning_sent = True if not available: Logger.logerr("Service client %s timed out!" % topic) return False else: if warning_sent: Logger.loginfo("Finally found service %s..." % (topic)) return True
def load_semantic_description(self, semantic_description): ''' Load semantic description from the parameter server for later retrieval ''' if semantic_description is None: Logger.logwarn(" No semantic description file loaded !") return None # Check existence of SRDF parameter. # Values are read during runtime to allow modifications. srdf_param = None if rospy.has_param(semantic_description): srdf_param = rospy.get_param(semantic_description) else: Logger.logwarn('Unable to get SRDF parameter - %s' % (semantic_description )) return None try: ProxyMoveItClient._srdf_ET = ET.fromstring(srdf_param) Logger.loginfo(" Loaded %s SRDF file " % (semantic_description)) except Exception as e: Logger.logerr('Unable to parse given semantic description (SRDF) file - %s\n%s' % (semantic_description, str(e))) return None try: for r in ProxyMoveItClient._srdf_ET.iter('robot'): if ProxyMoveItClient._robot_name is None: ProxyMoveItClient._robot_name = r.attrib['name'] ProxyMoveItClient._robot_semantics = r Logger.loginfo(" Found robot %s in SRDF" % (ProxyMoveItClient._robot_name)) else: Logger.loginfo(" additional robot %s in SRDF - not used at this time " % (r.attrib['name'])) except Exception as e: Logger.logerr('Unable to parse given semantic description (SRDF) element tree for robot - %s\n%s' % (semantic_description, str(e))) return None
def __init__(self, robot_description="/robot_description", semantic_description=None, move_group_capabilities="/move_group", wait_duration=0.5): """ Creates a proxy for RobotCommander using a particular robot description. @param robot_description : string name of robot description (default: "robot_description") @param semantic_description : string name of robot semantic description (default: "None") @param move_group_capabilities : string or dictionary of topics by group name @param wait_duration : how long to wait for action server to become available """ if (robot_description is None): #Logger.loginfo("ProxyMoveItClient - calling with robot_description is None " ) try: if ProxyMoveItClient._robot_commander is None: Logger.loginfo("ProxyMoveItClient - No robot commander is currently initialized!" ) else: Logger.loginfo("ProxyMoveItClient - Using existing RobotCommander instance with %s" % (ProxyMoveItClient._robot_description)) except: Logger.loginfo("ProxyMoveItClient global not initialized yet " ) elif (ProxyMoveItClient._robot_commander is None): Logger.loginfo("Initializing robot data for proxy MoveIt client with %s ..." % robot_description) moveit_commander.roscpp_initialize(sys.argv) Logger.loginfo(" Loading robot commander for %s" % (robot_description)) ProxyMoveItClient._robot_commander = moveit_commander.RobotCommander(robot_description) ProxyMoveItClient._robot_description = robot_description # Load the semantic description as element tree to provide more direct access to data if semantic_description is None: semantic_description = robot_description+"_semantic" Logger.loginfo(" Loading semantic description parameter %s" % (semantic_description)) ProxyMoveItClient._semantic_description = self.load_semantic_description(semantic_description) # Get the active move groups for this set up ProxyMoveItClient._move_group_list = ProxyMoveItClient._robot_commander.get_group_names() Logger.loginfo(" Available move groups %s for this robot on proxy MoveIt client ..." % (str(ProxyMoveItClient._move_group_list))) for group in ProxyMoveItClient._move_group_list: joint_names = ProxyMoveItClient._robot_commander.get_joint_names(group) ProxyMoveItClient._joint_names[group] = [name for name in joint_names] Logger.loginfo(" %s : %s " % (group, str(ProxyMoveItClient._joint_names[group]))) # Set the default data for this move group self.set_global_defaults(group) try: action_topic=None if isinstance(move_group_capabilities, dict) and group in move_group_capabilities: # Allow for a dictionary of action topics per move group action_topic = move_group_capabilities[group] else: # Should be a single string for default single move group capability action_topic = move_group_capabilities if not isinstance(action_topic,str): raise Exception(" ProxyMoveItClient - Invalid action topic %s " % ( action_topic )) if action_topic not in ProxyMoveItClient._action_clients: # Defining a new MoveGroupAction client for this move group Logger.loginfo("Initializing proxy MoveGroup client for "+action_topic+" ...") ProxyMoveItClient._action_clients[action_topic] = ProxyActionClient({action_topic: eval("MoveGroupAction") }, wait_duration) # Store the topic name of the client reference in per move group dictionary as well ProxyMoveItClient._move_group_clients_[group] = action_topic except Exception as e: Logger.logwarn("ProxyMoveItClient MoveGroupAction client setup error - %s"% (str(e))) raise e else: Logger.loginfo("Already initialized robot data for proxy MoveIt client - new call with %s ..." % robot_description) if robot_description is not None and robot_description != ProxyMoveItClient._robot_description: Logger.logerr(" Using existing Proxy with robot description %s vs. %s " % ( ProxyMoveItClient._robot_description, robot_description)) raise Exception(" ProxyMoveItClient - Requested Proxy with diffent robot description %s vs. %s - not supported!" % ( ProxyMoveItClient._robot_description, robot_description))