Feature/hannes metrics

.gitignore

@@ -39,10 +39,13 @@ qtcreator-*
 /planning/src
 
 *~
+*.orig
 
 # Emacs
 .#*
 
 # Catkin custom files
 CATKIN_IGNORE
 test_generated
+
+.idea/*

atf_core/src/atf_core/bagfile_helper.py

@@ -1,6 +1,6 @@
 #!/usr/bin/env python
 class BagfileWriter:
-    def __init__(self, bagfile, write_lock):
+    def __init__(self,test_config, bagfile, write_lock):
         """
         Class for writing data to a rosbag file.
         :param bagfile: The bagfile to which the data should be written.

atf_core/src/atf_core/recorder.py

@@ -34,14 +34,14 @@ def __init__(self, config, testblock_list):
         # create bag file writer handle
         self.lock_write = Lock()
         self.bag = rosbag.Bag(config["bagfile_output"] + self.config["test_name"] + ".bag", 'w')
-        self.bag_file_writer = atf_core.BagfileWriter(self.bag, self.lock_write)
+        self.bag_file_writer = atf_core.BagfileWriter(self.config, self.bag, self.lock_write)
 
         # Init metric recorder
         self.recorder_plugin_list = []
         if len(recorder_config) > 0:
             for value in recorder_config.values():
                 #print "value=", value
-                self.recorder_plugin_list.append(getattr(atf_recorder_plugins, value)(self.lock_write,
+                self.recorder_plugin_list.append(getattr(atf_recorder_plugins, value)(self.config, self.lock_write,
                                                                                       self.bag_file_writer))
 
         self.topic_pipeline = []
@@ -128,8 +128,9 @@ def create_subscriber_callback(self, event):
             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)
+                    rospy.Subscriber(topic, msg_class, self.global_topic_callback, callback_args=topic, queue_size=10)
                     self.subscriber.append(topic)
+                    print "new subscriber for: ", topic, "message class:", msg_class, " subscribers: ", self.subscriber
                 except Exception:
                     pass
 

atf_metrics/config/metrics.yaml

@@ -2,13 +2,23 @@ time:
   handler: CalculateTimeParamHandler
 path_length:
   handler: CalculatePathLengthParamHandler
-resources:
-  handler: CalculateResourcesParamHandler
+distance:
+  handler: CalculateDistanceParamHandler
+resource_cpu:
+  handler: CalculateResourcesCpuParamHandler
+resource_mem:
+  handler: CalculateResourcesMemParamHandler
+resource_io:
+  handler: CalculateResourcesIOParamHandler
+resource_network:
+  handler: CalculateResourcesNetworkParamHandler
 obstacle_distance:
   handler: CalculateDistanceToObstaclesParamHandler
 publish_rate:
   handler: CalculatePublishRateParamHandler
 interface:
-    handler: CalculateInterfaceParamHandler
+  handler: CalculateInterfaceParamHandler
+localization:
+  handler: CheckLocalizationParamHandler
 # example:
 #   handler: ExampleParamHandler

atf_metrics/src/atf_metrics/__init__.py

@@ -1,6 +1,9 @@
 from atf_metrics.calculate_path_length import CalculatePathLength, CalculatePathLengthParamHandler
 from atf_metrics.calculate_time import CalculateTime, CalculateTimeParamHandler
-from atf_metrics.calculate_resources import CalculateResources, CalculateResourcesParamHandler
+from atf_metrics.calculate_resources_cpu import CalculateResourcesCpu, CalculateResourcesCpuParamHandler
+from atf_metrics.calculate_resources_mem import CalculateResourcesMem, CalculateResourcesMemParamHandler
+from atf_metrics.calculate_resources_io import CalculateResourcesIO, CalculateResourcesIOParamHandler
+from atf_metrics.calculate_resources_network import CalculateResourcesNetwork, CalculateResourcesNetworkParamHandler
 from atf_metrics.calculate_distance_to_obstacles import CalculateDistanceToObstacles, CalculateDistanceToObstaclesParamHandler
 from atf_metrics.calculate_publish_rate import CalculatePublishRate, CalculatePublishRateParamHandler
 from atf_metrics.calculate_interface import CalculateInterface, CalculateInterfaceParamHandler

atf_metrics/src/atf_metrics/calculate_distance.py

@@ -0,0 +1,105 @@
+#!/usr/bin/env python
+import rospy
+import tf
+import math
+
+class CalculateDistanceParamHandler:
+    def __init__(self):
+        """
+        Class for returning the corresponding metric class with the given parameter.
+        """
+        pass
+
+    def parse_parameter(self, testblock_name, params):
+        """
+        Method that returns the metric method with the given parameter.
+        :param params: Parameter
+        """
+        metrics = []
+        if type(params) is not list:
+            rospy.logerr("metric config not a list")
+            return False
+
+        for metric in params:
+            # check for optional parameters
+            try:
+                groundtruth = metric["groundtruth"]
+                groundtruth_epsilon = metric["groundtruth_epsilon"]
+            except (TypeError, KeyError):
+                rospy.logwarn("No groundtruth parameters given, skipping groundtruth evaluation for metric 'distance' in testblock '%s'", testblock_name)
+                groundtruth = None
+                groundtruth_epsilon = None
+            metrics.append(CalculateDistance(metric["root_frame"], metric["measured_frame"], groundtruth, groundtruth_epsilon))
+        return metrics
+
+class CalculateDistance:
+    def __init__(self, root_frame, measured_frame, groundtruth, groundtruth_epsilon):
+        """
+        Class for calculating the distance to a given root frame.
+        The tf data is sent over the tf topic given in the robot_config.yaml.
+        :param root_frame: name of the first frame
+        :type  root_frame: string
+        :param measured_frame: name of the second frame. The distance will be measured in relation to the root_frame.
+        :type  measured_frame: string
+        """
+
+        self.active = False
+        self.root_frame = root_frame
+        self.measured_frame = measured_frame
+        self.distance = 0.0
+        self.distances = 0.0
+        self.dist_count = 0
+        self.tf_sampling_freq = 20.0  # Hz
+        self.groundtruth = groundtruth
+        self.groundtruth_epsilon = groundtruth_epsilon
+        self.finished = False
+
+        self.listener = tf.TransformListener()
+
+        rospy.Timer(rospy.Duration.from_sec(1 / self.tf_sampling_freq), self.record_tf)
+
+    def start(self, timestamp):
+        self.active = True
+
+    def stop(self, timestamp):
+        self.active = False
+        self.finished = True
+
+    def pause(self, timestamp):
+        self.active = False
+        self.first_value = True
+
+    def purge(self, timestamp):
+        pass
+
+    def record_tf(self, event):
+        if self.active:
+            try:
+                self.listener.waitForTransform(self.root_frame,
+                                               self.measured_frame,
+                                               rospy.Time(0),
+                                               rospy.Duration.from_sec(1 / (2*self.tf_sampling_freq)))
+                (trans, rot) = self.listener.lookupTransform(self.root_frame, self.measured_frame, rospy.Time(0))
+
+            except (tf.Exception, tf.LookupException, tf.ConnectivityException):
+                #rospy.logwarn(e)
+                pass
+            else:
+                self.distances = self.distances + math.sqrt(trans[0]**2 + trans[1]**2)
+                self.dist_count = self.dist_count + 1
+
+
+    def get_result(self):
+        groundtruth_result = None
+        details = {"root_frame": self.root_frame, "measured_frame": self.measured_frame}
+        if self.finished:
+            #print "Distances: "+str(self.distances)+" Dist Count: "+str(self.dist_count)
+            data = round(self.distances/self.dist_count, 3)
+            if self.groundtruth != None and self.groundtruth_epsilon != None:
+                if math.fabs(self.groundtruth - data) <= self.groundtruth_epsilon:
+                    groundtruth_result = True
+                else:
+                    groundtruth_result = False
+            return "distance", data, groundtruth_result, self.groundtruth, self.groundtruth_epsilon, details
+        else:
+            return False