Stable Model Detection

get_obj_dist/src/Camera Info

+header:
+  seq: 69
+  stamp:
+    secs: 1573208358
+    nsecs: 408673186
+  frame_id: "camera_depth_optical_frame"
+height: 480
+width: 640
+distortion_model: "plumb_bob"
+D: [0.0, 0.0, 0.0, 0.0, 0.0]
+K: [388.9610595703125, 0.0, 319.34539794921875, 0.0, 388.9610595703125, 238.4427032470703, 0.0, 0.0, 1.0]
+R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
+P: [388.9610595703125, 0.0, 319.34539794921875, 0.0, 0.0, 388.9610595703125, 238.4427032470703, 0.0, 0.0, 0.0, 1.0, 0.0]
+binning_x: 0
+binning_y: 0
+roi:
+  x_offset: 0
+  y_offset: 0
+  height: 0
+  width: 0
+  do_rectify: False

get_obj_dist/src/object_to_distance.py

@@ -6,11 +6,11 @@ import cv2
 import rospy
 import message_filters
 
-from visualization_msgs.msg import Marker
+from visualization_msgs.msg import Marker, MarkerArray
 from sensor_msgs.msg import Image, PointCloud2
 from cv_bridge import CvBridge, CvBridgeError
 from human_detection.msg import box_list
-from depth_filter import *
+from utils import *
 
 text_position = (10, 50)
 font = cv2.FONT_HERSHEY_SIMPLEX
@@ -24,12 +24,13 @@ class obj_dist:
     def __init__(self):
         # Declare functional variables
         self.bridge = CvBridge()
+        self.marker_array = MarkerArray()
         self.moving_average = [3] * size_of_moving_avg
+        self.set_marker_array(5, 'camera_color_optical_frame', 'package://get_obj_dist/human_model.STL')
 
         # Initializing ROS Topics
         self.dist_pub = rospy.Publisher('/obj_to_dist/human_distance', Image, queue_size=10)
-        self.render_pub = rospy.Publisher('/obj_to_dist/render_human', Marker, queue_size=10)
-        # self.roi_pub = rospy.Publisher('/obj_to_dist/disappera', Image, queue_size=10)
+        self.render_pub = rospy.Publisher('/obj_to_dist/show_people_marker_array', MarkerArray, queue_size=10)
 
         self.bbx_sub = message_filters.Subscriber('/human_detected_image/bounding_box', box_list)
         self.human_image_sub = message_filters.Subscriber('/human_detected_image/image', Image)
@@ -46,93 +47,72 @@ class obj_dist:
             cv_depth = self.bridge.imgmsg_to_cv2(depth, 'passthrough')
             cv_image = self.bridge.imgmsg_to_cv2(image, 'bgr8')
 
-            for box in bbx.people_list:
-                cood = self.get_human_distance(cv_depth, cv_image, box)
-                self.render_human(cood)
+            for i in range(0, 5):
+                if i <= bbx.length - 1:
+                    cood = self.get_human_distance(cv_depth, cv_image, bbx.people_list[i], i)
+                    self.set_model_coordinates(cood, i)
+                else:
+                    self.set_model_coordinates((-1, -1, -1), i)
+        self.render_pub.publish(self.marker_array)
 
-    def get_human_distance(self, cv_depth, cv_image, box):
+    def get_human_distance(self, cv_depth, cv_image, box, person_id):
         roi_depth = cv_depth[box.ymin:box.ymax, box.xmin:box.xmax]
         filtered_depth, _size = filter_background(roi_depth)
-        print(filtered_depth.data)
         if _size:
             self.moving_average.pop()
             self.moving_average.insert(0, filtered_depth.sum() / _size / 1000.0)
 
-        current_avg = sum(self.moving_average) / size_of_moving_avg
-
-        x_meter = self.get_x_in_meters(box.xmin, box.xmax, current_avg)
-        cv2.putText(cv_image, '{} meters'.format(round(current_avg, 2)),
-                    (box.xmin, box.ymax - 100),
-                    font,
-                    font_scale,
-                    font_color,
-                    line_type)
-
-        # self.roi_pub.publish(self.bridge.cv2_to_imgmsg(filtered_depth.data))
-        # self.roi_pub.publish(self.bridge.cv2_to_imgmsg(roi_depth))
-        self.dist_pub.publish(self.bridge.cv2_to_imgmsg(cv_image))
-
-        return x_meter, 0, current_avg
-
-    def render_human(self, cood):
-        marker = Marker()
-        marker.header.frame_id = 'camera_color_optical_frame'
-        marker.header.stamp = rospy.Time.now()
-
-        marker.id = 1
-        marker.action = Marker.ADD
-        marker.lifetime = rospy.Duration()
-        # marker.type = Marker.SPHERE
-
-        marker.type = Marker.MESH_RESOURCE
-        marker.mesh_resource = 'package://get_obj_dist/human_model.STL'
-
-        marker.pose.position.x = cood[0]
-        marker.pose.position.y = cood[1]
-        marker.pose.position.z = cood[2]
-
-        marker.pose.orientation.x = 0.0
-        marker.pose.orientation.y = 0.0
-        marker.pose.orientation.z = 0.0
-
-        marker.color.r = 1.0
-        marker.color.g = 1.0
-        marker.color.b = 1.0
-        marker.color.a = 1.0
-
-        marker.scale.x = 1.0
-        marker.scale.y = 1.0
-        marker.scale.z = 1.0
-
-        # marker.id = 0
-        # marker.action = Marker.ADD
-        # marker.lifetime = rospy.Duration()
-        # marker.type = Marker.SPHERE
-        #
-        # marker.pose.position.x = cood[0]
-        # marker.pose.position.y = cood[1]
-        # marker.pose.position.z = cood[2]
-        #
-        # marker.scale.x = 0.5
-        # marker.scale.y = 0.5
-        # marker.scale.z = 0.5
-        #
-        # marker.pose.orientation.x = 0
-        # marker.pose.orientation.y = 0
-        # marker.pose.orientation.z = 0
-        #
-        # marker.color.a = 1.0
-        # marker.color.r = 0.0
-        # marker.color.g = 1.0
-        # marker.color.b = 0.0
-
-        self.render_pub.publish(marker)
-
-    def get_x_in_meters(self, xmin, xmax, z_i):
-        # Tune z_c to get better value lol.
-        z_c = 100
-        ret_val = (z_i * (xmax + xmin - 600.0)) / (2 * z_c)
-        return ret_val
+            current_avg = sum(self.moving_average) / size_of_moving_avg
+
+            x_meter = get_x_in_meters(box.xmin, box.xmax, current_avg)
+            cv2.putText(cv_image, '{} meters / Person {}'.format(round(current_avg, 2), person_id),
+                        (box.xmin, box.ymax - 100),
+                        font,
+                        font_scale,
+                        font_color,
+                        line_type)
+            self.dist_pub.publish(self.bridge.cv2_to_imgmsg(cv_image))
+
+            # TODO: Add dynamic find floor
+            return x_meter, 0.3, current_avg
+        else:
+            return -1, -1, -1
+
+    def set_marker_array(self, size, frame_id, package):
+        # self.marker_array.markers = [Marker()] * size
+        for i in range(0, size):
+            self.marker_array.markers.append(Marker())
+            self.marker_array.markers[i].header.frame_id = frame_id
+            self.marker_array.markers[i].lifetime = rospy.Duration()
+            self.marker_array.markers[i].id = i
+            self.marker_array.markers[i].type = Marker.MESH_RESOURCE
+            self.marker_array.markers[i].mesh_resource = package
+
+    def set_model_coordinates(self, cood, person_id):
+        self.marker_array.markers[person_id].header.stamp = rospy.Time.now()
+        if cood == (-1, -1, -1):
+            self.marker_array.markers[person_id].action = Marker.DELETE
+        else:
+            self.marker_array.markers[person_id].header.stamp = rospy.Time.now()
+            self.marker_array.markers[person_id].action = Marker.ADD
+
+            self.marker_array.markers[person_id].color.r = 1.0
+            self.marker_array.markers[person_id].color.g = 1.0
+            self.marker_array.markers[person_id].color.b = 1.0
+            self.marker_array.markers[person_id].color.a = 1.0
+
+            # TODO: Change the scale of the DAE model so these numbers make more sense
+            self.marker_array.markers[person_id].scale.x = 0.0005
+            self.marker_array.markers[person_id].scale.y = 0.0009
+            self.marker_array.markers[person_id].scale.z = 0.0008
+
+            self.marker_array.markers[person_id].pose.orientation.x = 1.0
+            self.marker_array.markers[person_id].pose.orientation.y = 0.0
+            self.marker_array.markers[person_id].pose.orientation.z = 0.0
+
+            self.marker_array.markers[person_id].pose.position.x = cood[0] - 0.4
+            self.marker_array.markers[person_id].pose.position.y = cood[1]
+            self.marker_array.markers[person_id].pose.position.z = cood[2]
 
 
 def main(args):

get_obj_dist/src/depth_filter.py → get_obj_dist/src/utils.py

@@ -13,3 +13,11 @@ def filter_background(roi):
         return ret_val, _dict[False]
     else:
         return ret_val, 0
+
+
+def get_x_in_meters(xmin, xmax, z_i):
+    # Tune z_c to get better value lol.
+    # 500 is literally randomly chosen lol
+    z_c = 500
+    ret_val = (z_i * (xmax + xmin - 600.0)) / (2 * z_c)
+    return ret_val