adding vision_msgs, issue on labels because are missing in msgs

edgeimpulse_ros/image_classification.py

@@ -24,7 +24,9 @@
 
 from sensor_msgs.msg import Image
 
-#from vision_msgs.msg import BoundingBox2DArray
+from vision_msgs.msg import Detection2DArray
+from vision_msgs.msg import Detection2D
+from vision_msgs.msg import ObjectHypothesisWithPose
 #from vision_msgs.msg import VisionInfo
 
 import os
@@ -35,7 +37,9 @@
 
 
 
+
 class EI_Image_node(Node):
+
     def __init__(self):
 
         self.occupied = False
@@ -45,21 +49,27 @@ def __init__(self):
         super().__init__('ei_image_classifier_node')
         self.init_parameters()
         self.ei_classifier = self.EI_Classifier(self.modelfile, self.get_logger())
-        #self.publisher = self.create_publisher(BoundingBox2DArray,'/edge_impulse/detection',1)
 
         self.timer_parameter = self.create_timer(2,self.parameters_callback)
 
         self.image_publisher = self.create_publisher(Image,'/detection/output/image',1)
+        self.results_publisher = self.create_publisher(Detection2DArray,'/detection/output/results',1)
+
         self.timer_classify = self.create_timer(0.01,self.classify_callback)
         self.timer_classify.cancel()
         self.subscription = self.create_subscription(Image,'/detection/input/image',self.listener_callback,1)
         self.subscription 
 
-
+
+
+
     def init_parameters(self):
         self.declare_parameter('model.filepath','')
         self.modelfile= self.get_parameter('model.filepath').get_parameter_value().string_value
 
+        self.declare_parameter('frame_id','base_link')
+        self.frame_id= self.get_parameter('frame_id').get_parameter_value().string_value
+
         self.declare_parameter('show.overlay', True)
         self.show_overlay = self.get_parameter('show.overlay').get_parameter_value().bool_value
 
@@ -72,9 +82,6 @@ def init_parameters(self):
 
 
 
-
-
-
     def parameters_callback(self):
         self.show_labels_on_image = self.get_parameter('show.labels').get_parameter_value().bool_value
         self.show_extra_classification_info = self.get_parameter('show.classification_info').get_parameter_value().bool_value
@@ -91,13 +98,25 @@ def listener_callback(self, msg):
                 self.img = current_frame
                 self.timer_classify.reset()
 
+
+
+
+
     def classify_callback(self):
         self.occupied = True
+
+        # vision msgs
+        results_msg = Detection2DArray()
+        time_now = self.get_clock().now().to_msg()
+        results_msg.header.stamp = time_now
+        results_msg.header.frame_id = self.frame_id
+
 
         # classify
         features, cropped, res = self.ei_classifier.classify(self.img)
 
-        #prepare output
+
+        #p repare output
         if "classification" in res["result"].keys():
             if self.show_extra_classification_info:
                 self.get_logger().info('Result (%d ms.) ' % (res['timing']['dsp'] + res['timing']['classification']), end='')
@@ -112,6 +131,28 @@ def classify_callback(self):
                 self.get_logger().info('Found %d bounding boxes (%d ms.)' % (len(res["result"]["bounding_boxes"]), res['timing']['dsp'] + res['timing']['classification']))
 
             for bb in res["result"]["bounding_boxes"]:
+                result_msg = Detection2D()
+                result_msg.header.stamp = time_now
+                result_msg.header.frame_id = self.frame_id
+
+                # object with hypthothesis
+                obj_hyp = ObjectHypothesisWithPose()
+                #obj_hyp.id = 
+                obj_hyp.score = bb['value']
+                obj_hyp.pose.pose.position.x = float(bb['x'])
+                obj_hyp.pose.pose.position.y = float(bb['y'])
+                result_msg.results.append(obj_hyp)
+
+                # bounding box
+                result_msg.bbox.center.x = float(bb['x'])
+                result_msg.bbox.center.y = float(bb['y'])
+                result_msg.bbox.size_x = float(bb['width'])
+                result_msg.bbox.size_y = float(bb['height'])
+
+
+                results_msg.detections.append(result_msg)
+
+                # image
                 if self.show_extra_classification_info:
                     self.get_logger().info('%s (%.2f): x=%d y=%d w=%d h=%d' % (bb['label'], bb['value'], bb['x'], bb['y'], bb['width'], bb['height']))
                 if self.show_overlay:
@@ -124,6 +165,7 @@ def classify_callback(self):
 
         # publish message
         self.image_publisher.publish(self.cv_bridge.cv2_to_imgmsg(cropped,"bgr8"))
+        self.results_publisher.publish(results_msg)
 
         self.occupied= False
         self.timer_classify.cancel()
@@ -166,6 +208,8 @@ def classify(self, img):
                 self.logger.error('Error on classification')
 
 
+
+
 def main():
     rclpy.init()
     node = EI_Image_node()
@@ -174,7 +218,6 @@ def main():
     node.destroy_node()
     rclpy.shutdown()
 
-
 if __name__ == "__main__":
    main()
 

package.xml

@@ -14,7 +14,7 @@
   <test_depend>ament_pep257</test_depend>
   <test_depend>python3-pytest</test_depend>
 
-  <!--<exec_depend>vision_msgs</exec_depend>-->
+  <exec_depend>vision_msgs</exec_depend>
   <exec_depend>sensor_msgs</exec_depend>
   <exec_depend>ros2launch</exec_depend>
 

setup.py

@@ -4,7 +4,7 @@
 submodules = 'edgeimpulse_ros/submodules'
 setup(
     name=package_name,
-    version='0.0.1',
+    version='0.0.2',
     packages=[package_name, submodules],
     data_files=[
         ('share/ament_index/resource_index/packages',