def __init__(self): rospy.init_node('move_robot', anonymous=True) publishers, subscribers = rostopic.get_topic_list() topic = '/om_with_tb3/cmd_vel' if topic not in [sub[0] for sub in subscribers]: topic = '/cmd_vel' self.pub = rospy.Publisher(topic, Twist, queue_size=10) self.mc = Twist()
def has_breadcrumbs(robot_name): prefix = "/{}/".format(robot_name) breadcrumbs = prefix + "breadcrumb/deploy" publishers, subscribers = rostopic.get_topic_list() for name, _, _ in subscribers: if name == breadcrumbs: return True return False
def is_marsupial(robot_name): prefix = "/{}/".format(robot_name) detach_name = prefix + "detach" publishers, subscribers = rostopic.get_topic_list() for name, _, _ in subscribers: if name == detach_name: rospy.loginfo('Marsupial: ' + name) return True return False
def get_topic(): try: topics = get_topic_list() content = [] for i in topics[0]: k = "" for j in i[2]: k += j + "," k = k[0:len(k) - 1] content.append([i[0], i[1], k]) return jsonify(status=0, result=content) except: return jsonify(status=-1, result=[])
def wait_for_bridge(robot_name): prefix = "/{}/".format(robot_name) last_sub = set([]) last_pub = set([]) same_num = 0 while True: publishers, subscribers = rostopic.get_topic_list() names_sub = set([a[0] for a in subscribers if a[0].startswith(prefix)]) names_pub = set([a[0] for a in publishers if a[0].startswith(prefix)]) status = "{} subscribers, {} publishers".format(len(names_sub), len(names_pub)) if names_sub == last_sub and names_pub == last_pub: if same_num >= 3: rospy.loginfo("bridge ready: " + status) break else: same_num += 1 else: same_num = 0 rospy.loginfo("waiting for bridge "+prefix+"... " + status) rospy.sleep(1) last_sub = names_sub last_pub = names_pub
def get_topic_data(master=None): pubs, subs = rostopic.get_topic_list(master=master) topic_data = {} # print(f"subs {len(subs)}") for topic in pubs: name = topic[0] if name not in topic_data: topic_data[name] = {} topic_data[name]['type'] = topic[1] topic_data[name]['subscribers'] = [] topic_data[name]['publishers'] = topic[2] # print(f"subs {len(pubs)}") for topic in subs: name = topic[0] if name not in topic_data: topic_data[name] = {} topic_data[name]['type'] = topic[1] topic_data[name]['publishers'] = [] else: pass # TODO(lucasw) check for collisions that have mismatched types topic_data[name]['subscribers'] = topic[2] return topic_data
def writeToFile(self): """ Function to be called on shutdown. Stops timer and writes data to a file """ parentDir = os.path.dirname(__file__) fname = os.path.abspath(os.path.join(parentDir, '..','results',self.filename+'.xlsx')) with pd.ExcelWriter(fname) as writer: rospy.loginfo("Writing results to file: {}".format(fname)) for topic in self.loglist: topic.timer.shutdown() if topic.logging_df.shape[0] > 2: topic.logging_df.to_excel(writer, sheet_name=topic.topic) if __name__=="__main__": rospy.init_node("Freq_logger") topics = rospy.get_param("/topics", default=None) if topics is None: rospy.logwarn("No topics specified. Looking for All topics") topics = rostopic.get_topic_list() # use the wrapper list holding object LL = LoggerList(topics) while not rospy.is_shutdown(): try: rospy.spin() except rospy.ROSInterruptException as e: rospy.logerr_once(e)
def find_topic(msg_type): pubs, _ = rostopic.get_topic_list() for topic, topic_type, _ in pubs: if topic_type == msg_type: return topic return None
def _instantiate_monitors(self): if self.is_instantiated: return True try: msg_class, real_topic, _ = rostopic.get_topic_class(self.topic_name, blocking=False) topic_type, _, _ = rostopic.get_topic_type(self.topic_name, blocking=False) except rostopic.ROSTopicException as e: self.event_callback("Topic %s type cannot be determined, or ROS master cannot be contacted" % self.topic_name, "warn") return False if real_topic is None: self.event_callback("Topic %s is not published" % self.topic_name, "warn") if self.signal_when_cfg["signal_when"].lower() == 'not published' and self.signal_when_cfg["safety_critical"]: self.signal_when_is_safe = False return False # if rate > 0 set in config then throttle topic at that rate if self.rate > 0: COMMAND_BASE = ["rosrun", "topic_tools", "throttle"] subscribed_topic = "/sentor/monitoring/" + str(self.thread_num) + real_topic command = COMMAND_BASE + ["messages", real_topic, str(self.rate), subscribed_topic] subprocess.Popen(command, stdout=open(os.devnull, "wb")) else: subscribed_topic = real_topic # find out topic publishing nodes master = rosgraph.Master(rospy.get_name()) try: pubs, _ = rostopic.get_topic_list(master=master) # filter based on topic pubs = [x for x in pubs if x[0] == real_topic] nodes = [] for _, _, _nodes in pubs: nodes += _nodes self.nodes = nodes except socket.error: self.event_callback("Could not retrieve nodes for topic %s" % self.topic_name, "warn") # Do we need a hz monitor? hz_monitor_required = False if self.signal_when_cfg["signal_when"].lower() == 'not published': hz_monitor_required = True for signal_lambda in self.signal_lambdas_config: if "when_published" in signal_lambda: if signal_lambda["when_published"]: hz_monitor_required = True if hz_monitor_required: self.hz_monitor = self._instantiate_hz_monitor(subscribed_topic, self.topic_name, msg_class) if self.signal_when_cfg["signal_when"].lower() == 'published': print "Signaling 'published' for "+ bcolors.OKBLUE + self.topic_name + bcolors.ENDC +" initialized" self.pub_monitor = self._instantiate_pub_monitor(subscribed_topic, self.topic_name, msg_class) self.pub_monitor.register_published_cb(self.published_cb) if self.signal_when_cfg["safety_critical"]: self.signal_when_is_safe = False elif self.signal_when_cfg["signal_when"].lower() == 'not published': print "Signaling 'not published' for "+ bcolors.BOLD + str(self.signal_when_cfg["timeout"]) + " seconds" + bcolors.ENDC +" for " + bcolors.OKBLUE + self.topic_name + bcolors.ENDC +" initialized" if len(self.signal_lambdas_config): print "Signaling expressions for "+ bcolors.OKBLUE + self.topic_name + bcolors.ENDC + ":" self.lambda_monitor_list = [] for signal_lambda in self.signal_lambdas_config: lambda_fn_str = signal_lambda["expression"] lambda_config = self.process_lambda_config(signal_lambda) if lambda_fn_str != "": print "\t" + bcolors.OKGREEN + lambda_fn_str + bcolors.ENDC + " ("+ bcolors.BOLD+"timeout: %s seconds" % lambda_config["timeout"] + bcolors.ENDC +")" lambda_monitor = self._instantiate_lambda_monitor(subscribed_topic, msg_class, lambda_fn_str, lambda_config) # register cb that notifies when the lambda function is True lambda_monitor.register_satisfied_cb(self.lambda_satisfied_cb) lambda_monitor.register_unsatisfied_cb(self.lambda_unsatisfied_cb) self.lambda_monitor_list.append(lambda_monitor) print "" self.is_instantiated = True return True
def test_cmd_list(self): import rostopic cmd = 'rostopic' s = '/add_two_ints' # test failures for invalid in [['-ps'], ['-b' 'file.bag', '-s'], ['-b' 'file.bag', '-p']]: try: rostopic.rostopicmain([cmd, 'list'] + invalid) self.fail("should have failed") except SystemExit: pass # test main entry rostopic.rostopicmain([cmd, 'list']) # test through running command topics = [ '/chatter', '/foo/chatter', '/bar/chatter', '/rosout', '/rosout_agg' ] with fakestdout() as b: rostopic.rostopicmain([cmd, 'list']) v = [x.strip() for x in b.getvalue().split('\n') if x.strip()] self.failIf(set(topics) - set(v)) # test through public function topics = [ '/chatter', '/foo/chatter', '/bar/chatter', '/rosout', '/rosout_agg' ] v = rostopic.get_topic_list() self.failIf(set(topics) - set(v)) # publishers-only topics = [ '/chatter', '/foo/chatter', '/bar/chatter', '/rosout', '/rosout_agg' ] with fakestdout() as b: rostopic.rostopicmain([cmd, 'list', '-p']) v = [x.strip() for x in b.getvalue().split('\n') if x.strip()] self.failIf(set(topics) - set(v)) self.failIf('/clock' in v) # subscribers-only topics = ['/rosout'] with fakestdout() as b: rostopic.rostopicmain([cmd, 'list', '-s']) v = [x.strip() for x in b.getvalue().split('\n') if x.strip()] self.failIf(set(topics) - set(v), "%s vs. %s" % (topics, v)) self.failIf('/chatter' in v) # turn on verbosity, not checking output as it's not as stable with fakestdout() as b: rostopic.rostopicmain([cmd, 'list', '-v']) v = b.getvalue() self.assert_("Published topics:" in v) self.assert_("Subscribed topics:" in v) with fakestdout() as b: rostopic.rostopicmain([cmd, 'list', '-vs']) v = b.getvalue() self.failIf("Published topics:" in v) self.assert_("Subscribed topics:" in v) with fakestdout() as b: rostopic.rostopicmain([cmd, 'list', '-vp']) v = b.getvalue() self.assert_("Published topics:" in v) self.failIf("Subscribed topics:" in v) # test with multiple topic names try: rostopic.rostopicmain([cmd, 'list', 'rosout', 'rosout_agg']) self.fail("should have caused parser error") except SystemExit: pass # test with resolved names for n in topics: with fakestdout() as b: rostopic.rostopicmain([cmd, 'list', n]) self.assertEquals(n, b.getvalue().strip()) # test with relative names with fakestdout() as b: rostopic.rostopicmain([cmd, 'list', 'rosout']) self.assertEquals('/rosout', b.getvalue().strip()) # test with namespaces with fakestdout() as b: rostopic.rostopicmain([cmd, 'list', '/foo']) self.assertEquals('/foo/chatter', b.getvalue().strip()) with fakestdout() as b: rostopic.rostopicmain([cmd, 'list', 'bar']) self.assertEquals('/bar/chatter', b.getvalue().strip())
#!/usr/bin/env python import rospy import rostopic import PyQt5 # print(rospy.get_published_topics()) # tpc = rostopic.get_topic_list() # print(tpc[1][1][1]) # print(tpc[0]) # print(len(tpc)) # print(len(tpc[1])) # print(len(tpc[1][7])) # print(rostopic.get_topic_list()) tpc = rostopic.get_topic_list() my_list = [] for x in range(len(tpc)): for y in range(len(tpc[x])): # for z in range(len(tpc[x][y])): for z in range(2): # print(tpc[x][y][z]) # print(tpc[x][y][z].find('ditto')) if tpc[x][y][z].find('ditto') >= 0: # print(tpc[x][y][z]) string_smth = tpc[x][y][z].partition('/')[2] to_go_to_list = string_smth.partition('/')[0] # print(to_go_to_list) if not to_go_to_list in my_list: my_list.append(to_go_to_list) arranged_list = [] for x in my_list: arranged_list.append(int(x.lstrip('ditto')))