def _get_topic_type(topic): """ subroutine for getting the topic type :returns: topic type, real topic name and fn to evaluate the message instance if the topic points to a field within a topic, e.g. /rosout/msg, ``(str, str, fn)`` """ try: val = _master_get_topic_types(rosgraph.Master('/rostopic')) except socket.error: raise ROSTopicIOException("Unable to communicate with master!") # exact match first, followed by prefix match matches = [(t, t_type) for t, t_type in val if t == topic] if not matches: matches = [(t, t_type) for t, t_type in val if topic.startswith(t + '/')] # choose longest match matches.sort(key=itemgetter(0), reverse=True) if matches: t, t_type = matches[0] if t_type == rosgraph.names.ANYTYPE: return None, None, None return t_type, t, msgevalgen(topic[len(t):]) else: return None, None, None
def rosnode_cleanup(): """ This is a semi-hidden routine for cleaning up stale node registration information on the ROS Master. The intent is to remove this method once Master TTLs are properly implemented. """ pinged, unpinged = rosnode_ping_all() if unpinged: master = rosgraph.Master(ID) print("Unable to contact the following nodes:") print('\n'.join(' * %s' % n for n in unpinged)) print( "Warning: these might include alive and functioning nodes, e.g. in unstable networks." ) print( "Cleanup will purge all information about these nodes from the master." ) print("Please type y or n to continue:") input = sys.stdin.readline() while not input.strip() in ['y', 'n']: input = sys.stdin.readline() if input.strip() == 'n': print('aborting') return cleanup_master_blacklist(master, unpinged) print("done")
def get_process_for_ros_node(self, node_name): target_node = None caller_id = '/experiment_manager' master = rosgraph.Master(caller_id) running_nodes = rosnode.get_node_names() for node in running_nodes: if node_name in node: if target_node is None: target_node = node else: rospy.logwarn("Multiple nodes found whose name " "contains " + node_name) return None if target_node is None: rospy.logwarn("Could not find a node whose name contains " + node_name) return None target_node_api = rosnode.get_api_uri(master, target_node) if not target_node_api: rospy.logwarn("Could not connect to " + target_node + "'s API") return None target_node = ServerProxy(target_node_api) target_node_pid = rosnode._succeed( target_node.getPid(caller_id)) # NOLINT rospy.loginfo('Registered %s node PID %i for resource ' 'usage tracking' % (node_name, target_node_pid)) return psutil.Process(target_node_pid)
def get_machines_by_nodes(): """ Find machines connected to nodes. This is a very costly procedure as it must do N lookups with the Master, where N is the number of nodes. @return: list of machines connected @rtype: [str] @raise ROSNodeIOException: if unable to communicate with master """ master = rosgraph.Master(ID) # get all the node names, lookup their uris, parse the hostname # from the uris, and then compare the resolved hostname against # the requested machine name. machines = [] node_names = get_node_names() for n in node_names: try: uri = master.lookupNode(n) h = urlparse.urlparse(uri).hostname if h not in machines: machines.append(h) except socket.error: raise ROSNodeIOException("Unable to communicate with master!") except rosgraph.MasterError: # it's possible that the state changes as we are doing lookups. this is a soft-fail continue return machines
def _sub_rosnode_listnodes(namespace=None, list_uri=False, list_all=False): """ Subroutine for rosnode_listnodes(). Composes list of strings to print to screen. @param namespace: (default None) namespace to scope list to. @type namespace: str @param list_uri: (default False) return uris of nodes instead of names. @type list_uri: bool @param list_all: (default False) return names and uris of nodes as combined strings @type list_all: bool @return: new-line separated string containing list of all nodes @rtype: str """ master = rosgraph.Master(ID) nodes = get_node_names(namespace) nodes.sort() if list_all: return '\n'.join([ "%s \t%s" % (get_api_uri(master, n) or 'unknown address', n) for n in nodes ]) elif list_uri: return '\n'.join([(get_api_uri(master, n) or 'unknown address') for n in nodes]) else: return '\n'.join(nodes)
def is_ros_running(): try: rosgraph.Master("/rostopic").getPid() except socket.error: return False return True
def __init__(self, node_name): self.node_name = node_name if "ROS_MASTER_URI" not in os.environ: os.environ["ROS_MASTER_URI"] = GazeboHandler.DEFAULT_MASTER_URI logger.warning( 'ROS master URI not defined. Setting {} as default ROS master'. format(GazeboHandler.DEFAULT_MASTER_URI)) try: rosgraph.Master('/rostopic').getPid() logger.info('ROS master communication: ok') except socket.error as e: logging.error("Unable to communicate with ROS master!") raise e try: rospy.init_node(node_name, anonymous=True) logger.info("{}: gym ROS node created.".format(node_name)) except rospy.exceptions.ROSException as re: logger.error( "{}: exception raised creating gym ROS node. {}".format( node_name, re)) raise re self.un_pause_service = rospy.ServiceProxy('/gazebo/unpause_physics', Empty) self.pause_service = rospy.ServiceProxy('/gazebo/pause_physics', Empty) self.reset_service = rospy.ServiceProxy('/gazebo/reset_simulation', Empty) self.reset_world_service = rospy.ServiceProxy( '/gazebo/reset_simulation', Empty) self.set_model_state = rospy.ServiceProxy("/gazebo/set_model_state", SetModelState)
def probe_all_services(ctx): master = rosgraph.Master('/roswtf') errors = [] for service_name in ctx.services: try: service_uri = master.lookupService(service_name) except: ctx.errors.append( WtfError("cannot contact ROS Master at %s" % rosgraph.rosenv.get_master_uri())) raise WtfException( "roswtf lost connection to the ROS Master at %s" % rosgraph.rosenv.get_master_uri()) try: headers = rosservice.get_service_headers(service_name, service_uri) if not headers: errors.append("service [%s] did not return service headers" % service_name) except rosgraph.network.ROSHandshakeException as e: errors.append("service [%s] appears to be malfunctioning" % service_name) except Exception as e: errors.append("service [%s] appears to be malfunctioning: %s" % (service_name, e)) return errors
def __init__(self): #: rosgraph.Master: master API instance self.master = rosgraph.Master(rospy.get_name()) #: dict: dict structure of connections, by type. self.connections = create_empty_connection_type_dictionary( connection_types)
def main(): rospy.init_node('start_user_mon', anonymous=True) try: from xmlrpc.client import ServerProxy except ImportError: from xmlrpclib import ServerProxy ID = '/rosnode' master = rosgraph.Master(ID, master_uri=None) global process, loc process = {} loc = rospy.get_param("/script_location") while not rospy.is_shutdown(): nodes = rosnode.get_node_names() for node in nodes: name = " " + node name = name[2:] node_api = rosnode.get_api_uri(master, node) if not node_api: continue node = ServerProxy(node_api) pid = rosnode._succeed(node.getPid(ID)) if process.has_key(pid): continue process[pid] = name print("nohup " + loc + "/usage-mon " + str(pid) + " " + name + " &") os.system("nohup " + loc + "/usage-mon " + str(pid) + " " + name + " &")
def getString(topic): try: val = rosgraph.Master('/rostopic').getTopicTypes() except socket.error: raise ROSTopicIOException("Unable to communicate with master!") # exact match first, followed by prefix match matches = [(t, t_type) for t, t_type in val if t == topic] if not matches: matches = [(t, t_type) for t, t_type in val if topic.startswith(t+'/')] # choose longest match matches.sort(key=itemgetter(0), reverse=True) if matches: msg = matches[0][1] rospack = rospkg.RosPack() search_path = {} for p in rospack.list(): search_path[p] = [os.path.join(rospack.get_path(p), 'msg')] context = genmsg.MsgContext.create_default() try: spec = genmsg.load_msg_by_type(context, msg, search_path) genmsg.load_depends(context, spec, search_path) except Exception as e: raise ROSMsgException("Unable to load msg [%s]: %s"%(msg, e)) str = spec_to_str(context,spec) str = str.replace('\n',' ') str = ' '.join(str.split()) return str
def resolve_local_gateway(timeout=rospy.rostime.Duration(1.0)): ''' @param timeout : timeout on checking for the gateway. @type rospy.rostime.Duration @raise rocon_python_comms.NotFoundException: if no remote gateways or no matching gateways available. ''' master = rosgraph.Master(rospy.get_name()) gateway_namespace = None timeout_time = time.time() + timeout.to_sec() while not rospy.is_shutdown() and time.time() < timeout_time: unused_publishers, unused_subscribers, services = master.getSystemState( ) for service in services: service_name = service[0] # second part is the node name if re.search(r'remote_gateway_info', service_name): if service_name == '/remote_gateway_info': gateway_namespace = "/" break else: gateway_namespace = re.sub(r'/remote_gateway_info', '', service_name) break if gateway_namespace is not None: break else: rospy.rostime.wallsleep(0.1) if not gateway_namespace: raise rocon_python_comms.NotFoundException( "could not find a local gateway - did you start it?") #console.debug("Found a local gateway at %s"%gateway_namespace) return gateway_namespace
def _getOpenCameras(): poweredCameras = dict([(cam, False) for cam in CameraController._validCameras]) streamingCameras = dict([(cam, False) for cam in CameraController._validCameras]) # Using Cameras List service, get cameras that are recieving power rospy.wait_for_service('cameras/list') listService = rospy.ServiceProxy('cameras/list', ListCameras) try: resp = listService() for cam in resp.cameras: poweredCameras[cam] = True except rospy.ServiceException as err: raise OSError("error listing cameras: {}".format(err)) # NOTE (amal): it turns out to be unnecessary to know the streamingCameras # given how Baxter RSDK 1.1 works. Read the Readme for more details. # Using Rostopics, get the cameras that are currently streaming try: topics = rosgraph.Master('/rostopic').getPublishedTopics( '/cameras') for topic in topics: for cam in CameraController._validCameras: camTopic = "/cameras/%s/image" % cam if camTopic == topic[0]: streamingCameras[cam] = True except socket.error: raise ROSTopicIOException("Cannot communicate with master") return poweredCameras, streamingCameras
def get_topic_type_info(topic_name): """ subroutine for getting topic type information (nearly identical to rostopic._get_topic_type, except it returns rest of name instead of fn) :param topic_name: topic name of interest :type topic_name: str :returns: topic type, real topic name, and rest of name referenced if the topic points to a field within a topic, e.g. /rosout/msg, ``str, str, str`` """ try: node_name = rospy.get_name() master = rosgraph.Master(node_name) val = master.getTopicTypes() except: raise ROSException("unable to get list of topics from master") matches = [(t, t_type) for t, t_type in val if t == topic_name or topic_name.startswith(t + '/')] if matches: t, t_type = matches[0] if t_type == roslib.names.ANYTYPE: return None, None, None if t_type == topic_name: return t_type, None return t_type, t, topic_name[len(t):] else: return None, None, None
def __init__(self): try: rosgraph.Master('/rostopic').getPid() except Exception, e: print("Unable to communicate with master!") print("Try start ROSCORE or ROSLAUNCH some project") sys.exit(0)
def __init__(self, ros_master_uri, node_name, user_message_callback=None): self._user_message_callback = user_message_callback # set the ROS master uri that rospy will use os.environ['ROS_MASTER_URI'] = ros_master_uri # install an ugly hack to keep us from subscribing to ourselves _my_rospy_uri = None _orig_rospy_connect_cb = rospy.impl.registration.RegManager._connect_topic_thread def _subscribe_hack(self, topic, uri): if uri == _my_rospy_uri: return return _orig_rospy_connect_cb(self, topic, uri) rospy.impl.registration.RegManager._connect_topic_thread = _subscribe_hack rospy.init_node(node_name, anonymous=True) self._topics = {} self._topics_lock = threading.Lock() self._node_name = rospy.get_name() # create class for interfacing with the ROS master caller_id = rospy.get_caller_id() self._master = rosgraph.Master(caller_id) print 'My URI is: %s' % self._master.lookupNode(caller_id) _my_rospy_uri = self._master.lookupNode(caller_id)
def __init__(self): local_uri = rosgraph.get_master_uri() foreign_uri = rospy.get_param('~foreign_master', None) if not foreign_uri: raise Exception('foreign_master URI not specified') local_pubs = rospy.get_param('~local_pubs', []) foreign_pubs = rospy.get_param('~foreign_pubs', []) self._local_services = rospy.get_param('~local_services', []) self._foreign_services = rospy.get_param('~foreign_services', []) self.foreign_master = rosgraph.Master(rospy.get_name(), master_uri=foreign_uri) r = rospy.Rate(1) while not self.is_master_up(self.foreign_master) and not rospy.is_shutdown(): rospy.logdebug('Waiting for foreign master to come up...') r.sleep() self._local_manager = None self._foreign_manager = None if not rospy.is_shutdown(): self._local_manager = _RemoteManager(local_uri, self._local_publisher_update) self._foreign_manager = _RemoteManager(foreign_uri, self._foreign_publisher_update) for t in local_pubs: self._local_manager.subscribe(t) for t in foreign_pubs: self._foreign_manager.subscribe(t)
def __init__(self): super(TopicSelection, self).__init__() master = rosgraph.Master('rqt_bag_recorder') self.setWindowTitle("Select the topics you want to record") self.resize(500, 700) self.topic_list = [] self.selected_topics = [] self.items_list = [] self.area = QScrollArea(self) self.main_widget = QWidget(self.area) self.ok_button = QPushButton("Record", self) self.ok_button.clicked.connect(self.onButtonClicked) self.ok_button.setEnabled(False) self.main_vlayout = QVBoxLayout(self) self.main_vlayout.addWidget(self.area) self.main_vlayout.addWidget(self.ok_button) self.setLayout(self.main_vlayout) self.selection_vlayout = QVBoxLayout(self) self.item_all = QCheckBox("All", self) self.item_all.stateChanged.connect(self.updateList) self.selection_vlayout.addWidget(self.item_all) topic_data_list = master.getPublishedTopics('') topic_data_list.sort() for topic, datatype in topic_data_list: self.addCheckBox(topic) self.main_widget.setLayout(self.selection_vlayout) self.area.setWidget(self.main_widget) self.show()
def get_or_generate_uuid(options_runid, options_wait_for_master): """ :param options_runid: run_id value from command-line or ``None``, ``str`` :param options_wait_for_master: the wait_for_master command option. If this is True, it means that we must retrieve the value from the parameter server and need to avoid any race conditions with the roscore being initialized. ``bool`` """ # Three possible sources of the run_id: # # - if we're a child process, we get it from options_runid # - if there's already a roscore running, read from the param server # - generate one if we're running the roscore if options_runid: return options_runid # #773: Generate a run_id to use if we launch a master # process. If a master is already running, we'll get the # run_id from it instead param_server = rosgraph.Master('/roslaunch') val = None while val is None: try: val = param_server.getParam('/run_id') except: if not options_wait_for_master: val = roslaunch.core.generate_run_id() return val
def execute(self, inputs, outputs, gvm): port = inputs['port'] # it's actually just this command cmd = ['roscore'] if port != None: cmd.append('-p {}'.format(port)) # create the thread thread = Thread(target=startRosCoreFunction, args=( self, cmd, )) try: rosgraph.Master('/rostopic').getPid() except socket.error: #self.logger.warning("Unable to communicate with master!") # No ros core running -> start it return startRosCore(self, thread) # ros core active - restart if True if inputs['restart_if_running']: self.logger.info("ROS CORE already running - killing ROS CORE!") killRosCore(self) self.logger.info("Restarting ROS CORE!") return startRosCore(self, thread) self.logger.info("ROS CORE already running!") return "success"
def get_node_names(namespace=None): """ @param namespace: optional namespace to scope return values by. Namespace must already be resolved. @type namespace: str @return: list of node caller IDs @rtype: [str] @raise ROSNodeIOException: if unable to communicate with master """ master = rosgraph.Master(ID) try: state = master.getSystemState() except socket.error: raise ROSNodeIOException("Unable to communicate with master!") nodes = [] if namespace: # canonicalize namespace with leading/trailing slash g_ns = rosgraph.names.make_global_ns(namespace) for s in state: for t, l in s: nodes.extend( [n for n in l if n.startswith(g_ns) or n == namespace]) else: for s in state: for t, l in s: nodes.extend(l) return list(set(nodes))
def __init__(self): self._master = rosgraph.Master(rospy.get_name()) try: self.update() except Exception: self._system_state = [[], [], []] self._topic_types = []
def kill_nodes(node_names): """ Call shutdown on the specified nodes @return: list of nodes that shutdown was called on successfully and list of failures @rtype: ([str], [str]) """ master = rosgraph.Master(ID) success = [] fail = [] tocall = [] try: # lookup all nodes keeping track of lookup failures for return value for n in node_names: try: uri = master.lookupNode(n) tocall.append([n, uri]) except: fail.append(n) except socket.error: raise ROSNodeIOException("Unable to communicate with master!") for n, uri in tocall: # the shutdown call can sometimes fail to succeed if the node # tears down during the request handling, so we assume success try: p = xmlrpclib.ServerProxy(uri) _succeed(p.shutdown(ID, 'user request')) except: pass success.append(n) return success, fail
def _get_topic_type(topic): """ subroutine for getting the topic type (nearly identical to rostopic._get_topic_type, except it returns rest of name instead of fn) :returns: topic type, real topic name, and rest of name referenced if the topic points to a field within a topic, e.g. /rosout/msg, ``str, str, str`` """ try: master = rosgraph.Master('/rosplot') val = master.getTopicTypes() except: raise RosPlotException("unable to get list of topics from master") matches = [(t, t_type) for t, t_type in val if t == topic or topic.startswith(t + '/')] # Find the longest matching topic. So if user requests /foo/bar/baz and there are two topics, /foo and /foo/bar, # it will match /foo/bar and look for subfield .baz, instead of /foo looking for .bar.baz. # There is still ambiguity here, but hopefully this resolves it in the correct direction more often matches.sort(key=lambda x: len(x[0]), reverse=True) if matches: t, t_type = matches[0] if t_type == roslib.names.ANYTYPE: return None, None, None if t_type == topic: return t_type, None return t_type, t, topic[len(t):] else: return None, None, None
def rosnode_ping_all(verbose=False): """ Ping all running nodes @return [str], [str]: pinged nodes, un-pingable nodes @raise ROSNodeIOException: if unable to communicate with master """ master = rosgraph.Master(ID) try: state = master.getSystemState() except socket.error: raise ROSNodeIOException("Unable to communicate with master!") nodes = [] for s in state: for t, l in s: nodes.extend(l) nodes = list(set(nodes)) #uniq if verbose: print("Will ping the following nodes: \n" + ''.join([" * %s\n" % n for n in nodes])) pinged = [] unpinged = [] for node in nodes: if rosnode_ping(node, max_count=1, verbose=verbose): pinged.append(node) else: unpinged.append(node) return pinged, unpinged
def test_subscriber_appears_disappears(self): topic_type = rospy.get_param('~input_topic_type') check_connected_topics = rospy.get_param('~check_connected_topics') wait_time = rospy.get_param('~wait_for_connection', 0) msg_class = roslib.message.get_message_class(topic_type) # Subscribe topic and bond connection sub = rospy.Subscriber('~input', msg_class, self._cb_test_subscriber_appears_disappears) print('Waiting for connection for {} sec.'.format(wait_time)) rospy.sleep(wait_time) # Check assumed topics are there master = rosgraph.Master('test_connection') _, subscriptions, _ = master.getSystemState() for check_topic in check_connected_topics: for topic, sub_node in subscriptions: if topic == rospy.get_namespace() + check_topic: break else: raise ValueError('Not found topic: {}'.format(check_topic)) sub.unregister() # FIXME: below test won't pass on hydro ROS_DISTRO = os.environ.get('ROS_DISTRO', 'indigo') if ord(ROS_DISTRO[0]) < ord('i'): sys.stderr.write('WARNING: running on rosdistro %s, and skipping ' 'test for disconnection.\n' % ROS_DISTRO) return rospy.sleep(1) # wait for disconnection # Check specified topics do not exist _, subscriptions, _ = master.getSystemState() for check_topic in check_connected_topics: for topic, sub_node in subscriptions: if topic == rospy.get_namespace() + check_topic: raise ValueError('Found topic: {}'.format(check_topic))
def rosnode_info(node_name): """ Print information about node, including subscriptions and other debugging information. This will query the node over the network. @param node_name: name of ROS node @type node_name: str @raise ROSNodeIOException: if unable to communicate with master """ def topic_type(t, pub_topics): matches = [t_type for t_name, t_type in pub_topics if t_name == t] if matches: return matches[0] return 'unknown type' master = rosgraph.Master(ID) node_name = rosgraph.names.script_resolve_name('rosnode', node_name) print('-' * 80) print(get_node_info_description(node_name)) node_api = get_api_uri(master, node_name) if not node_api: print("cannot contact [%s]: unknown node" % node_name, file=sys.stderr) return print("\ncontacting node %s ..." % node_api) print(get_node_connection_info_description(node_api, master))
def get_live_message_types(): """ Gathers a list of all the types used in a running ROS system. Return: a list of types used in a running ROS system. """ print "Generating ROS Messages from rostopics" master = rosgraph.Master('/rostopic') rostopic_info = "" try: state = master.getSystemState() pubs, subs, _ = state topics_published_to = [item[0] for item in pubs] topics_subscribed_to = [item[0] for item in subs] unique_topics = list(set(topics_published_to + topics_subscribed_to)) types = [] for topic in unique_topics: topic_type = rostopic._get_topic_type(topic)[0] types.append(topic_type) output_list = set(types) print "Discovered %d types " % len(set(types)) return output_list except socket.error: raise ROSTopicIOException("Unable to communicate with master!")
def __init__(self, sample_rate=None, update_rate=None): self.sample_rate = sample_rate or rospy.Duration(0.1) self.update_rate = update_rate or rospy.Duration(2) # Generate a ROS compatible node name using the following options. # Preference Order: 1) ROS_HOSTNAME, 2) ROS_IP w/ underscores (10_0_0_5) # 3) hostname (if it is ROS naming compatible), 4) hashed hostname nodename = get_ros_hostname() or get_ros_ip() or get_sys_hostname() rospy.init_node('rosprofiler_%s' % nodename) self._master = rosgraph.Master(rospy.names.get_name()[1:]) self._lock = threading.Lock() self._monitor_timer = None self._publisher_timer = None self._graphupdate_timer = None # Data Structure for collecting information about the host self._host_monitor = HostMonitor() self._node_publisher = rospy.Publisher('/node_statistics', NodeStatistics, queue_size=10) self._host_publisher = rospy.Publisher('/host_statistics', HostStatistics, queue_size=10) # Processes we are watching self._nodes = dict()
def _restart_robin_node(cls, node_path): #TODO try to simplify """Restarts robin bridge node.""" # if node alive if rosnode.rosnode_ping(node_path, max_count=3): # kill node if node_path not in rosnode.kill_nodes([node_path])[0]: raise RuntimeError( "Failed to kill robin node '{}'.".format(node_path)) else: master = rosgraph.Master(rosnode.ID) rosnode.cleanup_master_blacklist(master, [node_path]) node_name = node_path.split('/')[-1] cls._wait_for(lambda: cls._get_node_path(node_name) == '') namespace = '/'.join(node_path.split('/')[:-1]) cmd = '''bash -c " cd {} && . *devel*/setup.bash && rosrun robin_bridge_generated robin_node __ns:={} & " > /dev/null 2>&1'''.format(catkin_ws, namespace) # " > /dev/null 2>&1'''.format(catkin_ws, node_path[:-len('/' + node_name)]) if os.system(cmd) != 0: raise RuntimeError('Failed to rerun robin node.') cls._wait_for(lambda: cls._get_node_path(node_name) != '', timeout=10)