[hrsi_representation] Adding correct smoothing and unittest

hrsi_representation/CMakeLists.txt

@@ -56,3 +56,7 @@ install(PROGRAMS
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
+if (CATKIN_ENABLE_TESTING)
+  find_package(catkin REQUIRED COMPONENTS rostest)
+  add_rostest(tests/online_creator_tester.test)
+endif()

hrsi_representation/package.xml

@@ -19,6 +19,8 @@
   <build_depend>nav_msgs</build_depend>
   <build_depend>qsr_lib</build_depend>
   <build_depend>rospy</build_depend>
+  <build_depend>roslib</build_depend>
+  <build_depend>rostest</build_depend>
   <build_depend>sensor_msgs</build_depend>
   <build_depend>std_msgs</build_depend>
   <run_depend>bayes_people_tracker</run_depend>
@@ -27,9 +29,13 @@
   <run_depend>nav_msgs</run_depend>
   <run_depend>qsr_lib</run_depend>
   <run_depend>rospy</run_depend>
+  <run_depend>roslib</run_depend>
+  <run_depend>rostest</run_depend>
   <run_depend>sensor_msgs</run_depend>
   <run_depend>std_msgs</run_depend>
 
+  <test_depend>rosunit</test_depend>
+
 
   <export/>
 </package>

hrsi_representation/scripts/online_qtc_creator.py

@@ -16,6 +16,7 @@
 import numpy as np
 import tf
 import json
+import thread
 
 class OnlineQTCCreator(object):
     """Creates QTC state sequences from online input"""
@@ -27,29 +28,36 @@ class OnlineQTCCreator(object):
     }
     _robot_pose = None
     _buffer = dict()
+    _smoothing_buffer = dict()
+    _msg_buffer = []
 
     def __init__(self, name):
         rospy.loginfo("Starting %s" % name)
-        self.input        = OnlineInput()
-        ppl_topic         = rospy.get_param("~ppl_topic", "/people_tracker/positions")
-        robot_topic       = rospy.get_param("~robot_topic", "/robot_pose")
-        self.target_frame = rospy.get_param("~target_frame", "/map")
-        self.dyn_srv      = DynServer(OnlineQTCCreatorConfig, self.dyn_callback)
-        self.listener     = tf.TransformListener()
-        self.pub          = rospy.Publisher("~qtc_array", QTCArray, queue_size=10)
+        self.input           = OnlineInput()
+        ppl_topic            = rospy.get_param("~ppl_topic", "/people_tracker/positions")
+        robot_topic          = rospy.get_param("~robot_topic", "/robot_pose")
+        self.target_frame    = rospy.get_param("~target_frame", "/map")
+        self.decay_time      = rospy.get_param("~decay_time", 120.)
+        self.processing_rate = rospy.get_param("~processing_rate", 30)
+        self.dyn_srv         = DynServer(OnlineQTCCreatorConfig, self.dyn_callback)
+        self.listener        = tf.TransformListener()
+        self.pub             = rospy.Publisher("~qtc_array", QTCArray, queue_size=10)
+        self.last_msg        = QTCArray()
         rospy.Subscriber(
             ppl_topic,
             PeopleTracker,
             callback=self.ppl_callback,
-            queue_size=1
+            queue_size=10
         )
         rospy.Subscriber(
             robot_topic,
             Pose,
             callback=self.pose_callback,
-            queue_size=1
+            queue_size=10
         )
 
+        self.request_thread = thread.start_new(self.generate_qtc, ())
+
     def dyn_callback(self, config, level):
         self.qtc_type            = self._qtc_types[config["qtc_type"]]
         self.quantisation_factor = config["quantisation_factor"]
@@ -60,47 +68,103 @@ def dyn_callback(self, config, level):
         return config
 
     def ppl_callback(self, msg):
-        print len(self._buffer)
-        out = QTCArray()
-        out.header = msg.header
-        out.header.frame_id = self.target_frame
-
-        for (uuid, pose) in zip(msg.uuids, msg.poses):
-            person = PoseStamped()
-            person.header = msg.header
-            person.pose = pose
-            try:
-                self.listener.waitForTransform(msg.header.frame_id, self.target_frame, msg.header.stamp, rospy.Duration(1.0))
-                transformed = self.listener.transformPose(self.target_frame, person)
-            except (tf.Exception, tf.LookupException, tf.ConnectivityException) as ex:
-                rospy.logwarn(ex)
-                return
-
-            if self._robot_pose:
-                if not uuid in self._buffer.keys():
-                    self._buffer[uuid] = {"data": np.array(
+        msgs = {
+            "ppl": msg,
+            "robot": self._robot_pose
+        }
+        self._msg_buffer.append(msgs)
+
+    def pose_callback(self, msg):
+        self._robot_pose = msg
+
+    def generate_qtc(self):
+        rate = rospy.Rate(self.processing_rate)
+        while not rospy.is_shutdown():
+            if not self._msg_buffer:
+                rate.sleep()
+                continue
+
+            ppl_msg = self._msg_buffer[0]["ppl"]
+            robot_msg = self._msg_buffer[0]["robot"]
+            del self._msg_buffer[0]
+            # Creating an new message
+            out = QTCArray()
+            out.header = ppl_msg.header
+            out.header.frame_id = self.target_frame
+
+            # Looping through detected humans
+            for (uuid, pose) in zip(ppl_msg.uuids, ppl_msg.poses):
+                # Transforming pose into target_frame if necessary
+                person = PoseStamped()
+                person.header = ppl_msg.header
+                person.pose = pose
+                if ppl_msg.header.frame_id != self.target_frame:
+                    try:
+                        self.listener.waitForTransform(ppl_msg.header.frame_id, self.target_frame, ppl_msg.header.stamp, rospy.Duration(1.0))
+                        transformed = self.listener.transformPose(self.target_frame, person)
+                    except (tf.Exception, tf.LookupException, tf.ConnectivityException) as ex:
+                        rospy.logwarn(ex)
+                        return
+                else:
+                    transformed = person
+
+
+                if not uuid in self._smoothing_buffer.keys(): # No entry yet
+                    self._smoothing_buffer[uuid] = {
+                        "start_time": ppl_msg.header.stamp.to_sec(),
+                        "data": np.array(
+                            [
+                                robot_msg.position.x,
+                                robot_msg.position.y,
+                                transformed.pose.position.x,
+                                transformed.pose.position.y
+                            ]
+                    ).reshape(-1,4), "last_seen": ppl_msg.header.stamp.to_sec()}
+                else: # Already in buffer
+                    self._smoothing_buffer[uuid]["data"] = np.append(
+                        self._smoothing_buffer[uuid]["data"],
                         [
-                            self._robot_pose.position.x,
-                            self._robot_pose.position.y,
+                            robot_msg.position.x,
+                            robot_msg.position.y,
                             transformed.pose.position.x,
                             transformed.pose.position.y
                         ]
-                    ).reshape(-1,4), "last_seen": msg.header.stamp.to_sec()}
-                else:
+                    ).reshape(-1,4)
+
+            # Flush smoothing buffer and create QSR
+            # Looping through smoothing buffer
+            for uuid, data in self._smoothing_buffer.items():
+                # If the smoothing time is not up, do nothing for this entry
+                if not data["start_time"] + self.smoothing_rate <= ppl_msg.header.stamp.to_sec():
+                    continue
+
+                # Put smoothed values in buffer
+                if not uuid in self._buffer.keys(): # No entry yet, create a new one
+                    self._buffer[uuid] = {"data": np.array(
+                        [
+                            np.mean(data["data"][:,0]), # Mean over the coordinates to smooth them
+                            np.mean(data["data"][:,1]),
+                            np.mean(data["data"][:,2]),
+                            np.mean(data["data"][:,3])
+                        ]
+                    ).reshape(-1,4), "last_seen": ppl_msg.header.stamp.to_sec()}
+                else: # Already in buffer, append latest values
                     self._buffer[uuid]["data"] = np.append(
                         self._buffer[uuid]["data"],
                         [
-                            self._robot_pose.position.x,
-                            self._robot_pose.position.y,
-                            transformed.pose.position.x,
-                            transformed.pose.position.y
+                            np.mean(data["data"][:,0]),
+                            np.mean(data["data"][:,1]),
+                            np.mean(data["data"][:,2]),
+                            np.mean(data["data"][:,3])
                         ]
                     ).reshape(-1,4)
-                    if self._buffer[uuid]["data"].shape[0] > 3:
-                        self._buffer[uuid]["data"] = self._buffer[uuid]["data"][:-1] if np.allclose(self._buffer[uuid]["data"][-1], self._buffer[uuid]["data"][-3]) else self._buffer[uuid]["data"]
-                self._buffer[uuid]["last_seen"] = msg.header.stamp.to_sec()
+                self._buffer[uuid]["last_seen"] = ppl_msg.header.stamp.to_sec() # Add time of laast update for decay
 
-                if self._buffer[uuid]["data"].shape[0] > 2:
+                del self._smoothing_buffer[uuid] # Delete element from smoothing buffer
+
+                # If there are more than 1 entries in the buffer for this person
+                # Create QTC representation
+                if self._buffer[uuid]["data"].shape[0] > 1:
                     qtc = self.input.convert(
                         data=self.input.generate_data_from_input(
                             agent1="Robot",
@@ -117,6 +181,7 @@ def ppl_callback(self, msg):
                         distance_threshold=self.distance_threshold
                     )[0]
 
+                    # Create new message
                     qtc_msg                     = QTC()
                     qtc_msg.collapsed           = not self.no_collapse
                     qtc_msg.qtc_type            = self.qtc_type
@@ -131,19 +196,18 @@ def ppl_callback(self, msg):
 
                     out.qtc.append(qtc_msg)
 
-        self.pub.publish(out)
-        self.decay()
-        rospy.sleep(self.smoothing_rate)
+            # If there is something to publish and it heasn't been published before, publish
+            if out.qtc and out.qtc != self.last_msg.qtc:
+                self.pub.publish(out)
+                self.last_msg = out
+            self.decay(ppl_msg.header.stamp) # Delete old elements from buffer
+            rate.sleep()
 
-    def pose_callback(self, msg):
-        self._robot_pose = msg
-
-    def decay(self):
+    def decay(self, last_time):
         for uuid in self._buffer.keys():
-            if self._buffer[uuid]["last_seen"] + 120. < rospy.Time.now().to_sec():
+            if self._buffer[uuid]["last_seen"] + self.decay_time < last_time.to_sec():
                 del self._buffer[uuid]
 
-
 if __name__ == "__main__":
     rospy.init_node("online_qtc_creator")
     oqc = OnlineQTCCreator(rospy.get_name())

hrsi_representation/tests/data/correct.txt

@@ -0,0 +1 @@
+[[[-1, -1, 0, 0]], [[-1, -1, 0, 0], [-1, -1, 1, 0]], [[-1, -1, 0, 0], [-1, -1, 1, 0], [-1, -1, 0, 0]], [[-1, -1, 0, 0], [-1, -1, 1, 0], [-1, -1, 0, 0], [-1, -1, 1, 0]], [[-1, -1, 0, 0], [-1, -1, 1, 0], [-1, -1, 0, 0], [-1, -1, 1, 0], [-1, -1, 1, 1]], [[-1, -1, 0, 0], [-1, -1, 1, 0], [-1, -1, 0, 0], [-1, -1, 1, 0], [-1, -1, 1, 1], [0, 0, 1, 1]], [[-1, -1, 0, 0], [-1, -1, 1, 0], [-1, -1, 0, 0], [-1, -1, 1, 0], [-1, -1, 1, 1], [0, 0, 1, 1], [0, 0, 1, 0]], [[-1, -1, 0, 0], [-1, -1, 1, 0], [-1, -1, 0, 0], [-1, -1, 1, 0], [-1, -1, 1, 1], [0, 0, 1, 1], [0, 0, 1, 0], [0, 0, 1, 1]], [[-1, -1, 0, 0], [-1, -1, 1, 0], [-1, -1, 0, 0], [-1, -1, 1, 0], [-1, -1, 1, 1], [0, 0, 1, 1], [0, 0, 1, 0], [0, 0, 1, 1], [0, 0, 1, 0], [1, 0, 1, 0]], [[-1, 0, 0, -1]], [[-1, 0, 0, -1], [-1, -1, 0, -1]], [[-1, 0, 0, -1], [-1, -1, 0, -1], [-1, -1, 0, 0]], [[-1, 0, 0, -1], [-1, -1, 0, -1], [-1, -1, 0, 0], [-1, -1, 0, -1]], [[-1, 0, 0, -1], [-1, -1, 0, -1], [-1, -1, 0, 0], [-1, -1, 0, -1], [-1, 0, 0, -1]], [[-1, 0, 0, -1], [-1, -1, 0, -1], [-1, -1, 0, 0], [-1, -1, 0, -1], [-1, 0, 0, -1], [-1, 0, -1, -1]], [[-1, 0, 0, -1], [-1, -1, 0, -1], [-1, -1, 0, 0], [-1, -1, 0, -1], [-1, 0, 0, -1], [-1, 0, -1, -1], [0, 0, -1, -1]], [[-1, 0, 0, -1], [-1, -1, 0, -1], [-1, -1, 0, 0], [-1, -1, 0, -1], [-1, 0, 0, -1], [-1, 0, -1, -1], [0, 0, -1, -1], [0, 0, -1, 0], [0, 1, -1, 0]], [[-1, 0, 0, -1], [-1, -1, 0, -1], [-1, -1, 0, 0], [-1, -1, 0, -1], [-1, 0, 0, -1], [-1, 0, -1, -1], [0, 0, -1, -1], [0, 0, -1, 0], [0, 1, -1, 0], [1, 1, -1, 0]], [[0, 0, 0, 0]], [[0, 0, 0, 0]], [[0, 0, 0, 0], [-1, -1, 0, 0]], [[0, 0, 0, 0], [-1, -1, 0, 0], [-1, -1, 0, -1]], [[0, 0, 0, 0], [-1, -1, 0, 0], [-1, -1, 0, -1], [-1, 0, 0, -1], [-1, 0, -1, -1]], [[0, 0, 0, 0], [-1, -1, 0, 0], [-1, -1, 0, -1], [-1, 0, 0, -1], [-1, 0, -1, -1], [0, 0, -1, -1]], [[0, 0, 0, 0], [-1, -1, 0, 0], [-1, -1, 0, -1], [-1, 0, 0, -1], [-1, 0, -1, -1], [0, 0, -1, -1], [0, 0, -1, 0]], [[0, 0, 0, 0], [-1, -1, 0, 0], [-1, -1, 0, -1], [-1, 0, 0, -1], [-1, 0, -1, -1], [0, 0, -1, -1], [0, 0, -1, 0], [1, 1, -1, 0]], [[0, 0, 0, 0], [-1, -1, 0, 0], [-1, -1, 0, -1], [-1, 0, 0, -1], [-1, 0, -1, -1], [0, 0, -1, -1], [0, 0, -1, 0], [1, 1, -1, 0], [1, 1, 0, 0]], [[0, -1, 0, 0]], [[0, -1, 0, 0], [-1, -1, 0, 0]], [[0, -1, 0, 0], [-1, -1, 0, 0], [-1, -1, 0, 1]], [[0, -1, 0, 0], [-1, -1, 0, 0], [-1, -1, 0, 1], [-1, 0, 0, 1], [-1, 0, 1, 1]], [[0, -1, 0, 0], [-1, -1, 0, 0], [-1, -1, 0, 1], [-1, 0, 0, 1], [-1, 0, 1, 1], [0, 0, 1, 1]], [[0, -1, 0, 0], [-1, -1, 0, 0], [-1, -1, 0, 1], [-1, 0, 0, 1], [-1, 0, 1, 1], [0, 0, 1, 1], [0, 0, 1, 0]], [[0, -1, 0, 0], [-1, -1, 0, 0], [-1, -1, 0, 1], [-1, 0, 0, 1], [-1, 0, 1, 1], [0, 0, 1, 1], [0, 0, 1, 0], [1, 1, 1, 0]], [[0, -1, 0, 0], [-1, -1, 0, 0], [-1, -1, 0, 1], [-1, 0, 0, 1], [-1, 0, 1, 1], [0, 0, 1, 1], [0, 0, 1, 0], [1, 1, 1, 0], [1, 0, 0, 0]]]

hrsi_representation/tests/online_creator_tester.py

@@ -0,0 +1,51 @@
+#!/usr/bin/env python
+PKG = 'hrsi_representation'
+NAME = 'online_creator_tester'
+
+import rospy
+import sys
+import unittest
+from roslib.packages import find_resource
+from hrsi_representation.msg import QTCArray
+import subprocess
+import json
+import os
+
+
+class TestOnlineQTC(unittest.TestCase):
+    RESULTS_FILE = find_resource(PKG, 'correct.txt')[0]
+    BAG_FILE = find_resource(PKG, 'test.bag')[0]
+
+    def __init__(self, *args):
+        super(TestOnlineQTC, self).__init__(*args)
+
+        rospy.init_node(NAME)
+
+        self.correct = self._load_file()
+        #print self.correct
+        self.test = []
+        self.seq = 0
+
+    def _load_file(self):
+        return json.load(file(self.RESULTS_FILE))
+
+    def _callback(self, msg):
+        self.seq = msg.header.seq
+        if msg.qtc:
+            self.test.append(json.loads(msg.qtc[0].qtc_serialised))
+
+    def test_online_creator(self):
+        rospy.Subscriber("/online_qtc_creator/qtc_array", QTCArray, callback=self._callback)
+        with open(os.devnull, 'w') as FNULL:
+            p = subprocess.Popen("rosbag play -r 2 "+self.BAG_FILE, stdin=subprocess.PIPE, shell=True, stdout=FNULL)
+            while p.poll() == None:
+                rospy.sleep(1)
+        #print self.test
+        self.assertEqual(self.test, self.correct)
+
+
+if __name__ == '__main__':
+    import rostest
+    rostest.rosrun(PKG, NAME, TestOnlineQTC, sys.argv)
+#    p = TestOnlineQTC(sys.argv)
+#    p.test_online_creator()

hrsi_representation/tests/online_creator_tester.test

@@ -0,0 +1,10 @@
+<launch>
+
+  <node name="qsr_lib" pkg="qsr_lib" type="qsrlib_ros_server.py" />
+  <node pkg="hrsi_representation" type="online_qtc_creator.py" name="online_qtc_creator" output="screen">
+    <param name="processing_rate" type="int" value="60"/>
+  </node>
+
+  <test test-name="online_creator_tester" pkg="hrsi_representation" type="online_creator_tester.py" />
+
+</launch>