Merge pull request #10 from CMU-cabot/ishihara/fix-predict-stationary

fix bug, stationary tag is not working correctly for people, obstacle prediction

track_people_py/scripts/predict_kf_obstacle.py

@@ -24,6 +24,7 @@
 import signal
 import sys
 
+import numpy as np
 import rclpy
 from geometry_msgs.msg import Point
 from people_msgs.msg import People, Person
@@ -33,8 +34,8 @@
 
 
 class PredictKfObstacle(PredictKfAbstract):
-    def __init__(self, input_time, duration_inactive_to_remove):
-        super().__init__('predict_obstacle_py', input_time, duration_inactive_to_remove)
+    def __init__(self):
+        super().__init__('predict_obstacle_py')
 
     def pub_result(self, msg, alive_track_id_list, track_pos_dict, track_vel_dict, track_vel_hist_dict):
         # init People message
@@ -60,22 +61,25 @@ def pub_result(self, msg, alive_track_id_list, track_pos_dict, track_vel_dict, t
             else:
                 person.reliability = 0.9
 
-            tvel = 0
-            tvelcount = 1
-            enough_duration = False
-            for i in range(-1, -len(track_vel_hist_dict[track_id]), -1):
-                (timestamp, vel) = track_vel_hist_dict[track_id][i]
-                if (rclpy.time.Time.from_msg(msg.header.stamp) - timestamp).nanoseconds/1e9 > self.stationary_detect_threshold_duration_:
-                    enough_duration = True
-                    break
-                tvel += math.sqrt(vel[0]*vel[0]+vel[1]*vel[1])
-                tvelcount += 1
-
-            if enough_duration and tvelcount > 0 and tvel / tvelcount < 0.1:
-                person.tags.append("stationary")
-            else:
-                if "stationary" in person.tags:
-                    person.tags.remove("stationary")
+            # calculate median velocity of track in recent time window to remove noise
+            track_vel_hist = []
+            for (track_hist_time, track_hist_vel) in track_vel_hist_dict[track_id]:
+                track_vel_hist.append(np.linalg.norm([track_hist_vel[0], track_hist_vel[1]]))
+            track_vel_hist_median = np.median(track_vel_hist)
+
+            # check if track is in stationary state
+            if track_vel_hist_median < self.stationary_detect_threshold_velocity_:
+                if track_id not in self.predict_buf.track_id_stationary_start_time_dict:
+                    # record time to start stationary state
+                    self.predict_buf.track_id_stationary_start_time_dict[track_id] = rclpy.time.Time.from_msg(msg.header.stamp)
+                else:
+                    # add stationary tag if enough time passes after starting stationary state
+                    stationary_start_time = self.predict_buf.track_id_stationary_start_time_dict[track_id]
+                    if (rclpy.time.Time.from_msg(msg.header.stamp) - stationary_start_time).nanoseconds/1e9 > self.stationary_detect_threshold_duration_:
+                        person.tags.append("stationary")
+            elif track_id in self.predict_buf.track_id_stationary_start_time_dict:
+                # clear time to start stationary state
+                del self.predict_buf.track_id_stationary_start_time_dict[track_id]
 
             people_msg.people.append(person)
 
@@ -85,10 +89,7 @@ def pub_result(self, msg, alive_track_id_list, track_pos_dict, track_vel_dict, t
 def main():
     rclpy.init()
 
-    input_time = 5  # number of frames to start prediction
-    duration_inactive_to_remove = 2.0  # duration (seconds) for a track to be inactive before removal (this value should be enough long because track_obstacle_py resturns recovered tracks)
-
-    predict_people = PredictKfObstacle(input_time, duration_inactive_to_remove)
+    predict_people = PredictKfObstacle()
 
     plt.ion()
     plt.show()

track_people_py/scripts/predict_kf_people.py

@@ -24,6 +24,7 @@
 import signal
 import sys
 
+import numpy as np
 import rclpy
 from geometry_msgs.msg import Point
 from people_msgs.msg import People, Person
@@ -33,8 +34,8 @@
 
 
 class PredictKfPeople(PredictKfAbstract):
-    def __init__(self, input_time, duration_inactive_to_remove):
-        super().__init__('predict_people_py', input_time, duration_inactive_to_remove)
+    def __init__(self):
+        super().__init__('predict_people_py')
 
         # start initialization
         self.publish_simulator_people = self.declare_parameter("publish_simulator_people", False).value
@@ -79,22 +80,25 @@ def pub_result(self, msg, alive_track_id_list, track_pos_dict, track_vel_dict, t
             else:
                 person.reliability = 0.9
 
-            tvel = 0
-            tvelcount = 1
-            enough_duration = False
-            for i in range(-1, -len(track_vel_hist_dict[track_id]), -1):
-                (timestamp, vel) = track_vel_hist_dict[track_id][i]
-                if (rclpy.time.Time.from_msg(msg.header.stamp) - timestamp).nanoseconds/1e9 > self.stationary_detect_threshold_duration_:
-                    enough_duration = True
-                    break
-                tvel += math.sqrt(vel[0]*vel[0]+vel[1]*vel[1])
-                tvelcount += 1
-
-            if enough_duration and tvelcount > 0 and tvel / tvelcount < 0.1:
-                person.tags.append("stationary")
-            else:
-                if "stationary" in person.tags:
-                    person.tags.remove("stationary")
+            # calculate median velocity of track in recent time window to remove noise
+            track_vel_hist = []
+            for (track_hist_time, track_hist_vel) in track_vel_hist_dict[track_id]:
+                track_vel_hist.append(np.linalg.norm([track_hist_vel[0], track_hist_vel[1]]))
+            track_vel_hist_median = np.median(track_vel_hist)
+
+            # check if track is in stationary state
+            if track_vel_hist_median < self.stationary_detect_threshold_velocity_:
+                if track_id not in self.predict_buf.track_id_stationary_start_time_dict:
+                    # record time to start stationary state
+                    self.predict_buf.track_id_stationary_start_time_dict[track_id] = rclpy.time.Time.from_msg(msg.header.stamp)
+                else:
+                    # add stationary tag if enough time passes after starting stationary state
+                    stationary_start_time = self.predict_buf.track_id_stationary_start_time_dict[track_id]
+                    if (rclpy.time.Time.from_msg(msg.header.stamp) - stationary_start_time).nanoseconds/1e9 > self.stationary_detect_threshold_duration_:
+                        person.tags.append("stationary")
+            elif track_id in self.predict_buf.track_id_stationary_start_time_dict:
+                # clear time to start stationary state
+                del self.predict_buf.track_id_stationary_start_time_dict[track_id]
 
             people_msg.people.append(person)
 
@@ -114,10 +118,7 @@ def on_tracked_boxes_cb(self, msg):
 def main():
     rclpy.init()
 
-    input_time = 5  # number of frames to start prediction
-    duration_inactive_to_remove = 2.0  # duration (seconds) for a track to be inactive before removal (this value should be enough long because track_people_py resturns recovered tracks)
-
-    predict_people = PredictKfPeople(input_time, duration_inactive_to_remove)
+    predict_people = PredictKfPeople()
 
     plt.ion()
     plt.show()

track_people_py/track_people_py/predict_kf_abstract.py

@@ -51,22 +51,23 @@ def __init__(self):
 
         self.track_id_kf_model_dict = {}
         self.track_id_missing_time_dict = {}
+        self.track_id_stationary_start_time_dict = {}
 
 
 
 class PredictKfAbstract(rclpy.node.Node):
-    def __init__(self, name, input_time, duration_inactive_to_remove):
+    def __init__(self, name):
         super().__init__(name)
         # settings for visualization
         self.vis_pred_image = False
 
         # start initialization
-        self.input_time = input_time
+        self.input_time = self.declare_parameter('input_time', 5).value # number of frames to start prediction
         self.kf_init_var = self.declare_parameter('kf_init_var', 1.0).value
         self.kf_process_var = self.declare_parameter('kf_process_var', 1000.0).value
         self.kf_measure_var = self.declare_parameter('kf_measure_var', 1.0).value
-        self.duration_inactive_to_remove = duration_inactive_to_remove
-        self.duration_inactive_to_stop_publish = self.declare_parameter('duration_inactive_to_stop_publish', 0.2).value # duration (seconds) for a track to be inactive before stop publishing in people topic
+        self.duration_inactive_to_remove = self.declare_parameter('duration_inactive_to_remove', 2.0).value # duration (seconds) to remove an inactive track (this value should be enough long because track_obstacle_py returns recovered tracks)
+        self.duration_inactive_to_stop_publish = self.declare_parameter('duration_inactive_to_stop_publish', 0.2).value # duration (seconds) to stop publishing an inactive track in people topic
 
         # buffers to predict
         self.predict_buf = PredictKfBuffer()
@@ -88,6 +89,7 @@ def __init__(self, name, input_time, duration_inactive_to_remove):
                                              FrequencyStatusParam({'min': target_fps, 'max': target_fps}, 0.2, 2))
 
         self.stationary_detect_threshold_duration_ = self.declare_parameter('stationary_detect_threshold_duration', 1.0).value
+        self.stationary_detect_threshold_velocity_ = self.declare_parameter('stationary_detect_threshold_velocity', 0.1).value
 
     @abstractmethod
     def pub_result(self, msg, alive_track_id_list, track_pos_dict, track_vel_dict, track_vel_hist_dict):
@@ -276,8 +278,12 @@ def tracked_boxes_cb(self, msg):
             if (now - self.predict_buf.track_id_missing_time_dict[track_id]).nanoseconds/1000000000 > self.duration_inactive_to_remove:
                 del self.predict_buf.track_input_queue_dict[track_id]
                 del self.predict_buf.track_time_queue_dict[track_id]
+                if track_id in self.predict_buf.track_vel_hist_dict:
+                    del self.predict_buf.track_vel_hist_dict[track_id]
                 del self.predict_buf.track_color_dict[track_id]
                 del self.predict_buf.track_id_missing_time_dict[track_id]
+                if track_id in self.predict_buf.track_id_stationary_start_time_dict:
+                    del self.predict_buf.track_id_stationary_start_time_dict[track_id]
                 if track_id in self.predict_buf.track_id_kf_model_dict:
                     del self.predict_buf.track_id_kf_model_dict[track_id]