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