Merge pull request #23 from simutisernestas/uuid

uuid for obstacle message

Author: SteveMacenski

kf_hungarian_tracker/kf_hungarian_tracker/kf_hungarian_node.py

@@ -1,4 +1,5 @@
 import numpy as np 
+import uuid
 from scipy.optimize import linear_sum_assignment
 
 from nav2_dynamic_msgs.msg import Obstacle, ObstacleArray
@@ -23,7 +24,6 @@ class KFHungarianTracker(Node):
 
     Attributes:
         obstacle_list: a list of ObstacleClass that currently present in the scene
-        max_id: the maximum id assigned 
         sec, nanosec: timing from sensor msg
         detection_sub: subscrib detection result from detection node
         tracker_obstacle_pub: publish tracking obstacles with ObstacleArray
@@ -58,7 +58,6 @@ def __init__(self):
         self.cost_filter = self.get_parameter("cost_filter")._value
 
         self.obstacle_list = []
-        self.max_id = 0
         self.sec = 0
         self.nanosec = 0
 
@@ -167,11 +166,13 @@ def callback(self, msg):
             marker_list = []
             # add current active obstacles
             for obs in filtered_obstacle_list:
-                (r, g, b) = colorsys.hsv_to_rgb(obs.msg.id * 30. % 360 / 360., 1., 1.) # encode id with rgb color
+                obstacle_uuid = uuid.UUID(bytes=bytes(obs.msg.uuid.uuid))
+                (r, g, b) = colorsys.hsv_to_rgb(obstacle_uuid.int % 360 / 360., 1., 1.) # encode id with rgb color
                 # make a cube 
                 marker = Marker()
                 marker.header = msg.header
-                marker.id = obs.msg.id
+                marker.ns = str(obstacle_uuid)
+                marker.id = 0
                 marker.type = 1 # CUBE
                 marker.action = 0
                 marker.color.a = 0.5
@@ -187,7 +188,8 @@ def callback(self, msg):
                 # make an arrow
                 arrow = Marker()
                 arrow.header = msg.header
-                arrow.id = 255 - obs.msg.id
+                arrow.ns = str(obstacle_uuid)
+                arrow.id = 1 
                 arrow.type = 0
                 arrow.action = 0
                 arrow.color.a = 1.0
@@ -202,27 +204,28 @@ def callback(self, msg):
                 arrow.scale.z = 0.05
                 marker_list.append(arrow)
             # add dead obstacles to delete in rviz
-            for idx in dead_object_list:
+            for uuid in dead_object_list:
                 marker = Marker()
                 marker.header = msg.header
-                marker.id = idx
+                marker.ns = str(uuid)
+                marker.id = 0
                 marker.action = 2 # delete
                 arrow = Marker()
                 arrow.header = msg.header
-                arrow.id = 255 - idx
+                arrow.ns = str(uuid)
+                arrow.id = 1
                 arrow.action = 2
                 marker_list.append(marker)
                 marker_list.append(arrow)
-
             marker_array.markers = marker_list
             self.tracker_marker_pub.publish(marker_array)
 
     def birth(self, det_ind, num_of_detect, detections):
         '''generate new ObstacleClass for detections that do not match any in current obstacle list'''
         for det in range(num_of_detect):
             if det not in det_ind:
-                self.obstacle_list.append(ObstacleClass(detections[det], self.max_id, self.top_down, self.measurement_noise_cov, self.error_cov_post, self.process_noise_cov))
-                self.max_id =  self.max_id  + 1
+                obstacle = ObstacleClass(detections[det], self.top_down, self.measurement_noise_cov, self.error_cov_post, self.process_noise_cov)
+                self.obstacle_list.append(obstacle)
 
     def death(self, obj_ind, num_of_obstacle):
         '''count obstacles' missing frames and delete when reach threshold'''
@@ -238,7 +241,8 @@ def death(self, obj_ind, num_of_obstacle):
             if self.obstacle_list[obs].dying < self.death_threshold:
                 new_object_list.append(self.obstacle_list[obs])
             else:
-                dead_object_list.append(self.obstacle_list[obs].msg.id)
+                obstacle_uuid = uuid.UUID(bytes=bytes(self.obstacle_list[obs].msg.uuid.uuid))
+                dead_object_list.append(obstacle_uuid)
 
         # add newly born obstacles
         for obs in range(num_of_obstacle, len(self.obstacle_list)):

kf_hungarian_tracker/kf_hungarian_tracker/obstacle_class.py

@@ -1,5 +1,7 @@
 import numpy as np 
 import cv2
+import uuid 
+from unique_identifier_msgs.msg import UUID
 
 class ObstacleClass:
     """wrap a kalman filter and extra information for one single obstacle
@@ -13,10 +15,14 @@ class ObstacleClass:
         dying: count missing frames for this obstacle, if reach threshold, delete this obstacle
     """
 
-    def __init__(self, obstacle_msg, idx, top_down, measurement_noise_cov, error_cov_post, process_noise_cov):
+    def __init__(self, obstacle_msg, top_down, measurement_noise_cov, error_cov_post, process_noise_cov):
         '''Initialize with an Obstacle msg and an assigned id'''
         self.msg = obstacle_msg
-        self.msg.id = idx
+
+        uuid_msg = UUID()
+        uuid_msg.uuid = list(uuid.uuid4().bytes)
+        self.msg.uuid = uuid_msg
+        
         position = np.array([[obstacle_msg.position.x, obstacle_msg.position.y, obstacle_msg.position.z]]).T # shape 3*1
         velocity = np.array([[obstacle_msg.velocity.x, obstacle_msg.velocity.y, obstacle_msg.velocity.z]]).T
 

nav2_dynamic_msgs/CMakeLists.txt

@@ -25,11 +25,12 @@ find_package(rosidl_default_generators REQUIRED)
 find_package(builtin_interfaces REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
+find_package(unique_identifier_msgs REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/Obstacle.msg"
   "msg/ObstacleArray.msg"
-  DEPENDENCIES std_msgs sensor_msgs
+  DEPENDENCIES std_msgs sensor_msgs unique_identifier_msgs
  )
 
 if(BUILD_TESTING)

nav2_dynamic_msgs/msg/Obstacle.msg

@@ -1,4 +1,4 @@
-int8 id
+unique_identifier_msgs/UUID uuid
 float32 score # detection confidence
 geometry_msgs/Point position # center
 geometry_msgs/Vector3 velocity

nav2_dynamic_msgs/package.xml

@@ -16,6 +16,7 @@
 
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
+  <depend>unique_identifier_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>