def __init__(self, topic_prefix, config_file, robot_config_file, write_lock, bag_file): self.topic = topic_prefix + "obstacle_distance" self.test_config = config_file self.robot_config_file = robot_config_file resources_timer_frequency = 100.0 # Hz self.timer_interval = 1 / resources_timer_frequency self.testblock_pipeline = [] self.BfW = BagfileWriter(bag_file, write_lock) # Wait for obstacle_distance node rospy.loginfo(rospy.get_name() + ": Waiting for obstacle distance node...") rospy.wait_for_service( self.robot_config_file["obstacle_distance"]["services"][0]) rospy.loginfo(rospy.get_name() + ": Obstacle distance node found!") self.obstacle_distance_server = rospy.ServiceProxy( self.robot_config_file["obstacle_distance"]["services"][0], GetObstacleDistance) rospy.Timer(rospy.Duration.from_sec(self.timer_interval), self.collect_obstacle_distances)
class RecordObstacleDistance: def __init__(self, topic_prefix, config_file, robot_config_file, write_lock, bag_file): self.topic_prefix = topic_prefix self.test_config = config_file self.robot_config_file = robot_config_file resources_timer_frequency = 100.0 # Hz self.timer_interval = 1/resources_timer_frequency self.res_pipeline = {} self.BfW = BagfileWriter(bag_file, write_lock) rospy.loginfo("Waiting for obstacle_distance node...") rospy.wait_for_service(self.robot_config_file["obstacle_distance"]["services"]) self.obstacle_distance_server = rospy.ServiceProxy(self.robot_config_file["obstacle_distance"]["services"], GetObstacleDistance) rospy.Timer(rospy.Duration.from_sec(self.timer_interval), self.collect_obstacle_distances) def trigger_callback(self, msg): # Process Trigger pass def collect_obstacle_distances(self, event): pipeline = copy(self.res_pipeline) if not len(pipeline) == 0: self.BfW.write_to_bagfile(topic, msg, rospy.Time.from_sec(time.time()))
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_prefix, config_file, robot_config_file, write_lock, bag_file): self.topic = topic_prefix + "interface" self.test_config = config_file self.robot_config_file = robot_config_file resources_timer_frequency = 100.0 # Hz self.timer_interval = 1 / resources_timer_frequency self.default_timeout = 20 # s self.testblock_pipeline = [] self.BfW = BagfileWriter(bag_file, write_lock) rospy.Timer(rospy.Duration.from_sec(self.timer_interval), self.check_interfaces)
def __init__(self, topic_prefix, config_file, robot_config_file, write_lock, bag_file): self.topic_prefix = topic_prefix self.test_config = config_file self.resources_timer_frequency = 4.0 # Hz self.timer_interval = 1 / self.resources_timer_frequency self.testblock_list = self.create_testblock_list() self.pid_list = self.create_pid_list() self.requested_nodes = {} self.res_pipeline = {} self.BfW = BagfileWriter(bag_file, write_lock) rospy.Timer(rospy.Duration.from_sec(self.timer_interval), self.collect_resource_data)
def __init__(self, topic_prefix, config_file, robot_config_file, write_lock, bag_file): self.topic_prefix = topic_prefix self.test_config = config_file self.resources_timer_frequency = 4.0 # Hz self.timer_interval = 1/self.resources_timer_frequency self.testblock_list = self.create_testblock_list() self.pid_list = self.create_pid_list() self.requested_nodes = {} self.res_pipeline = {} self.BfW = BagfileWriter(bag_file, write_lock) rospy.Timer(rospy.Duration.from_sec(self.timer_interval), self.collect_resource_data)
def __init__(self, topic_prefix, config_file, robot_config_file, write_lock, bag_file): self.topic_prefix = topic_prefix self.test_config = config_file self.robot_config_file = robot_config_file resources_timer_frequency = 100.0 # Hz self.timer_interval = 1/resources_timer_frequency self.res_pipeline = {} self.BfW = BagfileWriter(bag_file, write_lock) rospy.loginfo("Waiting for obstacle_distance node...") rospy.wait_for_service(self.robot_config_file["obstacle_distance"]["services"]) self.obstacle_distance_server = rospy.ServiceProxy(self.robot_config_file["obstacle_distance"]["services"], GetObstacleDistance) rospy.Timer(rospy.Duration.from_sec(self.timer_interval), self.collect_obstacle_distances)
class CheckInterface: def __init__(self, topic_prefix, config_file, robot_config_file, write_lock, bag_file): self.topic = topic_prefix + "interface" self.test_config = config_file self.robot_config_file = robot_config_file resources_timer_frequency = 100.0 # Hz self.timer_interval = 1 / resources_timer_frequency self.default_timeout = 20 # s self.testblock_pipeline = [] self.BfW = BagfileWriter(bag_file, write_lock) rospy.Timer(rospy.Duration.from_sec(self.timer_interval), self.check_interfaces) def trigger_callback(self, msg): # Only save data if testblock is in test_config.yaml if msg.name in self.test_config: self.update_requested_testblocks(msg) if msg.trigger.trigger == Trigger.ERROR: self.testblock_pipeline = [] def update_requested_testblocks(self, msg): if msg.trigger.trigger == Trigger.ACTIVATE and "interface" in self.test_config[msg.name]: self.testblock_pipeline.append(msg.name) elif msg.trigger.trigger == Trigger.FINISH and msg.name in self.testblock_pipeline: self.testblock_pipeline.remove(msg.name) def check_interfaces(self, event): pipeline = copy(self.testblock_pipeline) if len(pipeline) == 0: return msg = TestblockInterface() for testblock in pipeline: testblock_interface = TestblockInterfaceStatus() interface = self.test_config[testblock]["interface"] if "timeout" in interface: timeout = interface["timeout"] else: timeout = self.default_timeout if "publisher" in interface: # Check for publisher for publisher in interface["publisher"]: testblock_interface.publisher.append(publisher) try: rospy.wait_for_message(publisher, rostopic.get_topic_class(publisher, blocking=False)[0], timeout=timeout) except (rospy.ROSException, rostopic.ROSTopicException): testblock_interface.publisher_results.append(False) else: testblock_interface.publisher_results.append(True) if "subscriber" in interface: # Check for subscriber for subscriber in interface["subscriber"]: testblock_interface.subscriber.append(subscriber["topic"]) pub = rospy.Publisher(subscriber["topic"], subscriber["class"]) rospy.sleep(timeout) if pub.get_num_connections() > 0: testblock_interface.subscriber_results.append(True) else: testblock_interface.subscriber_results.append(False) if "services" in interface: # Check for services for service in interface["services"]: testblock_interface.services.append(service) try: rospy.wait_for_service(service, timeout=timeout) except rospy.ROSException: testblock_interface.services_results.append(False) else: testblock_interface.services_results.append(True) if "actions" in interface: # Check for actions for action in interface["actions"]: testblock_interface.actions.append(action["topic"]) client = SimpleActionClient(action["topic"], action["class"]) if client.wait_for_server(rospy.Duration(timeout)): testblock_interface.actions_results.append(True) else: testblock_interface.actions_results.append(False) msg.testblock.append(testblock_interface) self.BfW.write_to_bagfile(self.topic, msg, rospy.Time.from_sec(time.time()))
class RecordResources: def __init__(self, topic_prefix, config_file, robot_config_file, write_lock, bag_file): self.topic_prefix = topic_prefix self.test_config = config_file self.resources_timer_frequency = 4.0 # Hz self.timer_interval = 1 / self.resources_timer_frequency self.testblock_list = self.create_testblock_list() self.pid_list = self.create_pid_list() self.requested_nodes = {} self.res_pipeline = {} self.BfW = BagfileWriter(bag_file, write_lock) rospy.Timer(rospy.Duration.from_sec(self.timer_interval), self.collect_resource_data) def update_requested_nodes(self, msg): if msg.trigger.trigger == Trigger.ACTIVATE: for node in self.testblock_list[msg.name]: if node not in self.requested_nodes: self.requested_nodes[node] = copy( self.testblock_list[msg.name][node]) self.res_pipeline[node] = copy( self.testblock_list[msg.name][node]) else: for res in self.testblock_list[msg.name][node]: self.requested_nodes[node].append(res) if res not in self.res_pipeline[node]: self.res_pipeline[node].append(res) elif msg.trigger.trigger == Trigger.FINISH: for node in self.testblock_list[msg.name]: for res in self.testblock_list[msg.name][node]: self.requested_nodes[node].remove(res) if res not in self.requested_nodes[node]: self.res_pipeline[node].remove(res) if len(self.requested_nodes[node]) == 0: del self.requested_nodes[node] del self.res_pipeline[node] def create_testblock_list(self): testblock_list = {} for testblock in self.test_config: try: self.test_config[testblock]["resources"] except KeyError: continue else: for resource in self.test_config[testblock]["resources"]: try: testblock_list[testblock] except KeyError: testblock_list.update({testblock: {}}) for node_name in self.test_config[testblock]["resources"][ resource]: if node_name not in testblock_list[testblock]: testblock_list[testblock].update( {node_name: [resource]}) elif resource not in testblock_list[testblock][ node_name]: testblock_list[testblock][node_name].append( resource) return testblock_list def collect_resource_data(self, event): pipeline = copy(self.res_pipeline) if not len(pipeline) == 0: msg = Resources() topic = self.topic_prefix + "resources" for node in pipeline: msg_data = NodeResources() pid = self.pid_list[node] if pid is None: continue try: msg_data.node_name = node if "cpu" in pipeline[node]: msg_data.cpu = psutil.Process(pid).cpu_percent( interval=self.timer_interval) if "mem" in pipeline[node]: msg_data.memory = psutil.Process(pid).memory_percent() if "io" in pipeline[node]: data = findall('\d+', str(psutil.Process(pid).io_counters())) msg_data.io.read_count = int(data[0]) msg_data.io.write_count = int(data[1]) msg_data.io.read_bytes = int(data[2]) msg_data.io.write_bytes = int(data[3]) if "network" in pipeline[node]: data = findall('\d+', str(psutil.net_io_counters())) msg_data.network.bytes_sent = int(data[0]) msg_data.network.bytes_recv = int(data[1]) msg_data.network.packets_sent = int(data[2]) msg_data.network.packets_recv = int(data[3]) msg_data.network.errin = int(data[4]) msg_data.network.errout = int(data[5]) msg_data.network.dropin = int(data[6]) msg_data.network.dropout = int(data[7]) msg.nodes.append(msg_data) except (psutil.NoSuchProcess, psutil.AccessDenied): pass self.BfW.write_to_bagfile(topic, msg, rospy.Time.from_sec(time.time())) def trigger_callback(self, msg): # Only save node resources if testblock requests them if msg.name in self.testblock_list: self.update_requested_nodes(msg) if msg.trigger.trigger == Trigger.ERROR: self.res_pipeline = [] def create_pid_list(self): node_list = {} for (testblock, nodes) in self.testblock_list.iteritems(): for node in nodes: if node not in node_list: node_list[node] = self.get_pid(node) return node_list @staticmethod def get_pid(name): try: pid = [p.pid for p in psutil.process_iter() if name in str(p.name)] return pid[0] except IndexError: pass try: node_id = '/NODEINFO' node_api = rosnode.get_api_uri(rospy.get_master(), name) code, msg, pid = xmlrpclib.ServerProxy(node_api[2]).getPid(node_id) except IOError: pass else: return pid try: return int(check_output(["pidof", "-s", name])) except CalledProcessError: pass rospy.logerr("Node '" + name + "' is not running!") return None
class RecordResources: def __init__(self, topic_prefix, config_file, robot_config_file, write_lock, bag_file): self.topic_prefix = topic_prefix self.test_config = config_file self.resources_timer_frequency = 4.0 # Hz self.timer_interval = 1/self.resources_timer_frequency self.testblock_list = self.create_testblock_list() self.pid_list = self.create_pid_list() self.requested_nodes = {} self.res_pipeline = {} self.BfW = BagfileWriter(bag_file, write_lock) rospy.Timer(rospy.Duration.from_sec(self.timer_interval), self.collect_resource_data) def update_requested_nodes(self, msg): if msg.trigger.trigger == Trigger.ACTIVATE: for node in self.testblock_list[msg.name]: if node not in self.requested_nodes: self.requested_nodes[node] = copy(self.testblock_list[msg.name][node]) self.res_pipeline[node] = copy(self.testblock_list[msg.name][node]) else: for res in self.testblock_list[msg.name][node]: self.requested_nodes[node].append(res) if res not in self.res_pipeline[node]: self.res_pipeline[node].append(res) elif msg.trigger.trigger == Trigger.FINISH: for node in self.testblock_list[msg.name]: for res in self.testblock_list[msg.name][node]: self.requested_nodes[node].remove(res) if res not in self.requested_nodes[node]: self.res_pipeline[node].remove(res) if len(self.requested_nodes[node]) == 0: del self.requested_nodes[node] del self.res_pipeline[node] def create_testblock_list(self): testblock_list = {} for testblock in self.test_config: try: self.test_config[testblock]["resources"] except KeyError: continue else: for resource in self.test_config[testblock]["resources"]: try: testblock_list[testblock] except KeyError: testblock_list.update({testblock: {}}) for node_name in self.test_config[testblock]["resources"][resource]: if node_name not in testblock_list[testblock]: testblock_list[testblock].update({node_name: [resource]}) elif resource not in testblock_list[testblock][node_name]: testblock_list[testblock][node_name].append(resource) return testblock_list def collect_resource_data(self, event): pipeline = copy(self.res_pipeline) if not len(pipeline) == 0: msg = Resources() topic = self.topic_prefix + "resources" for node in pipeline: msg_data = NodeResources() pid = self.pid_list[node] if pid is None: continue try: msg_data.node_name = node if "cpu" in pipeline[node]: msg_data.cpu = psutil.Process(pid).cpu_percent(interval=self.timer_interval) if "mem" in pipeline[node]: msg_data.memory = psutil.Process(pid).memory_percent() if "io" in pipeline[node]: data = findall('\d+', str(psutil.Process(pid).io_counters())) msg_data.io.read_count = int(data[0]) msg_data.io.write_count = int(data[1]) msg_data.io.read_bytes = int(data[2]) msg_data.io.write_bytes = int(data[3]) if "network" in pipeline[node]: data = findall('\d+', str(psutil.net_io_counters())) msg_data.network.bytes_sent = int(data[0]) msg_data.network.bytes_recv = int(data[1]) msg_data.network.packets_sent = int(data[2]) msg_data.network.packets_recv = int(data[3]) msg_data.network.errin = int(data[4]) msg_data.network.errout = int(data[5]) msg_data.network.dropin = int(data[6]) msg_data.network.dropout = int(data[7]) msg.nodes.append(msg_data) except (psutil.NoSuchProcess, psutil.AccessDenied): pass self.BfW.write_to_bagfile(topic, msg, rospy.Time.from_sec(time.time())) def trigger_callback(self, msg): # Only save node resources if testblock requests them if msg.name in self.testblock_list: self.update_requested_nodes(msg) if msg.trigger.trigger == Trigger.ERROR: self.res_pipeline = [] def create_pid_list(self): node_list = {} for (testblock, nodes) in self.testblock_list.iteritems(): for node in nodes: if node not in node_list: node_list[node] = self.get_pid(node) return node_list @staticmethod def get_pid(name): try: pid = [p.pid for p in psutil.process_iter() if name in str(p.name)] return pid[0] except IndexError: pass try: node_id = '/NODEINFO' node_api = rosnode.get_api_uri(rospy.get_master(), name) code, msg, pid = xmlrpclib.ServerProxy(node_api[2]).getPid(node_id) except IOError: pass else: return pid try: return int(check_output(["pidof", "-s", name])) except CalledProcessError: pass rospy.logerr("Node '" + name + "' is not running!") return None
class RecordObstacleDistance: def __init__(self, topic_prefix, config_file, robot_config_file, write_lock, bag_file): self.topic = topic_prefix + "obstacle_distance" self.test_config = config_file self.robot_config_file = robot_config_file resources_timer_frequency = 100.0 # Hz self.timer_interval = 1 / resources_timer_frequency self.testblock_pipeline = [] self.BfW = BagfileWriter(bag_file, write_lock) # Wait for obstacle_distance node rospy.loginfo(rospy.get_name() + ": Waiting for obstacle distance node...") rospy.wait_for_service( self.robot_config_file["obstacle_distance"]["services"][0]) rospy.loginfo(rospy.get_name() + ": Obstacle distance node found!") self.obstacle_distance_server = rospy.ServiceProxy( self.robot_config_file["obstacle_distance"]["services"][0], GetObstacleDistance) rospy.Timer(rospy.Duration.from_sec(self.timer_interval), self.collect_obstacle_distances) def trigger_callback(self, msg): # Only save data if testblock is in test_config.yaml if msg.name in self.test_config: self.update_requested_testblocks(msg) if msg.trigger.trigger == Trigger.ERROR: self.testblock_pipeline = [] def update_requested_testblocks(self, msg): if msg.trigger.trigger == Trigger.ACTIVATE and "obstacle_distance" in self.test_config[ msg.name]: self.testblock_pipeline.append(msg.name) elif msg.trigger.trigger == Trigger.FINISH and msg.name in self.testblock_pipeline: self.testblock_pipeline.remove(msg.name) def collect_obstacle_distances(self, event): pipeline = copy(self.testblock_pipeline) if len(pipeline) == 0: return links = {} msg = ObstacleDistance() for testblock in pipeline: msg_link = ObstacleDistanceLink() for item in self.test_config[testblock]["obstacle_distance"]: if len(item) == 1: co_object = [] else: co_object = item[1] if isinstance(item[0], list): link = item[0] else: link = [item[0]] request = GetObstacleDistanceRequest(link, co_object) response = self.obstacle_distance_server(request) for idx, name in enumerate(response.link_to_object): link_name = name.split("_to_")[0] object_name = name.split("_to_")[1] try: links[link_name] except KeyError: links[link_name] = {} links[link_name][object_name] = response.distances[idx] for name in links: msg_link.name = name for co_object_name in links[name]: msg_link.objects.append(co_object_name) msg_link.distances.append(links[name][co_object_name]) msg.links.append(msg_link) self.BfW.write_to_bagfile(self.topic, msg, rospy.Time.from_sec(time.time()))
class CheckInterface: def __init__(self, topic_prefix, config_file, robot_config_file, write_lock, bag_file): self.topic = topic_prefix + "interface" self.test_config = config_file self.robot_config_file = robot_config_file resources_timer_frequency = 100.0 # Hz self.timer_interval = 1 / resources_timer_frequency self.default_timeout = 20 # s self.testblock_pipeline = [] self.BfW = BagfileWriter(bag_file, write_lock) rospy.Timer(rospy.Duration.from_sec(self.timer_interval), self.check_interfaces) def trigger_callback(self, msg): # Only save data if testblock is in test_config.yaml if msg.name in self.test_config: self.update_requested_testblocks(msg) if msg.trigger.trigger == Trigger.ERROR: self.testblock_pipeline = [] def update_requested_testblocks(self, msg): if msg.trigger.trigger == Trigger.ACTIVATE and "interface" in self.test_config[ msg.name]: self.testblock_pipeline.append(msg.name) elif msg.trigger.trigger == Trigger.FINISH and msg.name in self.testblock_pipeline: self.testblock_pipeline.remove(msg.name) def check_interfaces(self, event): pipeline = copy(self.testblock_pipeline) if len(pipeline) == 0: return msg = TestblockInterface() for testblock in pipeline: testblock_interface = TestblockInterfaceStatus() interface = self.test_config[testblock]["interface"] if "timeout" in interface: timeout = interface["timeout"] else: timeout = self.default_timeout if "publisher" in interface: # Check for publisher for publisher in interface["publisher"]: testblock_interface.publisher.append(publisher) try: rospy.wait_for_message( publisher, rostopic.get_topic_class(publisher, blocking=False)[0], timeout=timeout) except (rospy.ROSException, rostopic.ROSTopicException): testblock_interface.publisher_results.append(False) else: testblock_interface.publisher_results.append(True) if "subscriber" in interface: # Check for subscriber for subscriber in interface["subscriber"]: testblock_interface.subscriber.append(subscriber["topic"]) pub = rospy.Publisher(subscriber["topic"], subscriber["class"]) rospy.sleep(timeout) if pub.get_num_connections() > 0: testblock_interface.subscriber_results.append(True) else: testblock_interface.subscriber_results.append(False) if "services" in interface: # Check for services for service in interface["services"]: testblock_interface.services.append(service) try: rospy.wait_for_service(service, timeout=timeout) except rospy.ROSException: testblock_interface.services_results.append(False) else: testblock_interface.services_results.append(True) if "actions" in interface: # Check for actions for action in interface["actions"]: testblock_interface.actions.append(action["topic"]) client = SimpleActionClient(action["topic"], action["class"]) if client.wait_for_server(rospy.Duration(timeout)): testblock_interface.actions_results.append(True) else: testblock_interface.actions_results.append(False) msg.testblock.append(testblock_interface) self.BfW.write_to_bagfile(self.topic, msg, rospy.Time.from_sec(time.time()))
class ATFRecorder: 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 shutdown(self): self.lock_write.acquire() self.bag.close() self.lock_write.release() test_status = TestStatus() test_status.test_name = self.bag_name test_status.status_recording = 3 test_status.status_analysing = 0 test_status.total = self.number_of_tests self.test_status_publisher.publish(test_status) def create_testblock_list(self): testblock_list = {} for testblock in self.test_config: for metric in self.test_config[testblock]: if metric in self.robot_config_file and "topics" in self.robot_config_file[metric]: try: testblock_list[testblock] except KeyError: testblock_list[testblock] = self.robot_config_file[metric]["topics"] else: for topic in self.robot_config_file[metric]["topics"]: testblock_list[testblock].append(topic) else: try: if "topics" in self.test_config[testblock][metric]: try: testblock_list[testblock] except KeyError: testblock_list[testblock] = self.test_config[testblock][metric]["topics"] else: for topic in self.test_config[testblock][metric]["topics"]: testblock_list[testblock].append(topic) except TypeError: pass return testblock_list def update_requested_topics(self, msg): if msg.trigger.trigger == Trigger.ACTIVATE: for topic in self.testblock_list[msg.name]: self.requested_topics.append(topic) if topic not in self.topic_pipeline: self.topic_pipeline.append(topic) elif msg.trigger.trigger == Trigger.FINISH: for topic in self.testblock_list[msg.name]: self.requested_topics.remove(topic) if topic not in self.requested_topics: self.topic_pipeline.remove(topic) def command_callback(self, msg): if (msg.trigger.trigger == Trigger.ACTIVATE and msg.name in self.active_sections) or \ (msg.trigger.trigger == Trigger.FINISH and msg.name not in self.active_sections) or \ msg.name not in self.test_config: return RecorderCommandResponse(False) # Only process message if testblock requests topics if msg.name in self.testblock_list: self.update_requested_topics(msg) # Send message to all recorder plugins for recorder_plugin in self.recorder_plugin_list: recorder_plugin.trigger_callback(msg) if msg.trigger.trigger == Trigger.ACTIVATE: self.active_sections.append(msg.name) elif msg.trigger.trigger == Trigger.FINISH: self.active_sections.remove(msg.name) elif msg.trigger.trigger == Trigger.ERROR: self.topic_pipeline = [] self.BfW.write_to_bagfile(self.topic + msg.name + "/Trigger", Trigger(msg.trigger.trigger), rospy.Time.from_sec(time.time())) return RecorderCommandResponse(True) @staticmethod def load_data(filename): with open(filename, 'r') as stream: doc = yaml.load(stream) return doc def global_topic_callback(self, msg, name): if name in self.topic_pipeline: now = rospy.Time.from_sec(time.time()) try: for item in msg.transforms: item.header.stamp = now except AttributeError: pass self.BfW.write_to_bagfile(name, msg, now) def get_topics(self): topics = [] for item in self.test_config: for metric in self.test_config[item]: if metric in self.robot_config_file and "topics" in self.robot_config_file[metric]: # Get topics from robot_config.yaml for topic in self.robot_config_file[metric]["topics"]: if topic not in topics: topics.append(topic) else: try: if "topics" in self.test_config[item][metric]: # Get topics from test_config.yaml for topic in self.test_config[item][metric]["topics"]: if topic not in topics: topics.append(topic) except TypeError: pass return topics
def __init__(self, topic_prefix, config_file, robot_config_file, write_lock, bag_file): self.topic_prefix = topic_prefix self.test_config = config_file self.BfW = BagfileWriter(bag_file, write_lock)
class RecordObstacleDistance: def __init__(self, topic_prefix, config_file, robot_config_file, write_lock, bag_file): self.topic = topic_prefix + "obstacle_distance" self.test_config = config_file self.robot_config_file = robot_config_file resources_timer_frequency = 100.0 # Hz self.timer_interval = 1 / resources_timer_frequency self.testblock_pipeline = [] self.BfW = BagfileWriter(bag_file, write_lock) # Wait for obstacle_distance node rospy.loginfo(rospy.get_name() + ": Waiting for obstacle distance node...") rospy.wait_for_service(self.robot_config_file["obstacle_distance"]["services"][0]) rospy.loginfo(rospy.get_name() + ": Obstacle distance node found!") self.obstacle_distance_server = rospy.ServiceProxy( self.robot_config_file["obstacle_distance"]["services"][0], GetObstacleDistance ) rospy.Timer(rospy.Duration.from_sec(self.timer_interval), self.collect_obstacle_distances) def trigger_callback(self, msg): # Only save data if testblock is in test_config.yaml if msg.name in self.test_config: self.update_requested_testblocks(msg) if msg.trigger.trigger == Trigger.ERROR: self.testblock_pipeline = [] def update_requested_testblocks(self, msg): if msg.trigger.trigger == Trigger.ACTIVATE and "obstacle_distance" in self.test_config[msg.name]: self.testblock_pipeline.append(msg.name) elif msg.trigger.trigger == Trigger.FINISH and msg.name in self.testblock_pipeline: self.testblock_pipeline.remove(msg.name) def collect_obstacle_distances(self, event): pipeline = copy(self.testblock_pipeline) if len(pipeline) == 0: return links = {} msg = ObstacleDistance() for testblock in pipeline: msg_link = ObstacleDistanceLink() for item in self.test_config[testblock]["obstacle_distance"]: if len(item) == 1: co_object = [] else: co_object = item[1] if isinstance(item[0], list): link = item[0] else: link = [item[0]] request = GetObstacleDistanceRequest(link, co_object) response = self.obstacle_distance_server(request) for idx, name in enumerate(response.link_to_object): link_name = name.split("_to_")[0] object_name = name.split("_to_")[1] try: links[link_name] except KeyError: links[link_name] = {} links[link_name][object_name] = response.distances[idx] for name in links: msg_link.name = name for co_object_name in links[name]: msg_link.objects.append(co_object_name) msg_link.distances.append(links[name][co_object_name]) msg.links.append(msg_link) self.BfW.write_to_bagfile(self.topic, msg, rospy.Time.from_sec(time.time()))