def _wait_for_master(self): while True: time.sleep(0.1) if not self._wait_for_master_dialog: break try: rospy.get_master().getSystemState() except Exception: continue self._master_found_signal.emit(QMessageBox.Ok) break
def update_topic_list(self): self.topic_combo_box.clear() self.topic_combo_box.setEnabled(False) self.topic_combo_box.blockSignals(True) self.topic_combo_box.addItem('Updating...') # get topic list _, _, topic_type = rospy.get_master().getTopicTypes() topic_dict = dict(topic_type) # filter list topic_dict_filtered = dict() for k, v in topic_dict.items(): if (len(topic_type) == 0) or (v == self.topic_type): if (self.is_action_topic): topic_dict_filtered[k[:-5]] = v else: topic_dict_filtered[k] = v self.topic_combo_box.clear() self.topic_combo_box.addItems(sorted(topic_dict_filtered.keys())) if (self.topic_combo_box.count() > 0): self.topic_combo_box.setEnabled(True) self.topic_combo_box.blockSignals(False) self.topic_changed(self.topic_combo_box.currentText()) else: self.topic_combo_box.addItem('No topics available!')
def isMasterAlive(): """ return True if master alive and return False if master is not alive """ global previous_run_id try: # first check the host is available master_host = urlparse.urlsplit(rospy.core.rosgraph.get_master_uri()).hostname response = os.system("ping -W 10 -c 1 " + master_host + " > /dev/null") if response != 0: print "master machine looks down" return False assert 1 == rospy.get_master().getSystemState()[0] run_id = rospy.get_param("/run_id") if not previous_run_id: previous_run_id = run_id if run_id != previous_run_id: print "run_id is not same" previous_run_id = run_id return False return True except Exception, e: return False
def setUp(self): # first we setup our publishers and our node (used by rospy.resolve_name calls to remap topics) self.strpub = rospy.Publisher('/test/string', String, queue_size=1) self.emppub = rospy.Publisher('/test/empty', Empty, queue_size=1) self._master = rospy.get_master() self.service_if_pool = RosServiceIfPool() # we need to speed fast enough for the tests to not fail on timeout... rospy.set_param('/connection_cache/spin_freq', 2) # 2 Hz self.connection_cache_node = roslaunch.core.Node('rocon_python_comms', 'connection_cache.py', name='connection_cache', remap_args=[('~list', rospy.resolve_name('~connections_list')), ( '~diff', rospy.resolve_name('~connections_diff'))]) # Easier to remap the node topic to the proxy ones, instead of the opposite, since there is no dynamic remapping. # However for normal usecase, remapping the proxy handles is preferable. try: self.connection_cache_proc = self.launch.launch(self.connection_cache_node) except roslaunch.RLException as rlexc: raise nose.SkipTest("Connection Cache Node not found (part of rocon_python_comms pkg). Skipping test.") assert self.connection_cache_proc.is_alive() # wait for node to be started node_api = None with Timeout(5) as t: while not t.timed_out and node_api is None: node_api = rosnode.get_api_uri(rospy.get_master(), 'connection_cache') assert node_api is not None # make sure the connection cache node is started before moving on. self.connection_cache = connection_cache_proxy_create()
def _get_cart_status(self): """ If cart is not supposed to be attached, returns True If cart is supposed to be attached, checks last known distance of cart and returns True if cart is within threshold :returns: (bool, bool) """ is_cart_attached = False try: rospy.get_master().getPid() is_cart_attached = rospy.get_param(self.cart_attached_param_name, False) except: print('[cart_attachment_visual_3d_monitor] ROS master not running') if not is_cart_attached: return True, True if self.cart_state['cart_pose'] is None or \ (time.time() - self.cart_state['last_received_time']) > 10.0: self.toggle_cart_publisher_client = rospy.ServiceProxy(self.toggle_publisher_srv_name, ToggleObjectPublisher) self.toggle_cart_publisher_client(enable_publisher=True) return False, False dist = np.linalg.norm([self.cart_state['cart_pose'].position.x, self.cart_state['cart_pose'].position.y]) if (dist < self.nominal_plane_distance + self.plane_distance_tolerance and dist > self.nominal_plane_distance - self.plane_distance_tolerance): return True, True else: return False, False
def main(): parser = argparse.ArgumentParser( description='A script to test moveit planners. ' 'Should be run after launching the UR10 arm and shadow hand: \n ' 'roslaunch sr_robot_launch sr_right_ur10arm_hand.launch OR \n ' 'roslaunch sr_robot_launch sr_left_ur10arm_hand.launch', add_help=True, usage='%(prog)s [group_id]', formatter_class=argparse.RawTextHelpFormatter) parser.add_argument(dest='group_id', help="Should be either right_arm or left_arm") parser.parse_args() group_id = str(sys.argv[1]) try: rospy.get_master().getPid() except Exception: print("Please launch robot.") sys.exit() # sleep to wait for robot description to be loaded while not rospy.search_param( 'robot_description_semantic') and not rospy.is_shutdown(): print("Waiting for robot description, has robot been launched?") time.sleep(0.5) planner_list = [ "BKPIECEkConfigDefault", "ESTkConfigDefault", "KPIECEkConfigDefault", "LBKPIECEkConfigDefault", "PRMkConfigDefault", "RRTkConfigDefault", "SBLkConfigDefault", "PRMstarkConfigDefault", "RRTstarkConfigDefault" ] for planner in planner_list: TestPlanners(group_id, planner)
def _check_for_master(self): # check if master is available try: rospy.get_master().getSystemState() return except Exception: pass # spawn thread to detect when master becomes available self._wait_for_master_thread = threading.Thread( target=self._wait_for_master) self._wait_for_master_thread.start() self._wait_for_master_dialog = QMessageBox( QMessageBox.Question, self.tr('Waiting for ROS master'), self. tr("Could not find ROS master. Either start a 'roscore' or abort loading the plugin." ), QMessageBox.Abort) self._master_found_signal.connect(self._wait_for_master_dialog.done, Qt.QueuedConnection) button = self._wait_for_master_dialog.exec_() # check if master existence was not detected by background thread no_master = button != QMessageBox.Ok self._wait_for_master_dialog.deleteLater() self._wait_for_master_dialog = None if no_master: raise PluginLoadError( 'RosPyPluginProvider._init_node() could not find ROS master')
def __is_master_running(self): '''Returns True if a ROS master is running; returns False otherwise. ''' try: rospy.get_master().getPid() return True except: return False
def check_Ros_Master_status(): import rospy status = True try: rospy.get_master().getPid() except: status = False return status
def setUpClass(cls): cls.core = subprocess.Popen(['roscore']) rospy.init_node('test_driver') for _ in range(5): try: rospy.get_master() break except Exception as _: #pylint: disable=broad-except time.sleep(0.5)
def SafeConnect(): try: print "Cerium is connecting to ROS, please wait." rospy.get_master().getPid() # If an exception was not thrown, establish a connection. status = rospy.init_node('listener', anonymous=True) print "Cerium is connected to ROS!" return status except: print "Cerium cannot start due to a roscore issue." print "Please check the roscore configuration." exit()
def get_node_info(self, event): """ Get the list of all Nodes running on the host. Update the __node_list """ if not self.__is_enabled: pass if self.__is_enabled: nodes = [] for node_name in rosnode.get_node_names(): try: node_api = rosnode.get_api_uri(rospy.get_master(), node_name, skip_cache=True) if self._id in node_api[2]: nodes.append(node_name) except: pass for node in nodes: if node not in self.__node_list: """TODO currently not catching the exception here - master not running is a hard error so it does not make sense to continue running..""" node_api = rosnode.get_api_uri(rospy.get_master(), node, skip_cache=True) try: pid = self.get_node_pid(node_api, node) if not pid: continue node_process = psutil.Process(pid) new_node = NodeStatisticsHandler( self._id, node, node_process) self.__dict_lock.acquire(True) self.__node_list[node] = new_node self.__dict_lock.release() except psutil.NoSuchProcess: rospy.loginfo('pid of node %s could not be fetched' % node) continue self.__dict_lock.acquire() to_remove = [] for node_name in self.__node_list: if node_name not in nodes: rospy.logdebug('removing %s from host' % node_name) to_remove.append(node_name) for node_name in to_remove: self.remove_node(node_name) self.__dict_lock.release()
def WaitForRoscoreStart(self): bRoscoreFail = True print(bcolors.OKBLUE + "===========Waiting For Roscore=========" + bcolors.ENDC) while bRoscoreFail == True: bRoscoreFail = False try: rospy.get_master().getPid() except: bRoscoreFail = True rospy.sleep(1) print(bcolors.OKBLUE + "===========Roscore Running=========" + bcolors.ENDC)
def WaitForRoscoreClose(self): bRoscoreRunning = True print(bcolors.OKBLUE + "===========Waiting For Roscore to Close=========" + bcolors.ENDC) while bRoscoreRunning == True: try: rospy.get_master().getPid() except: bRoscoreRunning = False rospy.sleep(1) print(bcolors.OKBLUE + "===========Roscore Closed=========" + bcolors.ENDC)
def get_status(self): status_msg = self.get_status_message_template() status_msg['monitorName'] = self.config_params.name status_msg['monitorDescription'] = self.config_params.description status_msg['healthStatus'] = dict() master_running = True try: rospy.get_master().getPid() except: master_running = False status_msg['healthStatus']['status'] = master_running return status_msg
def isMasterAlive(): """ return True if master alive and return False if master is not alive """ global previous_run_id try: # first check the host is available master_host = urlparse.urlsplit( rospy.core.rosgraph.get_master_uri()).hostname response = os.system("ping -W 10 -c 1 " + master_host + " > /dev/null") if response != 0: print "master machine looks down" return False assert 1 == rospy.get_master().getSystemState()[0] run_id = rospy.get_param("/run_id") if not previous_run_id: previous_run_id = run_id if run_id != previous_run_id: print "run_id is not same" previous_run_id = run_id return False return True except Exception, e: return False
def setUp(self): # we need to speed fast enough for the tests to not fail on timeout... rospy.set_param('/connection_cache/spin_freq', 2) # 2 Hz self.connection_cache_node = roslaunch.core.Node( 'rocon_python_comms', 'connection_cache.py', name='connection_cache', remap_args=[ ('~list', '/pyros_ros/connections_list'), ('~diff', '/pyros_ros/connections_diff'), ]) try: self.connection_cache_proc = self.launch.launch( self.connection_cache_node) except roslaunch.RLException as rlexc: raise nose.SkipTest( "Connection Cache Node not found (part of rocon_python_comms pkg). Skipping test." ) node_api = None with Timeout(5) as t: while not t.timed_out and node_api is None: node_api = rosnode.get_api_uri(rospy.get_master(), 'connection_cache') assert node_api is not None # make sure the connection cache node is started before moving on. super(TestPyrosROSCache, self).setUp(enable_cache=True)
def search_namespace(self): cb = self.plugin_manager_widget.namespaceComboBox cb.blockSignals(True) cb.setEnabled(False) cb.clear() cb.addItem('Updating...') # get topic list _, _, topic_type = rospy.get_master().getTopicTypes() topic_dict = dict(topic_type) # filter list topic_dict_filtered = dict() for k, v in topic_dict.items(): if v == 'vigir_pluginlib_msgs/GetPluginStatesActionGoal': topic_dict_filtered[k] = v # update combo box with found namespaces cb.clear() namespaces = [ns[:-37] for ns in sorted(topic_dict_filtered.keys())] cb.addItems(namespaces) if cb.count() > 0: self.set_namespace(cb.currentText()) cb.setEnabled(True) cb.blockSignals(False) else: cb.addItem('No topics available!')
def listener(): master = rospy.get_master() # In ROS, nodes are uniquely named. If two nodes with the same # name are launched, the previous one is kicked off. The # anonymous=True flag means that rospy will choose a unique # name for our 'listener' node so that multiple listeners can # run simultaneously. rospy.init_node('listener', anonymous=True) rate = rospy.Rate(rospy.get_param("~rate")) # Hz rospy.Subscriber('chatter', String, callback, rate, queue_size=1) # spin() simply keeps python from exiting until this node is stopped #rospy.spin() # Implementation of spin() with additional check that ros master exists if not rospy.core.is_initialized(): raise rospy.exceptions.ROSInitException( "client code must call rospy.init_node() first") rospy.core.logdebug("node[%s, %s] entering spin(), pid[%s]", rospy.core.get_caller_id(), rospy.core.get_node_uri(), os.getpid()) try: while not rospy.core.is_shutdown(): try: pid = master.getPid() except: rospy.signal_shutdown('master killed') else: rospy.rostime.wallsleep(0.5) except KeyboardInterrupt: rospy.core.logdebug("keyboard interrupt, shutting down") rospy.core.signal_shutdown('keyboard interrupt')
def discover_controllers(self): rospy.logdebug("Discovering controllers") # see http://wiki.ros.org/ROS/Master_API master = rospy.get_master() system_state = master.getSystemState() cm_services = [ service for service, _provider in system_state[2][2] if LIST_CONTROLLERS_SRV_NAME in service ] for service in cm_services: list_controllers = rospy.ServiceProxy( service, controller_manager_msgs.srv.ListControllers) try: resp = list_controllers() except rospy.ServiceException as exc: rospy.logwarn("Service did not process request: " + str(exc)) else: for controller in resp.controller: if is_joint_trajectory_controller( controller) and is_active(controller): c = Controller() ns = service[:service.find(LIST_CONTROLLERS_SRV_NAME)] c.name = ns + "/" + controller.name c.topic = c.name + "/command" c.publisher = rospy.Publisher( c.topic, trajectory_msgs.msg.JointTrajectory, queue_size=1) c.joints = collect_joints(controller) rospy.logdebug("Added controller '" + c.name + ".") rospy.logdebug("--- topic: " + c.topic) rospy.logdebug("--- joints: " + ",".join(c.joints)) self.controllers.append(c)
def update_topic_list(self): self.topic_combo_box.clear() self.topic_combo_box.setEnabled(False) self.topic_combo_box.blockSignals(True) self.topic_combo_box.addItem('Updating...') # get topic list _, _, topic_type = rospy.get_master().getTopicTypes() topic_dict = dict(topic_type) # filter list topic_dict_filtered = dict() for k, v in topic_dict.items(): if (len(topic_type) == 0) or (v == self.topic_type): if self.is_action_topic: topic_dict_filtered[k[:-5]] = v else: topic_dict_filtered[k] = v self.topic_combo_box.clear() self.topic_combo_box.addItems(sorted(topic_dict_filtered.keys())) if self.topic_combo_box.count() > 0: self.topic_combo_box.setEnabled(True) self.topic_combo_box.blockSignals(False) self.topic_changed(self.topic_combo_box.currentText()) else: self.topic_combo_box.addItem('No topics available!')
def isMasterAlive(): """ return True if master alive and return False if master is not alive """ global previous_run_id try: # first check the host is available master = rospy.get_master() master_host = re.search('http://([a-zA-Z0-9\-_]*):', master.getUri()[2]).groups(1)[0] response = os.system("ping -W 10 -c 1 " + master_host + " > /dev/null") if response != 0: print "master machine looks down" return False master.getSystemState() run_id = rospy.get_param("/run_id") if not previous_run_id: previous_run_id = run_id if run_id != previous_run_id: print "run_id is not same" previous_run_id = run_id return False return True except Exception, e: return False
def get_process_ros(node_name, doprint=False): # get the node object from ros node_api = rosnode.get_api_uri(rospy.get_master(), node_name, skip_cache=True)[2] if not node_api: rospy.logerr("could not get api of node %s (%s)" % (node_name, node_api)) return False # now try to get the Pid of this process try: response = ServerProxy(node_api).getPid('/NODEINFO') except: rospy.logerr("failed to get of the pid of ros node %s (%s)" % (node_name, node_api)) return False # try to get the process using psutil try: process = psutil.Process(response[2]) if doprint: rospy.loginfo("adding new node monitor %s (pid %d)" % (node_name, process.pid)) return process except: rospy.logerr("unable to open psutil object for %s" % (response[2])) return False
def setup_pubsub(): """ Test basic publisher registering/unregistering functionality :return: """ setup() topicname = '/test/pubsub_test' master = rospy.get_master() pubs, subs, services = master.getSystemState()[2] assert_true(topicname not in [p[0] for p in pubs]) assert_true(topicname not in [s[0] for s in subs]) # create the publisher pub_test = rospy.Publisher(topicname, std_msgs.Empty, queue_size=1) time.sleep(5) # leave it time pubs, subs, services = master.getSystemState()[2] print("Publishers :") print([p[0] for p in pubs]) assert_true(topicname in [p[0] for p in pubs]) # create the subscriber sub_test = rospy.Subscriber(topicname, std_msgs.Empty, fake_sub_cb) time.sleep(2) # leave it time pubs, subs, services = master.getSystemState()[2] print("Subscribers:") print([s[0] for s in subs]) assert_true(topicname in [s[0] for s in subs]) return topicname, pub_test, sub_test
def test_rospy_unregister_sub(): """ Test basic subscriber registering/unregistering functionality :return: """ setup() topicname = '/test/sub_test' master = rospy.get_master() pubs, subs, services = master.getSystemState()[2] assert_true(topicname not in [s[0] for s in subs]) # create the publisher sub_test = rospy.Subscriber(topicname, std_msgs.Empty, fake_sub_cb) time.sleep(2) # leave it time pubs, subs, services = master.getSystemState()[2] assert_true(topicname in [s[0] for s in subs]) # FAIL ! Topic not registered sub_test.unregister() time.sleep(2) # leave it time pubs, subs, services = master.getSystemState()[2] assert_true(topicname not in [s[0] for s in subs]) teardown()
def run(self): try: master = rospy.get_master() master.getPid() self.ros_master_present = True except socket.error: return
def _update_thread_run(self): # update type_combo_box message_type_names = [] try: # this only works on fuerte and up packages = sorted([ pkg_tuple[0] for pkg_tuple in rosmsg.iterate_packages( self._rospack, rosmsg.MODE_MSG) ]) except: # this works up to electric packages = sorted(rosmsg.list_packages()) for package in packages: for base_type_str in rosmsg.list_msgs(package, rospack=self._rospack): message_class = roslib.message.get_message_class(base_type_str) if message_class is not None: message_type_names.append(base_type_str) self.type_combo_box.setItems.emit(sorted(message_type_names)) # update topic_combo_box _, _, topic_types = rospy.get_master().getTopicTypes() self._topic_dict = dict(topic_types) self.topic_combo_box.setItems.emit(sorted(self._topic_dict.keys()))
def get_node_list(): """ List all ROS Nodes Return: List containing ROS Nodes(name, pid) """ rospy.logdebug("GET_NODE_LIST:") node_array_temp = rosnode.get_node_names() node_list = [] j = 0 for node_name in node_array_temp: try: node_api = rosnode.get_api_uri(rospy.get_master(), node_name) if socket.gethostname() not in node_api[2] and socket.gethostbyname(socket.gethostname()) not in node_api[2]: # Only Get Info for Local Nodes continue code, msg, pid = xmlrpclib.ServerProxy(node_api[2]).getPid(ID) node_list.append(NODE(node_name, pid)) rospy.logdebug("Node_name: " + node_list[j].name + " Node_PID: " + str(node_list[j].pid)) j += 1 except socket_error as serr: pass rospy.logdebug("=============================") return node_list
def get_exe_path(node_name): ID = '/NODEINFO' node_api = rosnode.get_api_uri(rospy.get_master(), node_name) code, msg, pid = client.ServerProxy(node_api[2]).getPid(ID) p = psutil.Process(pid) #print(p.name()) return p.name()
def test_rospy_unregister_pub(): """ Test basic publisher registering/unregistering functionality :return: """ setup() topicname = '/test/pub_test' master = rospy.get_master() pubs, subs, services = master.getSystemState()[2] assert_true(topicname not in [p[0] for p in pubs]) # create the publisher pub_test = rospy.Publisher(topicname, std_msgs.Empty, queue_size=1) time.sleep(2) # leave it time pubs, subs, services = master.getSystemState()[2] assert_true(topicname in [p[0] for p in pubs]) pub_test.unregister() time.sleep(2) # leave it time pubs, subs, services = master.getSystemState()[2] assert_true(topicname not in [p[0] for p in pubs]) teardown()
def setUp(self): self.connection_cache_node = roslaunch.core.Node( 'rocon_python_comms', 'connection_cache.py', name='connection_cache', remap_args=[('~list', rospy.resolve_name('~connections_list')), ('~diff', rospy.resolve_name('~connections_diff'))]) # Easier to remap the node topic to the proxy ones, instead of the opposite, since there is no dynamic remapping. # However for normal usecase, remapping the proxy handles is preferable. try: self.connection_cache_proc = TestRosInterface.launch.launch( self.connection_cache_node) except roslaunch.RLException as rlexc: raise nose.SkipTest( "Connection Cache Node not found (part of rocon_python_comms pkg). Skipping test." ) assert self.connection_cache_proc.is_alive() # wait for node to be started node_api = None with timeout(5) as t: while not t.timed_out and node_api is None: node_api = rosnode.get_api_uri(rospy.get_master(), 'connection_cache') assert node_api is not None # make sure the connection cache node is started before moving on. self.strpub = rospy.Publisher('/test/string', String, queue_size=1) self.emppub = rospy.Publisher('/test/empty', Empty, queue_size=1) self.interface = RosInterface(True)
def get_node_info(self, event): """ Get the list of all Nodes running on the host. Update the __node_list """ if not self.__is_enabled: pass if self.__is_enabled: nodes = [] for node_name in rosnode.get_node_names(): try: node_api = rosnode.get_api_uri(rospy.get_master(), node_name, skip_cache=True) if self._id in node_api[2]: nodes.append(node_name) except : pass for node in nodes: if node not in self.__node_list: """TODO currently not catching the exception here - master not running is a hard error so it does not make sense to continue running..""" node_api = rosnode.get_api_uri(rospy.get_master(), node, skip_cache=True) try: pid = self.get_node_pid(node_api, node) if not pid: continue node_process = psutil.Process(pid) new_node = NodeStatisticsHandler( self._id, node, node_process) self.__dict_lock.acquire(True) self.__node_list[node] = new_node self.__dict_lock.release() except psutil.NoSuchProcess: rospy.loginfo('pid of node %s could not be fetched' % node) continue self.__dict_lock.acquire() to_remove = [] for node_name in self.__node_list: if node_name not in nodes: rospy.logdebug('removing %s from host' % node_name) to_remove.append(node_name) for node_name in to_remove: self.remove_node(node_name) self.__dict_lock.release()
def init_node(name, start_master=False): """ Register a client node with the master. Parameters ---------- name: str Name of the node. start_master: bool, default False If True, start a ROS master if one isn't already running. Returns ------- master: ROSLaunchParent or ROSMasterStub instance If a ROS master was started by this method, returns a ``ROSLaunchParent`` instance that can be used to shut down the master with its ``shutdown()`` method. Otherwise, a ``ROSMasterStub`` is returned that shows a warning when its ``shutdown()`` method is called. """ import roslaunch import rospy class ROSMasterStub: @staticmethod def shutdown(): warnings.warn( "ROS master was started somewhere else and cannot be shut " "down.") try: rospy.get_master().getPid() except ConnectionRefusedError: if start_master: master = roslaunch.parent.ROSLaunchParent("master", [], is_core=True) master.start() # make sure master is shut down on exit atexit.register(master.shutdown) else: raise RuntimeError("ROS master is not running.") else: master = ROSMasterStub() rospy.init_node(name) return master
def setupUi(self): self._ui.setupUi(self) _, _, topic_types = rospy.get_master().getTopicTypes() cmd_vel_topics = [topic[0] for topic in topic_types if topic[1] == "geometry_msgs/Twist"] self._ui.cmd_vel_topic_combo_box.setItems.emit(sorted(cmd_vel_topics)) # self.cmd_vel_publisher = rospy.Publisher(str(self.cmd_vel_topic_combo_box.currentText()), Twist) odom_topics = [topic[0] for topic in topic_types if topic[1] == "nav_msgs/Odometry"] self._ui.odom_topic_combo_box.setItems.emit(sorted(odom_topics))
def _check_for_master(self): # check if master is available try: rospy.get_master().getSystemState() return except Exception: pass # spawn thread to detect when master becomes available self._wait_for_master_thread = threading.Thread(target=self._wait_for_master) self._wait_for_master_thread.start() self._wait_for_master_dialog = QMessageBox(QMessageBox.Question, self.tr('Waiting for ROS master'), self.tr("Could not find ROS master. Either start a 'roscore' or abort loading the plugin."), QMessageBox.Abort) self._master_found_signal.connect(self._wait_for_master_dialog.done, Qt.QueuedConnection) button = self._wait_for_master_dialog.exec_() # check if master existence was not detected by background thread no_master = button != QMessageBox.Ok self._wait_for_master_dialog.deleteLater() self._wait_for_master_dialog = None if no_master: raise PluginLoadError('RosPyPluginProvider._init_node() could not find ROS master')
def _update_thread_run(self): _, _, topic_types = rospy.get_master().getTopicTypes() self._topic_dict = dict(topic_types) keys = list(self._topic_dict.keys()) namespaces = list() for i in keys: if i.endswith("get_blocks_action/goal"): namespaces.append(i[0:i.index("get_blocks_action/goal")]) self._widget.namespace_input.setItems.emit(namespaces)
def setUp(self): # Wait for topics self.topic_dict = {} while '/test_synergy/grasp_synergy' not in self.topic_dict: _, _, topic_types = rospy.get_master().getTopicTypes() self.topic_dict = dict(topic_types) print self.topic_dict rospy.sleep(0.02) rospy.sleep(0.1)
def setupUi(self): self._ui.setupUi(self) _, _, topic_types = rospy.get_master().getTopicTypes() cmd_vel_topics = [ topic[0] for topic in topic_types if topic[1] == 'geometry_msgs/Twist' ] self._ui.cmd_vel_topic_combo_box.setItems.emit(sorted(cmd_vel_topics)) #self.cmd_vel_publisher = rospy.Publisher(str(self.cmd_vel_topic_combo_box.currentText()), Twist) odom_topics = [ topic[0] for topic in topic_types if topic[1] == 'nav_msgs/Odometry' ] self._ui.odom_topic_combo_box.setItems.emit(sorted(odom_topics)) #self.odom_subscriber = rospy.Subscriber(str(self.odom_topic_combo_box.currentText()), Odometry, self.odometry_callback) core_sensor_topics = [ topic[0] for topic in topic_types if topic[1] == 'kobuki_msgs/SensorState' ] self._ui.core_topic_combo_box.setItems.emit(sorted(core_sensor_topics))
def isMasterAlive(): """ return True if master alive and return False if master is not alive """ try: master = rospy.get_master() master.getSystemState() return True except: return False
def _update_thread_run(self): """Update the list of namespaces.""" _, _, topic_types = rospy.get_master().getTopicTypes() self._topic_dict = dict(topic_types) keys = list(self._topic_dict.keys()) namespaces = list() for i in keys: print i if i.endswith("smach/container_status"): namespaces.append(i[0:i.index("smach/container_status")]) self._widget.namespace_input.setItems.emit(namespaces)
def get_node_info(self, node_name): node_api = rosnode.get_api_uri(rospy.get_master(), node_name) code, msg, pid = xmlrpclib.ServerProxy(node_api[2]).getPid(ID) if node_name in self.nodes: return self.nodes[node_name] else: try: p = psutil.Process(pid) self.nodes[node_name] = p return p except: return False
def __init__(self, node_name): super(ProcessInfoDisplay, self).__init__() self.node_name = node_name self.node_api = rosnode.get_api_uri(rospy.get_master(), node_name) code, msg, self.pid = xmlrpclib.ServerProxy(self.node_api[2]).getPid(ID) try: self.process = psutil.Process(self.pid) except: print 'Bad PID: %s, name: %s, code: %s, msg: %s' % (self.pid, self.node_name, code, msg) self.initUI() self.timer = QtCore.QTimer(self.parent()) self.timer.setInterval(1000) self.timer.timeout.connect(self.update_live_info) self.timer.start()
def main(): format = argparse.RawTextHelpFormatter parser = argparse.ArgumentParser(formatter_class=format) parser.add_argument( "-p", "--proceed", action="store_true", help="Continue testing after a failed test until all tests complete" ) parser.add_argument("-t", "--test", help=test_help()) args = parser.parse_args(rospy.myargv()[1:]) test_dict = { "version": None, "valid_tests": { "0.7.0": ["Enable", "Messages", "Services", "Head", "MoveArms", "Grippers", "BlinkLEDs", "Cameras"] }, } test_dict["version"] = get_version() if not test_dict["version"] in test_dict["valid_tests"].keys(): print ("Exiting: No tests specified for your software version: %s" % (test_dict["version"])) return 1 try: raw_input("Press <Enter> to Begin Smoke Test\n") except Exception: print ("\nExiting.") return 1 hostname = re.split("http://|.local", rospy.get_master().getUri()[2])[1] cur_time = time.localtime() filename = "%s-%s.%s.%s-rsdk-%s.smoketest" % ( hostname, cur_time.tm_mon, cur_time.tm_mday, cur_time.tm_year, test_dict["version"], ) if args.test == None: print "Performing All Tests" ros_init() for t in test_dict["valid_tests"][test_dict["version"]]: run_test(t, filename, args.proceed) elif args.test in test_dict["valid_tests"][test_dict["version"]]: ros_init() run_test(args.test, filename, args.proceed) else: print ("Exiting: Invalid test provided: %s for %s version software" % (args.test, test_dict["version"])) parser.print_help() return 0
def __init__(self, namespace, aggregation_topic, topic_blacklist=['rosout', 'rosout_agg', 'Aggregation', 'Aggregated'], topic_whitelist=[], check_rate=1): self.namespace = namespace self.aggregation_topic = aggregation_topic self.imported_packages = ["std_msgs", "teleop_msgs"] self.available_topics = {} self.blacklisted_topics = topic_blacklist self.whitelisted_topics = topic_whitelist self.topic_handlers = {} self.update_publisher = rospy.Publisher(aggregation_topic, SerializedMessage) self.master = rospy.get_master() rate = rospy.Rate(check_rate) rospy.loginfo("Loaded topic manager [TX]") while not rospy.is_shutdown(): self.update_available_topics() rate.sleep()
def isMasterAlive(): """ return True if master alive and return False if master is not alive """ try: master = rospy.get_master() master_host = re.search('http://([a-zA-Z0-9\-_]*):', master.getUri()[2]).groups(1)[0] response = os.system("ping -W 10 -c 1 " + master_host + " > /dev/null") if response != 0: return False master.getSystemState() return True except: return False
def get_node_info(self, node_name, skip_cache=False): node_api = rosnode.get_api_uri(rospy.get_master(), node_name, skip_cache=skip_cache) try: code, msg, pid = xmlrpclib.ServerProxy(node_api[2]).getPid(ID) if node_name in self.nodes: return self.nodes[node_name] else: try: p = psutil.Process(pid) self.nodes[node_name] = p return p except: return False except xmlrpclib.socket.error: if not skip_cache: return self.get_node_info(node_name, skip_cache=True) else: return False
def connect_to_ros(self, init_timeout=3): if self.connected_to_ros: return True # Attempt to connect to master node class InitNodeThread(threading.Thread): def __init__(self): threading.Thread.__init__(self) self.setDaemon(True) self.inited = False self.init_cv = threading.Condition() def run(self): rospy.loginfo('Master found. Connecting...') rospy.init_node('rxbag', anonymous=True, disable_signals=True) with self.init_cv: self.inited = True self.init_cv.notify_all() try: # Check whether ROS master is running master = rospy.get_master() master.getPid() # If so (i.e. no exception), attempt to initialize node init_thread = InitNodeThread() with init_thread.init_cv: init_thread.start() init_thread.init_cv.wait(init_timeout) if init_thread.inited: rospy.core.register_signals() rospy.loginfo('Connected to ROS master.') self.connected_to_ros = True return True else: rospy.logerr('Couldn\'t connect to ROS master.') except Exception: rospy.loginfo('ROS master not found.') return False
def _update_thread_run(self): # update type_combo_box message_type_names = [] try: # this only works on fuerte and up packages = sorted([pkg_tuple[0] for pkg_tuple in rosmsg.iterate_packages(self._rospack, rosmsg.MODE_MSG)]) except: # this works up to electric packages = sorted(rosmsg.list_packages()) for package in packages: for base_type_str in rosmsg.list_msgs(package, rospack=self._rospack): message_class = roslib.message.get_message_class(base_type_str) if message_class is not None: message_type_names.append(base_type_str) self.type_combo_box.setItems.emit(sorted(message_type_names)) # update topic_combo_box _, _, topic_types = rospy.get_master().getTopicTypes() self._topic_dict = dict(topic_types) self.topic_combo_box.setItems.emit(sorted(self._topic_dict.keys()))
def get_pid(name): try: pid = [p.pid for p in psutil.process_iter() if name in str(p.name)] return pid[0] except IndexError: pass try: node_id = '/NODEINFO' node_api = rosnode.get_api_uri(rospy.get_master(), name) code, msg, pid = xmlrpclib.ServerProxy(node_api[2]).getPid(node_id) except IOError: pass else: return pid try: return int(check_output(["pidof", "-s", name])) except CalledProcessError: pass rospy.logerr("Node '" + name + "' is not running!") return None
def __initRos(self): ''' Initializes Ros environment including * roscore * ardrone_driver * joy_node * ar_toolkit * own node ''' # ............................................................................. rospy.loginfo('stage 1: check roscore ....') # ............................................................................. roscoreRunning=False try: #check master master = rospy.get_master() master.getSystemState() roscoreRunning=True except: dlg = wx.MessageDialog(self, "Roscore is not running. Yudrone cannot work without roscore. Should it be executed by yudrone?", "Roscore is not running!", wx.YES_NO|wx.YES_DEFAULT|wx.ICON_QUESTION) result = dlg.ShowModal() dlg.Destroy() if result == wx.ID_YES: # open roscore as subprocess self.openRoscore() try: #check master again master = rospy.get_master() master.getSystemState() roscoreRunning=True except: roscoreRunning=False if roscoreRunning == False: rospy.logfatal('No sroscore; shutting down') self.Destroy() rospy.loginfo('Roscore is running') # load parameters setDefaultParameter() # ............................................................................. rospy.loginfo('stage 2: check network ....') # ............................................................................. ARDRONE_IP = rospy.get_param('yudrone/ARDRONE_IP') self.__ROSDIR = rospy.get_param('yudrone/ROSDIR') networkConnected = os.system('ping ' + ARDRONE_IP + ' -c 4 -i 0.2 -w 3 > /dev/null') if networkConnected != 0: rospy.logerr('not connected to ardrone (IP: ' + ARDRONE_IP + ')') dlg = wx.MessageDialog(self, "There is no ardrone connected (at IP= " + ARDRONE_IP + ")", "Network error", wx.OK|wx.ICON_ERROR) result = dlg.ShowModal() dlg.Destroy() else: rospy.loginfo('connected to ardrone (IP: ' + ARDRONE_IP + ')') # ............................................................................. rospy.loginfo('stage 3: check ardrone_driver ...') # ............................................................................. # check node self.ardroneDriverRunnig = os.system('rosnode list | grep /ardrone > /dev/null') if self.ardroneDriverRunnig != 0: # no driver dlg = wx.MessageDialog(self, "Ardrone_driver is not running. Should it be executed by yudrone?", "Ardrone_driver is not running!", wx.YES_NO|wx.YES_DEFAULT|wx.ICON_QUESTION) result = dlg.ShowModal() dlg.Destroy() if result == wx.ID_YES: # open roscore as subprocess self.openDriver() # check node again self.ardroneDriverRunnig = os.system('rosnode list | grep /ardrone > /dev/null') if self.ardroneDriverRunnig != 0: rospy.logerr('ardrone_driver is not running') else: rospy.loginfo('ardrone_driver is running') self.navdataCounter=0 # ............................................................................. rospy.loginfo('stage 4: check joy ....') # ............................................................................. # check joy_node joyNodeRunning = os.system('rosnode list | grep /joy_node > /dev/null') if joyNodeRunning != 0: # no joy_node dlg = wx.MessageDialog(self, "Joynode is not running. Should it be executed by yudrone?", "Joynode is not running!", wx.YES_NO|wx.YES_DEFAULT|wx.ICON_QUESTION) result = dlg.ShowModal() dlg.Destroy() if result == wx.ID_YES: # open joy_node as subprocess self.openJoyNode() # check joynode again joyNodeRunning = os.system('rosnode list | grep /joy_node > /dev/null') if joyNodeRunning != 0: rospy.logerr('joynode is not running') else: rospy.loginfo('joynode is running') # check yudrone_joy joypadCtrlRunning = os.system('rosnode list | grep /yudrone_joy > /dev/null') if joypadCtrlRunning != 0: # no joy_node dlg = wx.MessageDialog(self, "yudrone_joy is not running. Should it be executed by yudrone?", "yudrone_joy is not running!", wx.YES_NO|wx.YES_DEFAULT|wx.ICON_QUESTION) result = dlg.ShowModal() dlg.Destroy() if result == wx.ID_YES: # open joy_node as subprocess self.openJoyCtrl() # check yudrone_joy again joypadCtrlRunning = os.system('rosnode list | grep /yudrone_joy > /dev/null') if joypadCtrlRunning != 0: rospy.logerr('yudrone_joy is not running') else: rospy.loginfo('yudrone_joy is running') # ............................................................................. rospy.loginfo('stage 5: check ar_toolkit ....') # ............................................................................. arNodeRunning = os.system('rostopic list | grep /ar/image > /dev/null') if arNodeRunning != 0: # no ar_node dlg = wx.MessageDialog(self, "Ar_toolkit is not running. Should it be executed by yudrone?", "Ar_toolkit is not running!", wx.YES_NO|wx.YES_DEFAULT|wx.ICON_QUESTION) result = dlg.ShowModal() dlg.Destroy() if result == wx.ID_YES: # open ar-node as subprocess self.openArToolkit() # check ar_node again arNodeRunning = os.system('rostopic list | grep /ar/image > /dev/null') if arNodeRunning != 0: rospy.logerr('ar_node is not running') self.__imageTopic = '/ardrone/image_raw' else: rospy.loginfo('ar_node is running') self.__imageTopic = '/ar/image' # ............................................................................. rospy.loginfo('stage 6: check watchdog ....') # ............................................................................. wdNodeRunning = os.system('rosnode list | grep /yudrone_watchdog > /dev/null') if wdNodeRunning != 0: # no watchdog dlg = wx.MessageDialog(self, "Watchdog is not running. Should it be executed by yudrone?", "Watchdog is not running!", wx.YES_NO|wx.YES_DEFAULT|wx.ICON_QUESTION) result = dlg.ShowModal() dlg.Destroy() if result == wx.ID_YES: # open watchdog as subprocess self.openWatchdog() # check ar_node again wdNodeRunning = os.system('rostopic list | grep /ar/image > /dev/null') if wdNodeRunning != 0: rospy.logerr('watchdog is not running') else: rospy.loginfo('watchdog is running') # ............................................................................. rospy.loginfo('stage 7: init own node ...') # ............................................................................. #create node rospy.init_node('yudrone_flight') #subscribers self.sub_nav = rospy.Subscriber( "ardrone/navdata", Navdata, self.handle_navdata ) self.sub_tags = rospy.Subscriber( "tags", Tags, self.handle_tags ) # obtain tags (only for screen output) self.sub_yaw = rospy.Subscriber( "/cmd_vel", Twist, self.handle_twist ) # obtain twist msgs (only for screen output) self.sub_image = None rospy.sleep(0.1) #variables self.camState='front' # initially the camera is set to front on ardrone #stage 7: finalize ............................................ self.OnRBVideo() self.currYaw = Twist() rospy.loginfo('ROS initialized')
test_dict['version'] = get_version() if not test_dict['version'] in test_dict['valid_tests'].keys(): print("Exiting: No tests specified for your software version: %s" % \ (test_dict['version']) ) sys.exit(1) try: raw_input("Press <Enter> to Begin Smoke Test\n") except: print("\nExiting.") sys.exit(1) master_hostname = re.split( 'http://|.local', rospy.get_master().getUri()[2] )[1] cur_time = time.localtime() filename = ( "%s-%s.%s.%s-rsdk-%s.smoketest" % \ (master_hostname, cur_time.tm_mon, cur_time.tm_mday, \ cur_time.tm_year, test_dict['version'],) ) if args.test == None: print 'Performing All Tests' ros_init() for t in test_dict['valid_tests'][test_dict['version']]: run_test(t, filename, args.proceed) elif args.test in test_dict['valid_tests'][test_dict['version']]: ros_init() run_test(args.test, filename, args.proceed)
import matplotlib.pyplot as plt import matplotlib.patches as mpatches import numpy as np import cv2 from spinn_wrapper.msg import Spike import rwlock as lck cap = cv2.VideoCapture("/fzi/ids/mateev/catkin_ws/src/spinn_wrapper/src/spinn_wrapper/360x240.mp4") #Do this to make sure roscore starts before the script starts executing failed = True while failed == True: try: failed = False rospy.get_master().getPid() except: failed = True start_time = time.time() sources = [] #import a random grayscale image for testing purposes, extract 5 different values #img = cv2.imread('/fzi/ids/mateev/catkin_ws/src/spinn_wrapper/src/spinn_wrapper/img.jpg',0) #scaled_img = cv2.resize(img,(100,100)) pos_frame = cap.get(cv2.cv.CV_CAP_PROP_POS_FRAMES) fnumber = cap.get(cv2.cv.CV_CAP_PROP_FRAME_COUNT) while not cap.isOpened(): cap = cv2.VideoCapture("/fzi/ids/mateev/catkin_ws/src/spinn_wrapper/src/spinn_wrapper/360x240.mp4")
def poll(self): # Read the current system state from the ROS master publishers, subscribers, _ = map(dict, rospy.get_master().getSystemState()[2]) node_names = set(rosnode.get_node_names()) topic_types = rospy.get_master().getPublishedTopics('')[2] # Update the dictionary of topic types for topic, type in topic_types: if self.topic_types.get(topic, None) != type: self.updates.set_topic_type(topic, type) self.topic_types[topic] = type # Create new nodes (needed for nodes without any pub/sub topics) for name in node_names: self.node(name) # Link up published topics for topic in publishers: if topic in TOPIC_NAMES_TO_IGNORE: continue for name in publishers[topic]: if name in NODE_NAMES_TO_IGNORE: continue node = self.node(name) if node is not None: node.output(topic) node_names.add(name) # Link up subscribed topics for topic in subscribers: if topic in TOPIC_NAMES_TO_IGNORE: continue for name in subscribers[topic]: if name in NODE_NAMES_TO_IGNORE: continue node = self.node(name) if node is not None: node.input(topic) node_names.add(name) # Remove old node names killed_names = self.names_to_stop_avoiding - node_names self.names_to_stop_avoiding -= killed_names self.names_to_avoid -= killed_names # Remove old slots for name, node in self.nodes.items() + self.owned_nodes.items(): for input in node.inputs.values(): if input.topic not in subscribers or node.name not in subscribers[input.topic]: del node.inputs[input.topic] for output in node.outputs.values(): if output.topic not in publishers or node.name not in publishers[output.topic]: del node.outputs[output.topic] # Remove old nodes for name in self.nodes.keys(): if name not in node_names: del self.nodes[name] # Update owned nodes for name in self.owned_nodes: node = self.owned_nodes[name] # Check on the process if node.status == 'Starting...' and node.name in node_names: node.status = '' self.updates.update_owned_node(node) node.poll() # Remember all observed topics for when we restart this node in the future if node.path not in self.db.topics: self.db.topics[node.path] = set(), set() published, subscribed = self.db.topics[node.path] published |= set(topic for topic in node.outputs if node.outputs[topic].original_topic is None) subscribed |= set(topic for topic in node.inputs if node.inputs[topic].original_topic is None) # Save all observed topics self.db.save() # Keep polling killed child processes until they actually die for process in list(self.killed_processes): process.poll() if process.returncode is not None: self.killed_processes.remove(process) # Update launch files for launch_file in list(self.launch_files.values()): launch_file.poll()
def wait_topics(): _, _, topic_types = rospy.get_master().getTopicTypes() topic_dict = dict(topic_types) while "/allegroHand_test/joint_states" not in topic_dict and "/allegroHand_test/tf" not in topic_dict: rospy.sleep(0.02)