Publish 2d and 3d detections

get_obj_dist/launch/node.launch

@@ -2,7 +2,7 @@
 <!-- -->
 <launch>
 
-  <arg name="node_name"        default="person_bbox_to_distance"/>
+  <arg name="node_name"        default="human_bbox_to_distance"/>
   <arg name="camera_in_topic"  default="/camera/color/image_raw"/>
 
   <arg name="bbox_in_topic"      default="/human_detection/bounding_box"/>

get_obj_dist/src/object_to_distance.py

@@ -11,6 +11,8 @@ from cv_bridge import CvBridge, CvBridgeError
 from human_detection.msg import box_list
 from utils import *
 
+from spencer_tracking_msgs.msg import DetectedPersons, DetectedPerson
+
 text_position = (10, 50)
 font = cv2.FONT_HERSHEY_SIMPLEX
 font_scale = 0.75
@@ -30,8 +32,9 @@ class obj_dist:
         self.set_marker_array(marker_size, 'camera_color_optical_frame', 'package://get_obj_dist/human_model.STL')
 
         # Initiale publishers
-        self.dist_pub = rospy.Publisher('~detection_distance_image_out', Image, queue_size=10)
-        self.render_pub = rospy.Publisher('~people_marker_array_out', MarkerArray, queue_size=10)
+        self.image_pub = rospy.Publisher('~detection_distance_image_out', Image, queue_size=1)
+        self.marker_pub = rospy.Publisher('~people_marker_array_out', MarkerArray, queue_size=1)
+        self.detections_pub = rospy.Publisher('~detected_persons', DetectedPersons, queue_size=1)
 
         # Initialize subscribers
         self.bbx_sub = message_filters.Subscriber('~bounding_box_in', box_list)
@@ -46,6 +49,10 @@ class obj_dist:
         ts.registerCallback(self.callback)
 
     def callback(self, bbx, image, depth):
+
+        detections = DetectedPersons()
+        detections.header = depth.header
+
         #print('working lol')
         if bbx.length:
             cv_depth = self.bridge.imgmsg_to_cv2(depth, 'passthrough')
@@ -55,9 +62,22 @@ class obj_dist:
                 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)
+
+                    det = DetectedPerson()
+                    det.confidence = 0 #TODO: subscribe to detection2d_array which has confidence (bbox doesnt)
+                    det.pose.pose.position.x = cood[0]
+                    det.pose.pose.position.y = cood[1]
+                    det.pose.pose.position.z = cood[2]
+                    det.pose.pose.orientation.w=1.0
+                    det.modality = "rgbd"
+
+                    detections.detections.append(det)
+
                 else:
                     self.set_model_coordinates((-1, -1, -1), i)
-        self.render_pub.publish(self.marker_array)
+
+        self.marker_pub.publish(self.marker_array)
+        self.detections_pub.publish(detections)
 
     def get_human_distance(self, cv_depth, cv_image, box, person_id):
         roi_depth = cv_depth[box.ymin:box.ymax, box.xmin:box.xmax]
@@ -76,10 +96,10 @@ class obj_dist:
                         font_scale,
                         font_color,
                         line_type)
-            self.dist_pub.publish(self.bridge.cv2_to_imgmsg(cv_image))
+            self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image))
 
             # TODO: Add dynamic find floor
-            return x_meter, 0.3, current_avg
+            return x_meter, 0.0, current_avg
         else:
             return -1, -1, -1
 

human_detection/CMakeLists.txt

@@ -11,6 +11,7 @@ find_package(catkin REQUIRED COMPONENTS
   roslib
   rospy
   std_msgs
+  vision_msgs
   message_generation
 )
 
@@ -107,7 +108,7 @@ catkin_package(
 #  INCLUDE_DIRS include
 #  LIBRARIES human_detection
    CATKIN_DEPENDS roslib rospy std_msgs message_runtime
-			
+
 #  DEPENDS system_lib
 )
 

human_detection/package.xml

@@ -52,7 +52,7 @@
   <build_depend>roslib</build_depend>
   <build_depend>rospy</build_depend>
   <build_depend>std_msgs</build_depend>
-  <build_depend>message_generation</build_depend>  
+  <build_depend>message_generation</build_depend>
   <build_export_depend>roslib</build_export_depend>
   <build_export_depend>rospy</build_export_depend>
   <build_export_depend>std_msgs</build_export_depend>
@@ -61,6 +61,8 @@
   <exec_depend>rospy</exec_depend>
   <exec_depend>std_msgs</exec_depend>
 
+  <depend>vision_msgs</depend>
+
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

human_detection/rviz/test.rviz

@@ -6,6 +6,8 @@ Panels:
       Expanded:
         - /Global Options1
         - /Status1
+        - /DetectionDistanceImage1
+        - /MarkerArray1
       Splitter Ratio: 0.5
     Tree Height: 617
   - Class: rviz/Selection
@@ -128,7 +130,7 @@ Visualization Manager:
       Value: true
     - Class: rviz/Image
       Enabled: true
-      Image Topic: /person_bbox_to_distance/detection_distance_image
+      Image Topic: /human_bbox_to_distance/detection_distance_image
       Max Value: 1
       Median window: 5
       Min Value: 0
@@ -140,7 +142,7 @@ Visualization Manager:
       Value: true
     - Class: rviz/MarkerArray
       Enabled: true
-      Marker Topic: /person_bbox_to_distance/people_marker_array
+      Marker Topic: /human_bbox_to_distance/people_marker_array
       Name: MarkerArray
       Namespaces:
         "": true

human_detection/src/human_detector.py

@@ -20,6 +20,8 @@ from human_detection.msg import bounding_box, box_list
 from sensor_msgs.msg import Image
 from cv_bridge import CvBridge, CvBridgeError
 
+from vision_msgs.msg import Detection2DArray, Detection2D, ObjectHypothesisWithPose
+
 # Import utilites
 from object_detection.utils import label_map_util
 from object_detection.utils import visualization_utils as vis_util
@@ -72,8 +74,9 @@ class human_detector:
         self.bridge = CvBridge()
         self.image_sub = rospy.Subscriber("~camera/color/image_raw", Image, self.callback)
 
-        self.image_pub = rospy.Publisher("~detection_image", Image, queue_size=10)
-        self.bbx_pub = rospy.Publisher("~bounding_box", box_list, queue_size=10)
+        self.image_pub = rospy.Publisher("~detection_image", Image, queue_size=1)
+        self.bbx_pub = rospy.Publisher("~bounding_box", box_list, queue_size=1)
+        self.detection2d_array_pub = rospy.Publisher("~detection2d_array", Detection2DArray, queue_size=1)
 
     def callback(self, data):
         t0 = time.time()
@@ -133,17 +136,34 @@ class human_detector:
         list_length = 0
         bbx_list = box_list()
         bbx_list.header.stamp = data.header.stamp
+
+        det2d_array = Detection2DArray()
+        det2d_array.header.stamp = data.header.stamp
+
         spotted = False
 
         for score in scores[0]:
             if score > 0.6:
                 list_length += 1
-                coordinates = bounding_box()
-                coordinates.ymin = int(boxes[0, counter, 0] * screen_height)
-                coordinates.xmin = int(boxes[0, counter, 1] * screen_width)
-                coordinates.ymax = int(boxes[0, counter, 2] * screen_height)
-                coordinates.xmax = int(boxes[0, counter, 3] * screen_width)
-                bbx_list.people_list.append(coordinates)
+                bbox = bounding_box()
+                bbox.ymin = int(boxes[0, counter, 0] * screen_height)
+                bbox.xmin = int(boxes[0, counter, 1] * screen_width)
+                bbox.ymax = int(boxes[0, counter, 2] * screen_height)
+                bbox.xmax = int(boxes[0, counter, 3] * screen_width)
+                bbx_list.people_list.append(bbox)
+
+                det2d = Detection2D()
+                det2d.header = det2d_array.header
+                obj = ObjectHypothesisWithPose()
+                obj.score = score
+                det2d.results.append(obj)
+                det2d.bbox.size_x = bbox.xmax - bbox.xmin
+                det2d.bbox.size_y = bbox.ymax - bbox.ymin
+                det2d.bbox.center.x = bbox.xmin + det2d.bbox.size_x/2.0
+                det2d.bbox.center.y = bbox.ymin + det2d.bbox.size_y/2.0
+                det2d.bbox.center.theta = 0
+
+                det2d_array.detections.append(det2d)
 
             counter += 1
 
@@ -151,6 +171,8 @@ class human_detector:
         bbx_list.length = list_length
         self.bbx_pub.publish(bbx_list)
 
+        self.detection2d_array_pub.publish(det2d_array)
+
         try:
             cvt_msg = self.bridge.cv2_to_imgmsg(cv_image, "bgr8")
             cvt_msg.header.stamp = data.header.stamp