def add_trigger(topic, expr, select, index, wait=False):
global topics
if topic in topics:
topic_type = topics[topic]
topic_class = roslib.message.get_message_class(topic_type)
else:
topic_type, _, _ = rostopic.get_topic_type(topic)
topic_class, _, _ = rostopic.get_topic_class(topic)
if topic_type is None:
if wait is False:
rospy.loginfo('%s is not published yet', topic)
return None
elif wait is True:
rate = rospy.Rate(1)
while not rospy.is_shutdown() and topic_type is None:
topic_type, _, _ = rostopic.get_topic_type(topic)
topic_class, _, _ = rostopic.get_topic_class(topic)
rospy.loginfo('waiting topic %s' % topic)
rate.sleep()
else:
raise ValueError('wait should be bool')
if (topic_class is None):
rospy.loginfo('%s is not builded yet', topic_type)
return None
try:
cb_obj = gen_callback(expr, select, index)
rospy.loginfo("start subscribe (topic=%s type=%s)", topic, topic_type)
sub = rospy.Subscriber(topic, topic_class, cb_obj)
except Exception, e:
rospy.loginfo(str(e))
return None
def checkroscore(self, uri="localhost", port=11311):
"""
Checks if roscore is running
Parameters
-------------
uri: `string`
Specifies ROS_MASTER_URI
A valid IP address string or resolvable hostname
port: `integer` [0-65535]
port number where roscore will start
A valid port ranges from 0 to 65535 but note that not all port numbers are allowed.
Returns
---------
`boolean`
Returns `True` if roscore is running otherwise returns false
"""
os.environ["ROS_MASTER_URI"] = "http://" + uri + ":" + str(port)
roscore_status = False
try:
rostopic.get_topic_class('/rosout')
roscore_status = True
except rostopic.ROSTopicIOException as e:
roscore_status = False
pass
return roscore_status
def check_ros_running():
try:
rostopic.get_topic_class('/rosout')
return True
except:
print('WARNING: Unable to detect rosmaster. Is rosmaster running?\n')
return False
def CheckRosMaster(self):
# thanks to https://github.com/ros-visualization/rqt_robot_plugins/blob/eb5a4f702b5b5c92b85aaf9055bf6319f42f4249/rqt_moveit/src/rqt_moveit/moveit_widget.py#L251
try:
# Checkif rosmaster is running or not.
rostopic.get_topic_class('/rosout')
return True
except rostopic.ROSTopicIOException as e:
return False
def exceptionROS(self):
# Checkif rosmaster is running else run roscore
try:
rostopic.get_topic_class('/rosout') # is_rosmaster_running = True
except rostopic.ROSTopicIOException as e:
roscore = subprocess.Popen('roscore') # then start roscore yourself
time.sleep(1) # wait a bit to be sure the roscore is really launched
subprocess.Popen(["roslaunch", "Kinefly", "main.launch"]) # start kinefly
def is_core_running():
"""
Return True is the ROS core is running.
"""
try:
rostopic.get_topic_class('/roscore')
except rostopic.ROSTopicIOException as e:
return False
return True
def is_roscore_running():
"""
@rtype: bool
"""
try:
# Checkif rosmaster is running or not.
rostopic.get_topic_class('/rosout')
return True
except rostopic.ROSTopicIOException as e:
return False
def is_roscore_running():
"""
@rtype: bool
"""
try:
# Checkif rosmaster is running or not.
rostopic.get_topic_class('/rosout')
return True
except rostopic.ROSTopicIOException as e:
return False
def ros_core_is_running():
"""
Check whether roscore is running
:return:
"""
try:
# Checkif rosmaster is running or not.
rostopic.get_topic_class('/rosout')
is_rosmaster_running = True
except rostopic.ROSTopicIOException as e:
is_rosmaster_running = False
return is_rosmaster_running
def __init__(self, name):
self.name = name
self.last_status = 0
self.goal_received = rospy.get_rostime()
self.last_preemption = roslib.rostime.Time(0.0)
try:
self.activate_class, _, __ = rostopic.get_topic_class(self.name + '/activate')
self.feedback_class, _, __ = rostopic.get_topic_class(self.name + '/feedback')
rospy.Subscriber(self.name + '/activate', self.activate_class, self.activate_cb)
rospy.Subscriber(self.name + '/feedback', self.feedback_class, self.feedback_cb)
rospy.Subscriber(self.name + '/preempt', Empty, self.preempt_cb)
except roslib.packages.InvalidROSPkgException, ex:
print "Cannot watch %s because you do not have the messages built" % name
def get_topic_data(topic_name):
"""
GET /api/<version>/topic_data/<topic_name>
Get a single data point from a topic over HTTP.
"""
topic_name = "/" + topic_name
try:
msg_class, real_topic, _ = rostopic.get_topic_class(topic_name)
except rostopic.ROSTopicIOException as e:
raise e
if not real_topic:
return error("Topic does not exist", 404)
msg = rospy.wait_for_message(real_topic, msg_class)
data = getattr(msg, "data", None)
if data is None:
json = '{"status": ['
for x in msg.status:
if x == msg.status[-1]:
json += '{"name": \"%s\", "level": %d, "message": \"%s\", "hardware_id": \"%s\", "values": %s}]}' % \
(x.name if x.name else "null", x.level, \
x.message if x.message else "null", x.hardware_id if x.hardware_id else "null", \
x.values)
else:
json += '{"name": \"%s\", "level": %d, "message": \"%s\", "hardware_id": \"%s\", "values": %s},' % \
(x.name if x.name else "null", x.level, \
x.message if x.message else "null", x.hardware_id if x.hardware_id else "null", \
x.values)
return Response(json, mimetype = 'application/json')
else:
return jsonify({"result": data})
def init_publish(self, process):
try:
topic_name = process["publish"]["topic_name"]
topic_name = self.get_name(topic_name)
topic_latched = False
if "topic_latched" in process["publish"]:
topic_latched = process["publish"]["topic_latched"]
msg_class, real_topic, _ = rostopic.get_topic_class(topic_name)
pub = rospy.Publisher(real_topic,
msg_class,
latch=topic_latched,
queue_size=10)
msg = msg_class()
for arg in process["publish"]["topic_args"]:
exec(arg)
d = {}
d["name"] = "publish"
d["verbose"] = self.is_verbose(process["publish"])
d["def_msg"] = ("Publishing to topic '{}'".format(topic_name),
"info", msg)
d["func"] = "self.publish(**kwargs)"
d["kwargs"] = {}
d["kwargs"]["pub"] = pub
d["kwargs"]["msg"] = msg
self.processes.append(d)
except Exception as e:
self.event_cb(self.init_err_str.format("publish", str(e)), "warn")
self.processes.append("not_initialised")
def _configure_internal_topics(self, pub_names):
"""
Create subscriptions to all the topics that we externally publish.
"""
i = 0
for name in pub_names:
resolved_name = rospy.resolve_name(name)
rospy.loginfo("configuring internal subscriber [%s]",
resolved_name)
try:
real_msg_class, _, _ = rostopic.get_topic_class(resolved_name)
except rostopic.ROSTopicException:
raise rospy.ROSInitException(
"cannot proxy subscription [%s]: unknown topic type" %
resolved_name)
i += 1
topic_class = classobj('t_passthrough_%s' % i,
(rospy.msg.AnyMsg, ), {
'_type': real_msg_class._type,
'_md5sum': real_msg_class._md5sum,
})
self.subs_internal[resolved_name] = rospy.Subscriber(
name, topic_class)
rospy.loginfo("proxying %s", resolved_name)
def create_worker(name, topic, mongodb_host, mongodb_port, mongodb_name, collname, no_specific=False):
msg_class, _, _ = rostopic.get_topic_class(topic, blocking=True)
if no_specific:
loggerClass = TopicLogger
else:
loggerClass = logger_registry.get_logger(msg_class, TopicLogger)
return loggerClass(name, topic, collname, mongodb_host, mongodb_port, mongodb_name)
def run(self):
# wait for first message
while len(self.missing_topics) != 0 and not rospy.is_shutdown():
for topic in self.topics:
msg_class, real_topic, msg_eval = rostopic.get_topic_class(
topic, blocking=False) #pause hz until topic is published
if real_topic:
if real_topic in self.missing_topics:
self.missing_topics.remove(real_topic)
try:
rospy.logdebug(
"hz monitor is waiting for type of topics {}.".format(
self.missing_topics))
rospy.sleep(1.0)
except rospy.exceptions.ROSInterruptException:
pass
# call rostopic hz
self.rt_HZ_store = dict()
for topic in self.topics:
rt = rostopic.ROSTopicHz(self.window_size)
rospy.Subscriber(topic, rospy.AnyMsg, rt.callback_hz)
self.rt_HZ_store[topic] = rt
rospy.loginfo("subscribed to [{}]".format(topic))
def test_get_topic_class(self):
import rostopic
self.assertEquals((None, None, None), rostopic.get_topic_class('/fake'))
from rosgraph_msgs.msg import Log
c, n, f = rostopic.get_topic_class('/rosout')
self.assertEquals(Log, c)
self.assertEquals('/rosout', n)
self.assert_(f is None)
c, n, f = rostopic.get_topic_class('/rosout/name')
self.assertEquals(c, Log)
self.assertEquals('/rosout', n)
self.failIf(f is None)
self.assertEquals("bob", f(Log(name="bob")))
def __init__(self, topic, attr):
msg_class, real_topic, msg_eval = rostopic.get_topic_class(topic, blocking=True)
rospy.Subscriber(real_topic, msg_class, self._callback)
self._attr = attr
self._triggers = {}
self._actions = []
self._last_value = None
def __init__(self, topic_name, timeout):
self.topic_name = topic_name
self.deadline = rospy.Time.now() + rospy.Duration(timeout)
msg_class, _, _ = rostopic.get_topic_class(
rospy.resolve_name(topic_name), blocking=True)
self.msg = None
self.sub = rospy.Subscriber(topic_name, msg_class, self._callback)
def __subscribe(self, backoff=1.0):
"""
Try to subscribe to the set topic
:param backoff: How long to wait until another try
"""
data_class, _, _ = rostopic.get_topic_class(self.topic)
if data_class is not None:
# topic is known
self.__subscriber = rospy.Subscriber(self.topic,
data_class,
self.__message_callback,
queue_size=1,
tcp_nodelay=True)
rospy.loginfo('Subscribed to topic {}'.format(self.topic))
else:
# topic is not yet known
rospy.loginfo(
'Topic {} is not yet known. Retrying in {} seconds'.format(
self.topic, int(backoff)))
if backoff > 30:
backoff = 30
rospy.Timer(rospy.Duration(int(backoff)),
lambda event: self.__subscribe(backoff * 1.2),
oneshot=True)
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self.__last_request = {}
self.people = set([])
self.predicate = rospy.get_param("~distance_predicate",
"robot_distance")
self.__update_srv_name = rospy.get_param(
"~update_srv_name", "/kcl_rosplan/update_knowledge_base_array")
self.__get_details_srv_name = rospy.get_param(
"~get_details_srv_name",
"/kcl_rosplan/get_domain_predicate_details")
self.__get_instances_srv_name = rospy.get_param(
"~get_instances_srv_name", "/kcl_rosplan/get_current_instances")
with open(rospy.get_param("~config_file"), 'r') as f:
config = yaml.load(f)
rospy.loginfo("Inserting static instances.")
self.update_knowledgebase(
instances=self._create_instances(config["static_instances"]))
self.subscribers = []
for inputs in config["inputs"]:
rospy.loginfo("Subsribing to '%s'." % inputs["topic"])
self.__last_request[inputs["topic"]] = {
0: KnowledgeUpdateServiceArrayRequest(),
1: KnowledgeUpdateServiceArrayRequest()
}
self.subscribers.append(
rospy.Subscriber(name=inputs["topic"],
data_class=rostopic.get_topic_class(
inputs["topic"], blocking=True)[0],
callback=self.callback,
callback_args=inputs))
rospy.loginfo("... done")
def subscribe(self, time_info=None):
"""
Subscribe to the topics defined in the yaml configuration file
Function checks subscription status True/False of each topic
if True: topic has already been sucessfully subscribed to
if False: topic still needs to be subscribed to and
subscriber will be run.
Each element in self.subscriber list is a list [topic, Bool]
where the Bool tells the current status of the subscriber (sucess/failure).
Return number of topics that failed subscription
"""
if self.successful_subscription_count == len(self.subscriber_list):
if self.resubscriber is not None:
self.resubscriber.shutdown()
rospy.loginfo(
'All topics subscribed too! Shutting down resubscriber')
for topic, (time, subscribed) in self.subscriber_list.items():
if not subscribed:
msg_class = rostopic.get_topic_class(topic)
if msg_class[1] is not None:
self.successful_subscription_count += 1
rospy.Subscriber(topic,
msg_class[0],
lambda msg, _topic=topic: self.
bagger_callback(msg, _topic))
self.subscriber_list[topic] = (time, True)
def __init__(self, topic_name, timeout):
self.topic_name = topic_name
self.deadline = rospy.Time.now() + rospy.Duration(timeout)
msg_class, _, _ = rostopic.get_topic_class(
rospy.resolve_name(topic_name), blocking=True)
self.msg = None
self.sub = rospy.Subscriber(topic_name, msg_class, self._callback)
def init(self):
global use_setproctitle
if use_setproctitle:
setproctitle("mongodb_log %s" % self.topic)
self.mongoconn = MongoClient(self.mongodb_host, self.mongodb_port)
self.mongodb = self.mongoconn[self.mongodb_name]
self.mongodb.set_profiling_level = SLOW_ONLY
self.collection = self.mongodb[self.collname]
self.collection.count()
self.queue.cancel_join_thread()
# clear signal handlers in this child process, rospy will handle signals for us
signal.signal(signal.SIGTERM, signal.SIG_DFL)
signal.signal(signal.SIGINT, signal.SIG_DFL)
worker_node_name = WORKER_NODE_NAME % (self.nodename_prefix, self.id, self.collname)
# print "Calling init_node with %s from process %s" % (worker_node_name, mp.current_process())
rospy.init_node(worker_node_name, anonymous=False)
self.subscriber = None
while not self.subscriber and not self.is_quit():
try:
msg_class, real_topic, msg_eval = rostopic.get_topic_class(self.topic, blocking=True)
self.subscriber = rospy.Subscriber(real_topic, msg_class, self.enqueue, self.topic)
except rostopic.ROSTopicIOException:
print("FAILED to subscribe, will keep trying %s" % self.name)
time.sleep(randint(1,10))
except rospy.ROSInitException:
print("FAILED to initialize, will keep trying %s" % self.name)
time.sleep(randint(1,10))
self.subscriber = None
def init(self):
global use_setproctitle
if use_setproctitle:
setproctitle("mongodb_log %s" % self.topic)
self.mongoconn = Connection(self.mongodb_host, self.mongodb_port)
self.mongodb = self.mongoconn[self.mongodb_name]
self.mongodb.set_profiling_level = SLOW_ONLY
self.collection = self.mongodb[self.collname]
self.collection.count()
self.queue.cancel_join_thread()
rospy.init_node(WORKER_NODE_NAME % (self.nodename_prefix, self.id, self.collname),
anonymous=False)
self.subscriber = None
while not self.subscriber:
try:
msg_class, real_topic, msg_eval = rostopic.get_topic_class(self.topic, blocking=True)
self.subscriber = rospy.Subscriber(real_topic, msg_class, self.enqueue, self.topic)
except rostopic.ROSTopicIOException:
print("FAILED to subscribe, will keep trying %s" % self.name)
time.sleep(randint(1,10))
except rospy.ROSInitException:
print("FAILED to initialize, will keep trying %s" % self.name)
time.sleep(randint(1,10))
self.subscriber = None
def subscribe(self):
"""
Subscribe to the topics defined in the yaml configuration file
Function checks subscription status True/False of each topic
if True: topic has already been sucessfully subscribed to
if False: topic still needs to be subscribed to and
subscriber will be run.
Each element in self.subscriber list is a list [topic, Bool]
where the Bool tells the current status of the subscriber (sucess/failure).
Return number of topics that failed subscription
"""
for topic in self.subscriber_list:
if not topic[2]:
msg_class = rostopic.get_topic_class(topic[0])
if msg_class[1] is not None:
self.successful_subscription_count = self.successful_subscription_count + 1
rospy.Subscriber(topic[0],
msg_class[0],
lambda msg, _topic=topic[0]: self.
bagger_callback(msg, _topic))
topic[2] = True # successful subscription
def __init__(self):
# Get topic class
topic_class = None
rospy.loginfo("Waiting for topic to become available...")
while not rospy.is_shutdown() and topic_class == None:
topic_class, topic, _ = rostopic.get_topic_class(
rospy.resolve_name('in'))
rospy.sleep(0.5)
if hasattr(topic_class, 'header'):
cb = self.header_cb
elif topic_class == tf2_msgs.msg.TFMessage:
cb = self.tf_cb
self.frames = set(rospy.get_param('~frames'))
self.frame_prefix = rospy.get_param('~frame_prefix')
else:
print("Message type {} doesn't have a header field.".format(
topic_class))
sys.exit(1)
self.start_time = None
self.actual_start_time = None
self.time_offset = None
self.sub = rospy.Subscriber('in', topic_class, cb)
self.pub = rospy.Publisher('out', topic_class, queue_size=1)
def _init(self):
"""
This method initializes this process.
It initializes the connection to the MongoDB and subscribes to the topic.
"""
self.mongoconn = Connection(self.mongodb_host, self.mongodb_port)
self.mongodb = self.mongoconn[self.mongodb_name]
self.mongodb.set_profiling_level = SLOW_ONLY
self.collection = self.mongodb[self.collname]
self.collection.count()
self.queue.cancel_join_thread()
self.subscriber = None
while not self.subscriber:
try:
msg_class, real_topic, msg_eval = rostopic.get_topic_class(self.topic, blocking=True)
self.subscriber = rospy.Subscriber(real_topic, msg_class, self._enqueue, self.topic)
except rostopic.ROSTopicIOException:
rospy.logwarn("FAILED to subscribe, will keep trying %s" % self.name)
time.sleep(randint(1, 10))
except rospy.ROSInitException:
rospy.logwarn("FAILED to initialize, will keep trying %s" % self.name)
time.sleep(randint(1, 10))
self.subscriber = None
def stream_topic(topic_name):
"""
GET /api/<version>/topic_stream/<topic_name>
Stream a topic over HTTP by keeping the http connection alive.
"""
topic_name = "/" + topic_name
try:
msg_class, real_topic, _ = rostopic.get_topic_class(topic_name)
except rostopic.ROSTopicIOException as e:
raise e
if not real_topic:
return error("Topic does not exist", 404)
queue = Queue(5)
def callback(data, queue=queue):
data = getattr(data, "data", None)
if data is None:
data = {k: getattr(res, k) for k in res.__slots__}
queue.put(data)
sub = rospy.Subscriber(real_topic, msg_class, callback)
def gen(queue=queue):
while True:
x = queue.get()
yield str(x) + "\n"
return Response(gen())
def __init__(self):
# Get topic class
topic_class = None
rospy.loginfo("Waiting for topic to become available...")
while not rospy.is_shutdown() and topic_class == None:
topic_class, topic, _ = rostopic.get_topic_class(rospy.resolve_name('in'))
rospy.sleep(0.5)
if hasattr(topic_class, 'header'):
cb = self.header_cb
elif topic_class == tf2_msgs.msg.TFMessage:
cb = self.tf_cb
self.frames = set(rospy.get_param('~frames'))
self.frame_prefix = rospy.get_param('~frame_prefix')
else:
print("Message type {} doesn't have a header field.".format(topic_class))
sys.exit(1)
self.start_time = None
self.actual_start_time = None
self.time_offset = None
self.sub = rospy.Subscriber('in', topic_class, cb)
self.pub = rospy.Publisher('out', topic_class, queue_size=1)
def dragEnterEvent(self, event):
if event.mimeData().hasText():
topic_name = str(event.mimeData().text())
if len(topic_name) == 0:
qWarning('PoseViewWidget.dragEnterEvent(): event.mimeData() text is empty')
return
else:
if not hasattr(event.source(), 'selectedItems') or len(event.source().selectedItems()) == 0:
qWarning('PoseViewWidget.dragEnterEvent(): event.source() has no attribute selectedItems or length of selectedItems is 0')
return
item = event.source().selectedItems()[0]
topic_name = item.data(0, Qt.UserRole)
if topic_name is None:
qWarning('PoseViewWidget.dragEnterEvent(): selectedItem has no UserRole data with a topic name')
return
# check for valid topic
msg_class, self._topic_name, _ = get_topic_class(topic_name)
if msg_class is None:
qWarning('PoseViewWidget.dragEnterEvent(): No message class was found for topic "%s".' % topic_name)
return
# check for valid message class
quaternion_slot_path, point_slot_path = self._get_slot_paths(msg_class)
if quaternion_slot_path is None and point_slot_path is None:
qWarning('PoseViewWidget.dragEnterEvent(): No Pose, Quaternion or Point data was found outside of arrays in "%s" on topic "%s".'
% (msg_class._type, topic_name))
return
event.acceptProposedAction()
def init(self):
global use_setproctitle
if use_setproctitle:
setproctitle("mongodb_log %s" % self.topic)
self.mongoconn = Connection(self.mongodb_host, self.mongodb_port)
self.mongodb = self.mongoconn[self.mongodb_name]
self.mongodb.set_profiling_level = SLOW_ONLY
self.collection = self.mongodb[self.collname]
self.collection.count()
self.queue.cancel_join_thread()
rospy.init_node(WORKER_NODE_NAME %
(self.nodename_prefix, self.id, self.collname),
anonymous=False)
self.subscriber = None
while not self.subscriber:
try:
msg_class, real_topic, msg_eval = rostopic.get_topic_class(
self.topic, blocking=True)
self.subscriber = rospy.Subscriber(real_topic, msg_class,
self.enqueue, self.topic)
except rostopic.ROSTopicIOException:
print("FAILED to subscribe, will keep trying %s" % self.name)
time.sleep(randint(1, 10))
except rospy.ROSInitException:
print("FAILED to initialize, will keep trying %s" % self.name)
time.sleep(randint(1, 10))
self.subscriber = None
def topic_wait_type(self, topic_name, retries=5, retrysleep=1):
resolved_topic_name = rospy.resolve_name(topic_name)
topic_type, _, _ = rostopic.get_topic_type(resolved_topic_name)
topic_class, _, _ = rostopic.get_topic_class(resolved_topic_name)
retry = 0
while not topic_type and not topic_class and retry < 5:
print('Topic {topic} not found. Retrying...'.format(
topic=resolved_topic_name))
rospy.rostime.wallsleep(retrysleep)
retry += 1
topic_type, _, _ = rostopic.get_topic_type(resolved_topic_name)
topic_class, _, _ = rostopic.get_topic_class(resolved_topic_name)
if retry >= retries:
self.fail(
"Topic {0} not found ! Failing.".format(resolved_topic_name))
return topic_type, topic_class
def subscribe(self, time_info=None):
"""
Subscribe to the topics defined in the yaml configuration file
Function checks subscription status True/False of each topic
if True: topic has already been sucessfully subscribed to
if False: topic still needs to be subscribed to and
subscriber will be run.
Each element in self.subscriber list is a list [topic, Bool]
where the Bool tells the current status of the subscriber (sucess/failure).
Return number of topics that failed subscription
"""
if self.successful_subscription_count == len(self.subscriber_list):
if self.resubscriber is not None:
self.resubscriber.shutdown()
rospy.loginfo(
'All topics subscribed too! Shutting down resubscriber')
for topic, (time, subscribed) in self.subscriber_list.items():
if not subscribed:
msg_class = rostopic.get_topic_class(topic)
if msg_class[1] is not None:
self.successful_subscription_count += 1
rospy.Subscriber(topic, msg_class[0],
lambda msg, _topic=topic: self.bagger_callback(msg, _topic))
self.subscriber_list[topic] = (time, True)
def subscribeToOrigin(self):
# This line blocks until initialize_origin is alive and has advertised the origin topic
origin_class = rostopic.get_topic_class(ORIGIN_TOPIC, blocking=True)[0]
rospy.loginfo("Origin is a " + origin_class._type + " message")
self.assertIsNotNone(origin_class, msg=ORIGIN_TOPIC+" was never advertised")
self.assertIn(origin_class, ORIGIN_TYPES)
return rospy.Subscriber(ORIGIN_TOPIC, origin_class, self.originCallback)
def register_topics(self, bot, chat_id):
default_topics = ' '.join(
['/speak/goal', '/notification', '/notification_image'])
topics = rospy.get_param('~subscribed_topics',
default_topics).split(' ')
for t in topics:
try:
msg_class, _, _ = get_topic_class(t)
if msg_class is not None:
if chat_id not in self.subscriptions:
self.subscriptions[chat_id] = {}
if t not in self.subscriptions[chat_id]:
s = rospy.Subscriber(
t,
msg_class,
lambda msg, topic=t, bot=bot, chat_id=chat_id: self
.topic_callback(msg, topic, bot, chat_id),
queue_size=1)
self.subscriptions[chat_id][t] = s
rospy.loginfo('register subscriber for topic'
' %s with type %s' % (t, str(msg_class)))
else:
rospy.logwarn("couldn't determine type of topic '%s'."
" So not subscribing to it." % t)
except:
rospy.logwarn("couldn't subscribe to topic %s" % t)
def dragEnterEvent(self, event):
if event.mimeData().hasText():
topic_name = str(event.mimeData().text())
if len(topic_name) == 0:
qWarning('PoseViewWidget.dragEnterEvent(): event.mimeData() text is empty')
return
else:
if not hasattr(event.source(), 'selectedItems') or len(event.source().selectedItems()) == 0:
qWarning('PoseViewWidget.dragEnterEvent(): event.source() has no attribute selectedItems or length of selectedItems is 0')
return
item = event.source().selectedItems()[0]
topic_name = item.data(0, Qt.UserRole)
if topic_name is None:
qWarning('PoseViewWidget.dragEnterEvent(): selectedItem has no UserRole data with a topic name')
return
# check for valid topic
msg_class, self._topic_name, _ = get_topic_class(topic_name)
if msg_class is None:
qWarning('PoseViewWidget.dragEnterEvent(): No message class was found for topic "%s".' % topic_name)
return
# check for valid message class
quaternion_slot_path, point_slot_path = self._get_slot_paths(msg_class)
if quaternion_slot_path is None and point_slot_path is None:
qWarning('PoseViewWidget.dragEnterEvent(): No Pose, Quaternion or Point data was found outside of arrays in "%s" on topic "%s".'
% (msg_class._type, topic_name))
return
event.acceptProposedAction()
def stream_topic(topic_name):
"""
GET /api/<version>/topic_stream/<topic_name>
Stream a topic over HTTP by keeping the http connection alive.
"""
topic_name = "/" + topic_name
try:
msg_class, real_topic, _ = rostopic.get_topic_class(topic_name)
except rostopic.ROSTopicIOException as e:
raise e
if not real_topic:
return error("Topic does not exist", 404)
queue = Queue(5)
def callback(data, queue=queue):
data = getattr(data, "data", None)
if data is None:
data = {k: getattr(res, k) for k in res.__slots__}
queue.put(data)
sub = rospy.Subscriber(real_topic, msg_class, callback)
def gen(queue=queue):
while True:
x = queue.get()
yield str(x) + "\n"
return Response(gen())
def sub_odometry_topic(name):
odom_topic_sub = None
for t in get_topics(name):
msg_class, _, _ = rostopic.get_topic_class(t)
if msg_class == Odometry:
odom_topic_sub = rospy.Subscriber(
t, Odometry, lambda msg: odometry_callback(msg, name))
return odom_topic_sub
def subscribeToOrigin(self):
# This line blocks until initialize_origin is alive and has advertised the origin topic
origin_class = rostopic.get_topic_class(ORIGIN_TOPIC, blocking=True)[0]
rospy.loginfo("Origin is a " + origin_class._type + " message")
self.assertIsNotNone(origin_class, msg=ORIGIN_TOPIC+" was never advertised")
self.assertIn(origin_class, ORIGIN_TYPES)
self.test_stamp = False # Enable this for auto origin
return rospy.Subscriber(ORIGIN_TOPIC, origin_class, self.originCallback)
def test_get_topic_class(self):
import rostopic
self.assertEquals((None, None, None),
rostopic.get_topic_class('/fake'))
from rosgraph_msgs.msg import Log
c, n, f = rostopic.get_topic_class('/rosout')
self.assertEquals(Log, c)
self.assertEquals('/rosout', n)
self.assert_(f is None)
c, n, f = rostopic.get_topic_class('/rosout/name')
self.assertEquals(c, Log)
self.assertEquals('/rosout', n)
self.failIf(f is None)
self.assertEquals("bob", f(Log(name="bob")))
def subscribe_topic(self, topic_name):
msg_class, self._topic_name, _ = get_topic_class(topic_name)
if (msg_class.__name__ == 'Quaternion'):
self._subscriber = rospy.Subscriber(
self._topic_name, msg_class, self.message_callback_quaternion)
else:
self._subscriber = rospy.Subscriber(self._topic_name, msg_class,
self.message_callback_pose)
def __start__(self):
while not self.kill:
try:
time.sleep(self.TIMEOUT)
topic = rostopic.get_topic_class('/modbus_tcp_cmd')
self.ros_alive = False if topic[0] == None else True
except:
self.ros_alive = False
def __init__(self, topic, attr):
msg_class, real_topic, msg_eval = rostopic.get_topic_class(
topic, blocking=True)
rospy.Subscriber(real_topic, msg_class, self._callback)
self._attr = attr
self._triggers = {}
self._actions = []
self._last_value = None
def __init__(self):
button_topic = rospy.get_param('~button_topic', '/drone/joy/buttons[4]')
button_topic_class, button_real_topic, self.button_eval_func = rostopic.get_topic_class(rospy.resolve_name(button_topic))
self.sub_button = rospy.Subscriber(button_real_topic, button_topic_class, self.button_cb)
self.last_button = None
self.button_pressed = False
self.button_released = False
self.land_action_ns = rospy.get_param('~land_action_ns', '/drone/land_action')
robot_odom_topic = rospy.get_param('~robot_odom_topic', '/drone/odom')
self.sub_odom = rospy.Subscriber(robot_odom_topic, Odometry, self.robot_odom_cb)
self.robot_current_pose = PoseStamped()
self.state_name_topic = rospy.get_param('~state_name_topic', '~state')
self.pub_state_name = rospy.Publisher(self.state_name_topic, String, queue_size = 10, latch = True)
robot_state_topic = rospy.get_param('~robot_state_topic', '/drone/flight_state')
self.last_known_flight_state = FlightState.Landed
self.sub_flight_state = rospy.Subscriber(robot_state_topic, FlightState, self.flight_state_cb)
self.flight_state_event = threading.Event()
landing_spot = rospy.get_param('landing_spot', {'x': float('nan'), 'y': float('nan'), 'z': float('nan'), 'tolerance': float('nan')})
self.landing_spot, self.landing_tolerance = self.get_landing_spot(**landing_spot)
if math.isnan(self.landing_spot.Norm()):
rospy.logwarn('Landing spot is not specified, allow landing anywhere')
self.sm = smach.StateMachine(outcomes = ['FINISH'])
with self.sm:
# smach.StateMachine.add('WAIT_USER',
# smach.CBState(self.check_button, cb_kwargs = {'context': self}),
# transitions = {'pressed': 'FOLLOW_POINTING',
# 'preempted': 'FINISH'})
smach.StateMachine.add('WAIT_USER',
smach.CBState(self.wait_for_flying, cb_kwargs = {'context': self}),
transitions = {'succeeded': 'FOLLOW_POINTING',
'aborted': 'FINISH',
'preempted': 'WAIT_USER'})
smach.StateMachine.add('FOLLOW_POINTING',
smach.CBState(self.wait_for_landing, cb_kwargs = {'context': self}),
transitions = {'succeeded': 'LAND',
'aborted': 'FOLLOW_POINTING',
'preempted': 'FINISH'})
smach.StateMachine.add('LAND',
smach_ros.SimpleActionState(self.land_action_ns, EmptyAction),
transitions = {'succeeded': 'WAIT_USER',
'preempted': 'FINISH',
'aborted': 'WAIT_USER'})
self.sm.register_start_cb(self.state_transition_cb, cb_args = [self.sm])
self.sm.register_transition_cb(self.state_transition_cb, cb_args = [self.sm])
self.sis = smach_ros.IntrospectionServer('smach_server', self.sm, '/SM_JOY')
def main():
parser = argparse.ArgumentParser()
parser.add_argument('topics',
nargs='+',
help='leaf topic names of topic graph')
parser.add_argument('-p',
'--search-parent',
action='store_true',
help='search parent topics to check')
args = parser.parse_args()
topics = args.topics
if args.search_parent:
# get all parent topics
parent_topics = []
for topic in topics:
parent_topics.extend(get_parent_topics(topic, impl=True))
parent_topics = list(set(parent_topics))
topics.extend(parent_topics)
topics = np.array(topics)
# prepare topic hz checkers
delay_checkers = []
for t in topics:
rt = ROSTopicDelay(window_size=-1)
delay_checkers.append(rt)
msg_class, real_topic, _ = rostopic.get_topic_class(t, blocking=True)
rospy.Subscriber(real_topic, msg_class, rt.callback_delay)
rospy.loginfo('subscribed {} topics'.format(len(topics)))
# main loop
while not rospy.is_shutdown():
# wait for first message
if not all(rt.delays for rt in delay_checkers):
continue
# collect topic hz stats
show_topics, stats = [], []
for i, rt in enumerate(delay_checkers):
delay_stat = rt.get_delay()
if delay_stat is None:
continue
delay, min_delta, max_delta, std_dev, window = delay_stat
show_topics.append(topics[i])
stats.append([delay, min_delta, max_delta, std_dev, window])
show_topics, stats = np.array(show_topics), np.array(stats)
if stats.size == 0:
rospy.logwarn('no messages')
else:
sort_indices = np.argsort(stats[:, 0])[::-1]
show_topics, stats = show_topics[sort_indices], stats[sort_indices]
stats = np.hstack((show_topics.reshape(-1, 1), stats)).tolist()
# print stats result
header = [
'topic', 'delay', 'min_delta', 'max_delta', 'std_dev', 'window'
]
table = Texttable(max_width=0)
table.set_deco(Texttable.HEADER)
table.add_rows([header] + stats)
print(table.draw())
# wait for next check
rospy.rostime.wallsleep(1.0)
def subscribe_to_topic(self, topic_name):
topic_type, real_topic_name, _ = get_topic_class(topic_name)
self.topic_sub = rospy.Subscriber(topic_name, topic_type,
self.topic_cb, queue_size=1)
self.subscribed_to_topic = True
self.current_topic = topic_name
rospy.loginfo("Subscribed to " + self.current_topic +
" and type: " + topic_type)
self.bt_pause.set_text("Pause topic")
def __init__(self, topic_name, timeout = 5, echo = False):
self.topic_name = topic_name
self.timeout = timeout
self.launched_time = rospy.Time.now()
self.first_time_callback = True
self.echo = echo
print " Checking %s" % (topic_name)
msg_class, _, _ = rostopic.get_topic_class(topic_name, blocking=True)
self.sub = rospy.Subscriber(topic_name, msg_class, self.callback)
def test_Recording(self):
# Wait for topics and services
for topic in self.topics:
rospy.wait_for_message(topic, rostopic.get_topic_class(topic, blocking=True)[0], timeout=None)
for service in self.services:
rospy.wait_for_service(service, timeout=None)
self.app.execute()
def create_subscriber_callback(self, event):
#print "self.topics=", self.topics
for topic in self.topics:
if topic not in self.subscriber:
try:
msg_class, _, _ = rostopic.get_topic_class(topic)
rospy.Subscriber(topic, msg_class, self.global_topic_callback, callback_args=topic)
self.subscriber.append(topic)
except Exception:
pass
def _subscribe_topic(self, topic_name):
msg_class, self._topic_name, _ = get_topic_class(topic_name)
quaternion_slot_path, point_slot_path = self._get_slot_paths(msg_class)
self._subscriber = rospy.Subscriber(
self._topic_name,
msg_class,
self.message_callback,
callback_args=(quaternion_slot_path, point_slot_path)
)
def __init__(self, name):
self.name = name
if name[0] != "/":
name = "/" + name
msg_class, real_topic, _ = rostopic.get_topic_class(name, blocking=True) # pause hz until topic is published
self.sub = rospy.Subscriber(real_topic, msg_class, self.valuecb)
self.var = None
self.count = 0
self.lastcount = 0
self.lasttime = -1.0
self.cfreq = -1.0
def add_pub(self, topic):
topic_type = idlman.topicinfo[topic]
if(topic_type == None):
rospy.loginfo('%s is not published yet', topic)
return None
topic_class, _, _ = rostopic.get_topic_class(topic)
if(topic_class == None):
rospy.loginfo('%s is not builded yet', topic_type)
return None
self.pubs[topic] = rospy.Publisher(topic, topic_class)
return True
def main():
parser = argparse.ArgumentParser()
parser.add_argument('topics', nargs='+',
help='leaf topic names of topic graph')
parser.add_argument('-p', '--search-parent', action='store_true',
help='search parent topics to check')
args = parser.parse_args()
topics = args.topics
if args.search_parent:
# get all parent topics
parent_topics = []
for topic in topics:
parent_topics.extend(get_parent_topics(topic, impl=True))
parent_topics = list(set(parent_topics))
topics.extend(parent_topics)
topics = np.array(topics)
# prepare topic hz checkers
delay_checkers = []
for t in topics:
rt = ROSTopicDelay(window_size=-1)
delay_checkers.append(rt)
msg_class, real_topic, _ = rostopic.get_topic_class(t, blocking=True)
rospy.Subscriber(real_topic, msg_class, rt.callback_delay)
rospy.loginfo('subscribed {} topics'.format(len(topics)))
# main loop
while not rospy.is_shutdown():
# wait for first message
if not all(rt.delays for rt in delay_checkers):
continue
# collect topic hz stats
show_topics, stats = [], []
for i, rt in enumerate(delay_checkers):
delay_stat = rt.get_delay()
if delay_stat is None:
continue
delay, min_delta, max_delta, std_dev, window = delay_stat
show_topics.append(topics[i])
stats.append([delay, min_delta, max_delta, std_dev, window])
show_topics, stats = np.array(show_topics), np.array(stats)
if stats.size == 0:
rospy.logwarn('no messages')
else:
sort_indices = np.argsort(stats[:, 0])[::-1]
show_topics, stats = show_topics[sort_indices], stats[sort_indices]
stats = np.hstack((show_topics.reshape(-1, 1), stats)).tolist()
# print stats result
header = ['topic', 'delay', 'min_delta',
'max_delta', 'std_dev', 'window']
table = Texttable(max_width=0)
table.set_deco(Texttable.HEADER)
table.add_rows([header] + stats)
print(table.draw())
# wait for next check
rospy.rostime.wallsleep(1.0)
def __init__(self):
self.bag_name = rosparam.get_param("/test_name")
self.number_of_tests = rosparam.get_param("/number_of_tests")
self.robot_config_file = self.load_data(rosparam.get_param("/robot_config"))
if not os.path.exists(rosparam.get_param(rospy.get_name() + "/bagfile_output")):
os.makedirs(rosparam.get_param(rospy.get_name() + "/bagfile_output"))
self.topic = "/atf/"
self.lock_write = Lock()
self.bag = rosbag.Bag(rosparam.get_param(rospy.get_name() + "/bagfile_output") + self.bag_name + ".bag", 'w')
self.test_config = self.load_data(rosparam.get_param(rospy.get_name() + "/test_config_file")
)[rosparam.get_param("/test_config")]
recorder_config = self.load_data(rospkg.RosPack().get_path("atf_recorder_plugins") +
"/config/recorder_plugins.yaml")
self.BfW = BagfileWriter(self.bag, self.lock_write)
# Init metric recorder
self.recorder_plugin_list = []
for (key, value) in recorder_config.iteritems():
self.recorder_plugin_list.append(getattr(atf_recorder_plugins, value)(self.topic,
self.test_config,
self.robot_config_file,
self.lock_write,
self.bag))
self.topic_pipeline = []
self.active_sections = []
self.requested_topics = []
self.testblock_list = self.create_testblock_list()
for topic in self.get_topics():
msg_type = rostopic.get_topic_class(topic, blocking=True)[0]
rospy.Subscriber(topic, msg_type, self.global_topic_callback, queue_size=5, callback_args=topic)
self.test_status_publisher = rospy.Publisher(self.topic + "test_status", TestStatus, queue_size=10)
rospy.Service(self.topic + "recorder_command", RecorderCommand, self.command_callback)
# Wait for subscriber
num_subscriber = self.test_status_publisher.get_num_connections()
while num_subscriber == 0:
num_subscriber = self.test_status_publisher.get_num_connections()
test_status = TestStatus()
test_status.test_name = self.bag_name
test_status.status_recording = 1
test_status.status_analysing = 0
test_status.total = self.number_of_tests
self.test_status_publisher.publish(test_status)
rospy.loginfo(rospy.get_name() + ": Node started!")
def __init__(self, topic_name, timeout=5, echo=False, data_class=None):
self.topic_name = topic_name
self.timeout = timeout
self.launched_time = rospy.Time.now()
self.first_time_callback = True
self.echo = echo
print " Checking %s" % (topic_name)
msg_class, _, _ = rostopic.get_topic_class(topic_name, blocking=True)
if (data_class is not None) and (msg_class is not data_class):
raise rospy.ROSException("Topic msg type is different.")
self.sub = rospy.Subscriber(topic_name, msg_class, self.callback)
def subscribe_topics(self):
for topic in self.topic_list:
rospy.loginfo("Subscribing to topic: '" + topic + "'")
topic_class = rostopic.get_topic_class(topic, blocking=False)[0]
if topic_class == None:
rospy.logerr("Topic is not yet instantiated. Skipping.")
self.topic_list.remove(topic)
continue
print "Data class: ", topic_class
topic_sub = rospy.Subscriber(topic, topic_class, self.generic_cb, callback_args=topic)
self.topic_sub_list.append(topic_sub)
def addTopic(self,topic, starttime, endtime):
"""Diese Methode registriert ein Topic für die Aufzeichnung in die Cassandra Datenbank. Außerdem werden Metadaten in eine separate Tabelle geschrieben"""
#TODO: Anfangs und Endzeit berücksichtigen, mit thrading.Timer in eigenen Thread auslagern und diesen zum Zeitpunkt "endtime" automatisiert beenden.
if not self.subscriberList.has_key(str(topic)) :
msg_class, real_topic, _ = rostopic.get_topic_class(topic, blocking=True)
self.metadata.insert(str(topic), {'tablename' : str(topic), 'msg_class' : str(msg_class)})
self.subscriberList[str(topic)] = rospy.Subscriber(real_topic, msg_class, self.__insertCassandra, topic)
print "Added topic: " + str(topic)
else :
print "Topic \"" + str(topic) +"\" already exists"
def receiveVisu(self, msg):
if self.started: return
with self.mutex:
if msg.state == "INIT":
self.agents.add(msg.agent)
if len(self.expectedAgents) == len(self.agents):
while rostopic.get_topic_class("/vnet/reload")[1] is None:
logger.warning("Cannot find vnet ?!?")
logger.warning(rostopic.get_topic_class("/vnet/reload"))
time.sleep(5)
self.pub_vnet = rospy.Publisher("/vnet/reload", Empty, queue_size=1)
threading.Timer(1, lambda : self.pub_vnet.publish()).start()
logger.info("Autostart has found everyone. Starting in 3 seconds.")
self.started = True
threading.Timer(3, lambda : self.pub.publish()).start()
threading.Timer(5, lambda : rospy.signal_shutdown("Done")).start()
def __init__(self, topic_name, timeout=5, echo=False,
data_class=None, echo_noarr=False):
self.msg = None
self.topic_name = topic_name
self.deadline = rospy.Time.now() + rospy.Duration(timeout)
self.echo = echo
self.echo_noarr = echo_noarr
print(' Checking %s for %d seconds' % (topic_name, timeout))
msg_class, _, _ = rostopic.get_topic_class(topic_name, blocking=True)
if (data_class is not None) and (msg_class is not data_class):
raise rospy.ROSException('Topic msg type is different.')
self.sub = rospy.Subscriber(topic_name, msg_class, self.callback)