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