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')))
