Merge branch 'before_task' of https://github.com/butia-bots/butia_vision

 into feat/i2w_pose2D_robot_impl

butia_image2world/scripts/butia_image2world/image2world/image2world.py

@@ -307,18 +307,47 @@ def __instanceSegmentationDescriptionProcessing(self, source_data, description2d
     def __poseDescriptionProcessing(self, data, description2d : Description2D, header):
         description3d : Description3D = self.__detectionDescriptionProcessing(data, description2d ,header)
 
-        kpt : KeyPoint2D
-        for kpt in description2d.pose:
-            kpt3D = KeyPoint3D()
-            kpt3D.id = kpt.id
-            kpt3D.score = kpt.score
-            kpt3D.x = kpt.x
-            kpt3D.y = kpt.y
-            kpt3D.z = 0
-            description3d.pose.append(kpt3D)
+        if 'image_depth' in data and 'camera_info' in data:
+            image_depth = data['image_depth']
+            camera_info = data['camera_info']
 
-        return description3d
+            self.__mountLutTable(camera_info)
+
+            h, w = image_depth.shape
+
+            kpt : KeyPoint2D
+            for kpt in description2d.pose:
+                kpt3D = KeyPoint3D()
+                kpt3D.id = kpt.id
+
+                u = int(kpt.x)
+                v = int(kpt.y)
 
+                if u >= w or u < 0 or v >= h or v < 0:
+                    kpt3D.score = 0
+
+                else:
+                    depth = image_depth[v, u]
+
+                    if depth <= 0:
+                        kpt3D.score = 0
+
+                    else:
+                        kpt3D.score = kpt.score
+
+                        depth /= 1000.
+
+                        vertex_3d = np.zeros(3)
+                        vertex_3d[:2] = self.lut_table[v, u, :]*depth
+                        vertex_3d[2] = depth
+
+                        kpt3D.x = vertex_3d[0]
+                        kpt3D.y = vertex_3d[1]
+                        kpt3D.z = vertex_3d[2]
+
+                description3d.pose.append(kpt3D)
+
+            return description3d
 
     def __createDescription3D(self, source_data, description2d : Description2D, header):
         if description2d.type in self.DESCRIPTION_PROCESSING_ALGORITHMS:
@@ -419,6 +448,29 @@ def publishMarkers(self, descriptions3d):
             marker.text = '{} ({:.2f})'.format(name, det.score)
             marker.lifetime = rospy.Time.from_sec(2)
             markers.markers.append(marker)
+
+            for idx, kpt3D in enumerate(det.pose):
+                if kpt3D.score > 0:
+                    marker = Marker()
+                    marker.header = det.poses_header
+                    marker.type = Marker.SPHERE
+                    marker.id = idx
+                    marker.color.r = color[1]
+                    marker.color.g = color[2]
+                    marker.color.b = color[0]
+                    marker.color.a = 1
+                    marker.scale.x = 0.05
+                    marker.scale.y = 0.05
+                    marker.scale.z = 0.05
+                    marker.pose.position.x = kpt3D.x
+                    marker.pose.position.y = kpt3D.y
+                    marker.pose.position.z = kpt3D.z
+                    marker.pose.orientation.x = 0.0
+                    marker.pose.orientation.y = 0.0
+                    marker.pose.orientation.z = 0.0
+                    marker.pose.orientation.w = 1.0
+                    marker.lifetime = rospy.Time.from_sec(2)
+                    markers.markers.append(marker)
 
         self.marker_publisher.publish(markers)
 

butia_people_tracking/config/ros.yaml

@@ -4,7 +4,6 @@ services:
     stop_tracking: "/butia_vision/pt/stop"
 
 topics:
-  butia_recognition:
-    people_detection: "/butia_vision/br/people_detection"
   people_tracking:
+    people_detection: "/butia_vision/br/recognitions2D"
     people_tracking: "/butia_vision/pt/people_tracking"

butia_people_tracking/scripts/people_tracking_node.py

@@ -115,11 +115,11 @@ def stopTracking(req):
 
     queue_size = rospy.get_param("/people_tracking/queue/size", 1)
 
-    param_start_service = rospy.get_param("/services/people_tracking/start_tracking", "/butia_vision/pt/start")
-    param_stop_service = rospy.get_param("/services/people_tracking/stop_tracking", "/butia_vision/pt/stop")
+    param_start_service = rospy.get_param("people_tracking/services/people_tracking/start_tracking", "/butia_vision/pt/start")
+    param_stop_service = rospy.get_param("people_tracking/services/people_tracking/stop_tracking", "/butia_vision/pt/stop")
 
-    param_people_detection_topic = rospy.get_param("/topics/butia_recognition/people_detection", "/butia_vision/br/people_detection")
-    param_people_tracking_topic = rospy.get_param("/topics/people_tracking/people_tracking", "/butia_vision/pt/people_tracking")
+    param_people_detection_topic = rospy.get_param("people_tracking/topics/people_tracking/people_detection", "/butia_vision/br/recognitions2D")
+    param_people_tracking_topic = rospy.get_param("people_tracking/topics/people_tracking/people_tracking", "/butia_vision/pt/people_tracking")
 
     tracker_publisher = rospy.Publisher(param_people_tracking_topic, Recognitions2D, queue_size = queue_size)
     view_publisher = rospy.Publisher('/butia_vision/pt/people_tracking_view', Image, queue_size = queue_size)

butia_recognition/config/yolo_tracker_bag_recognition.yaml

@@ -0,0 +1,53 @@
+debug_kpt_threshold: 0.5
+
+model_file: yolov8s-pose.pt
+
+subscribers:
+
+  queue_size: 1
+  exact_time: true
+  slop: 0.1
+
+  image_rgb: /butia_vision/bvb/image_rgb
+
+  image_depth: /butia_vision/bvb/image_depth
+
+  camera_info: /butia_vision/bvb/camera_info
+
+
+publishers:
+
+  recognition:
+    topic: /butia_vision/br/recognitions2D
+    queue_size: 1
+
+  debug:
+    topic: /butia_vision/br/debug2
+    queue_size: 1
+
+  tracking: 
+    topic: /butia_vision/pt/tracking2D
+    queue_size: 1
+
+servers:
+  start:
+    service: /butia_vision/br/yolo_tracking/start
+
+  stop:
+    service: /butia_vision/br/yolo_tracking/stop
+
+services:
+  tracking:
+    start: /butia_vision/yolo_pt/start
+    stop: /butia_vision/yolo_pt/stop
+
+tracking:
+  start_on_init: false
+  use_boxmot: false
+  model_file: osnet_x0_25_msmt17.pt
+  thresholds:
+    reid_threshold: 0.3
+    det_threshold: 0.5
+    iou_threshold: 0.15
+    max_age: 10
+    max_time: 10

butia_recognition/config/yolov5_people_detection.yaml

@@ -24,7 +24,7 @@ subscribers:
 publishers:
 
   people_detection:
-    topic: /butia_vision/br/face_recognition
+    topic: /butia_vision/br/recognition2D
     queue_size: 1
 
 servers:

butia_recognition/config/yolov8_bag_recognition.yaml

@@ -0,0 +1,42 @@
+threshold: 0.5
+classes_by_category:
+    bag: ['bag']
+all_classes: 
+    - bag
+
+max_sizes:
+    - [0.5, 2.5, 0.5]
+
+model_file: yolov8_bag.pt
+
+subscribers:
+
+  queue_size: 1
+  exact_time: false
+  slop: 0.2
+
+  image_rgb: /butia_vision/bvb/image_rgb
+  camera_info: /butia_vision/bvb/camera_info
+  image_depth: /butia_vision/bvb/image_depth
+  # points: /butia_vision/bvb/points
+
+  # image_rgb: /camera/color/image_raw
+  # camera_info: /camera/color/camera_info
+  # image_depth: /camera/aligned_depth_to_color/image_raw
+
+publishers:
+
+  object_recognition:
+    topic: /butia_vision/br/object_recognition
+    queue_size: 1
+
+servers:
+
+  list_classes:
+    service: /butia_vision/br/object_recognition/list_classes
+
+  start:
+    service: /butia_vision/br/object_recognition/start
+
+  stop:
+    service: /butia_vision/br/object_recognition/stop

butia_recognition/config/yolov8_object_recognition.yaml

@@ -0,0 +1,47 @@
+threshold: 0.5
+classes_by_category:
+    fruits: ['Apple', 'Banana', 'Grape', 'Orange', 'Pineapple', 'Watermelon']
+all_classes: 
+    - Apple
+    - Banana
+    - Grape
+    - Orange
+    - Pineapple
+    - Watermelon
+
+max_sizes:
+    - [0.5, 2.5, 0.5]
+
+model_file: yolov8_lab_objects.pt
+
+subscribers:
+
+  queue_size: 1
+  exact_time: false
+  slop: 0.2
+
+  # image_rgb: /butia_vision/bvb/image_rgb
+  # camera_info: /butia_vision/bvb/camera_info
+  # image_depth: /butia_vision/bvb/image_depth
+  #points: /butia_vision/bvb/points
+
+  image_rgb: /camera/color/image_raw
+  camera_info: /camera/color/camera_info
+  image_depth: /camera/aligned_depth_to_color/image_raw
+
+publishers:
+
+  object_recognition:
+    topic: /butia_vision/br/object_recognition
+    queue_size: 1
+
+servers:
+
+  list_classes:
+    service: /butia_vision/br/object_recognition/list_classes
+
+  start:
+    service: /butia_vision/br/object_recognition/start
+
+  stop:
+    service: /butia_vision/br/object_recognition/stop

butia_recognition/config/yolov8_object_recognition_backup.yaml

@@ -0,0 +1,97 @@
+threshold: 0.5
+classes_by_category:
+    drink: ['guarana', 'Guarana', 'IceTea', 'tonic', Tonic', 'Coke', 'Beer can', 'CoconutWater', 'Water', Soda can']
+    snack: ['corn flakes', 'CornFlakes', 'pringles', 'Pringles', 'Popcorn', 'Cookies', 'Cracker', 'Chocolate Milk', 'TunaCan']
+    fruits: ['apple', 'Apple', 'kiwi', 'Kiwi', 'orange', 'Banana', 'Orange']
+    cutlelary: ['bowl', 'Bowl', 'fork', 'knife', 'plate', 'Plate', 'Cleaner', 'Napkin', 'Cloth', 'EnglishSauce', 'TomatoSauce']
+    misc: ['sponge', 'Sponge', 'Bag', 'Alcohol Bottle', 'Soap', 'Mug', 'Carrot', 'Yeast', 'Coconut Milk', 'Chocolate Powder', 'Ketchup', 'Mayonnaise', 'Mustard', 'Toothpaste', 'Gelatine']
+all_classes: 
+    - Alcohol Bottle
+    - Apple
+    - Bag
+    - Banana
+    - Beer can
+    - Bowl
+    - Carrot
+    - Chocolate Milk
+    - Chocolate Powder
+    - Cleaner
+    - Cloth
+    - Coconut Milk
+    - CoconutWater
+    - Coke
+    - Cookies
+    - CornFlakes
+    - Cracker
+    - EnglishSauce
+    - Gelatine
+    - Guarana
+    - IceTea
+    - Ketchup
+    - Kiwi
+    - Mayonnaise
+    - Mug
+    - Mustard
+    - Napkin
+    - Orange
+    - Plate
+    - Popcorn
+    - Pringles
+    - Soap
+    - Soda can
+    - Sponge
+    - TomatoSauce
+    - Tonic
+    - Toothpaste
+    - TunaCan
+    - Water
+    - Yeast
+    - apple
+    - bowl
+    - corn flakes
+    - fork
+    - guarana
+    - kiwi
+    - knife
+    - orange
+    - plate
+    - pringles
+    - sponge
+    - tonic
+
+max_sizes:
+    - [0.5, 2.5, 0.5]
+
+model_file: yolov8_lab_objects.pt
+
+subscribers:
+
+  queue_size: 1
+  exact_time: false
+  slop: 0.2
+
+  # image_rgb: /butia_vision/bvb/image_rgb
+  # camera_info: /butia_vision/bvb/camera_info
+  # image_depth: /butia_vision/bvb/image_depth
+  #points: /butia_vision/bvb/points
+
+  image_rgb: /camera/color/image_raw
+  camera_info: /camera/color/camera_info
+  image_depth: /camera/aligned_depth_to_color/image_raw
+
+publishers:
+
+  object_recognition:
+    topic: /butia_vision/br/object_recognition
+    queue_size: 1
+
+servers:
+
+  list_classes:
+    service: /butia_vision/br/object_recognition/list_classes
+
+  start:
+    service: /butia_vision/br/object_recognition/start
+
+  stop:
+    service: /butia_vision/br/object_recognition/stop

butia_recognition/launch/yolov8_bag_recognition.launch

@@ -0,0 +1,21 @@
+<launch>
+  <arg name="machine" default="localhost"/>
+  <arg name="use_machine" default="true"/>
+  <arg name="output" default="screen"/>
+  <arg name="node_name" default="butia_bag_recognition"/>
+  <arg name="config_file" default="yolov8_bag_recognition.yaml"/>
+
+  <machine name="localhost" address="localhost" if="$(arg use_machine)"/>
+
+  <node pkg="butia_recognition" type="yolov8_recognition.py" output="$(arg output)" name="$(arg node_name)" machine="$(arg machine)">
+    <rosparam command="load" file="$(find butia_recognition)/config/$(arg config_file)"/>
+  </node>
+
+  <node pkg="butia_image2world" type="image2world.py" output="$(arg output)" name="image2world_object_recognition_node" machine="$(arg machine)">
+    <remap from="sub/recognitions2d"  to="/butia_vision/br/object_recognition"/>
+    <remap from="pub/recognitions3d"  to="/butia_vision/br/object_recognition3d"/>
+    <remap from="pub/markers" to="/butia_vision/br/markers"/>
+    <rosparam param="color">[255,0,0]</rosparam>
+  </node>
+
+</launch>