Ros2 nodes for 3D Detection and 2D/3D tracking

.gitignore

@@ -71,3 +71,9 @@ temp/
 projects/opendr_ws/.catkin_workspace
 projects/opendr_ws/devel/
 projects/python/control/eagerx/eagerx_ws/
+
+projects/opendr_ws_2/build
+projects/opendr_ws_2/install
+projects/opendr_ws_2/log
+projects/opendr_ws_2/MOT
+projects/opendr_ws_2/KITTI

projects/opendr_ws_2/src/opendr_bridge/opendr_bridge/bridge.py

@@ -13,17 +13,25 @@
 # limitations under the License.
 
 import numpy as np
-from opendr.engine.data import Image, Timeseries
-from opendr.engine.target import Pose, BoundingBox, BoundingBoxList, Category, \
-    TrackingAnnotation
-
+from opendr.engine.data import Image, PointCloud, Timeseries
+from opendr.engine.target import (
+    Pose, BoundingBox, BoundingBoxList, Category,
+    BoundingBox3D, BoundingBox3DList, TrackingAnnotation
+)
 from cv_bridge import CvBridge
 from std_msgs.msg import String, ColorRGBA, Header
-from sensor_msgs.msg import Image as ImageMsg
-from vision_msgs.msg import Detection2DArray, Detection2D, BoundingBox2D, ObjectHypothesis, ObjectHypothesisWithPose, \
-    Classification2D
+from sensor_msgs.msg import Image as ImageMsg, PointCloud as PointCloudMsg, ChannelFloat32 as ChannelFloat32Msg
+from vision_msgs.msg import (
+    Detection2DArray, Detection2D, BoundingBox2D, ObjectHypothesisWithPose,
+    Detection3D, Detection3DArray, BoundingBox3D as BoundingBox3DMsg,
+    Classification2D, ObjectHypothesis
+)
 from shape_msgs.msg import Mesh, MeshTriangle
-from geometry_msgs.msg import Point, Pose2D
+from geometry_msgs.msg import (
+    Pose2D, Point32 as Point32Msg,
+    Quaternion as QuaternionMsg, Pose as Pose3D,
+    Point
+)
 from opendr_interface.msg import OpenDRPose2D, OpenDRPose2DKeypoint, OpenDRPose3D, OpenDRPose3DKeypoint
 
 
@@ -291,6 +299,130 @@ def to_ros_face_id(self, category):
         result.data = category.description
         return result
 
+    def from_ros_point_cloud(self, point_cloud: PointCloudMsg):
+        """
+        Converts a ROS PointCloud message into an OpenDR PointCloud
+        :param point_cloud: ROS PointCloud to be converted
+        :type point_cloud: sensor_msgs.msg.PointCloud
+        :return: OpenDR PointCloud
+        :rtype: engine.data.PointCloud
+        """
+
+        points = np.empty([len(point_cloud.points), 3 + len(point_cloud.channels)], dtype=np.float32)
+
+        for i in range(len(point_cloud.points)):
+            point = point_cloud.points[i]
+            x, y, z = point.x, point.y, point.z
+
+            points[i, 0] = x
+            points[i, 1] = y
+            points[i, 2] = z
+
+            for q in range(len(point_cloud.channels)):
+                points[i, 3 + q] = point_cloud.channels[q].values[i]
+
+        result = PointCloud(points)
+
+        return result
+
+    def to_ros_point_cloud(self, point_cloud, time_stamp):
+        """
+        Converts an OpenDR PointCloud message into a ROS2 PointCloud
+        :param point_cloud: OpenDR PointCloud
+        :type point_cloud: engine.data.PointCloud
+        :param time_stamp: Time stamp
+        :type time_stamp: ROS Time
+        :return: ROS PointCloud
+        :rtype: sensor_msgs.msg.PointCloud
+        """
+
+        ros_point_cloud = PointCloudMsg()
+
+        header = Header()
+
+        header.stamp = time_stamp
+        ros_point_cloud.header = header
+
+        channels_count = point_cloud.data.shape[-1] - 3
+
+        channels = [ChannelFloat32Msg(name="channel_" + str(i), values=[]) for i in range(channels_count)]
+        points = []
+
+        for point in point_cloud.data:
+            point_msg = Point32Msg()
+            point_msg.x = float(point[0])
+            point_msg.y = float(point[1])
+            point_msg.z = float(point[2])
+            points.append(point_msg)
+            for i in range(channels_count):
+                channels[i].values.append(float(point[3 + i]))
+
+        ros_point_cloud.points = points
+        ros_point_cloud.channels = channels
+
+        return ros_point_cloud
+
+    def from_ros_boxes_3d(self, ros_boxes_3d):
+        """
+        Converts a ROS2 Detection3DArray message into an OpenDR BoundingBox3D object.
+        :param ros_boxes_3d: The ROS boxes to be converted.
+        :type ros_boxes_3d: vision_msgs.msg.Detection3DArray
+        :return: An OpenDR BoundingBox3DList object.
+        :rtype: engine.target.BoundingBox3DList
+        """
+        boxes = []
+
+        for ros_box in ros_boxes_3d:
+
+            box = BoundingBox3D(
+                name=ros_box.results[0].id,
+                truncated=0,
+                occluded=0,
+                bbox2d=None,
+                dimensions=np.array([
+                    ros_box.bbox.size.position.x,
+                    ros_box.bbox.size.position.y,
+                    ros_box.bbox.size.position.z,
+                ]),
+                location=np.array([
+                    ros_box.bbox.center.position.x,
+                    ros_box.bbox.center.position.y,
+                    ros_box.bbox.center.position.z,
+                ]),
+                rotation_y=ros_box.bbox.center.rotation.y,
+                score=ros_box.results[0].score,
+            )
+            boxes.append(box)
+
+        result = BoundingBox3DList(boxes)
+        return result
+
+    def to_ros_boxes_3d(self, boxes_3d):
+        """
+        Converts an OpenDR BoundingBox3DList object into a ROS2 Detection3DArray message.
+        :param boxes_3d: The OpenDR boxes to be converted.
+        :type boxes_3d: engine.target.BoundingBox3DList
+        :return: ROS message with the boxes
+        :rtype: vision_msgs.msg.Detection3DArray
+        """
+        ros_boxes_3d = Detection3DArray()
+        for i in range(len(boxes_3d)):
+            box = Detection3D()
+            box.bbox = BoundingBox3DMsg()
+            box.results.append(ObjectHypothesisWithPose())
+            box.bbox.center = Pose3D()
+            box.bbox.center.position.x = float(boxes_3d[i].location[0])
+            box.bbox.center.position.y = float(boxes_3d[i].location[1])
+            box.bbox.center.position.z = float(boxes_3d[i].location[2])
+            box.bbox.center.orientation = QuaternionMsg(x=0.0, y=float(boxes_3d[i].rotation_y), z=0.0, w=0.0)
+            box.bbox.size.x = float(boxes_3d[i].dimensions[0])
+            box.bbox.size.y = float(boxes_3d[i].dimensions[1])
+            box.bbox.size.z = float(boxes_3d[i].dimensions[2])
+            box.results[0].id = boxes_3d[i].name
+            box.results[0].score = float(boxes_3d[i].confidence)
+            ros_boxes_3d.detections.append(box)
+        return ros_boxes_3d
+
     def from_ros_mesh(self, mesh_ROS):
         """
         Converts a ROS mesh into arrays of vertices and faces of a mesh

projects/opendr_ws_2/src/opendr_data_generation/package.xml

@@ -6,8 +6,7 @@
   <description>OpenDR's ROS2 nodes for data generation package</description>
   <maintainer email="[email protected]">tefas</maintainer>
   <license>Apache License v2.0</license>
-
-  <depend>rclcpp></depend>
+
   <depend>sensor_msgs</depend>
 
   <exec_depend>rclpy</exec_depend>

projects/opendr_ws_2/src/opendr_perception/opendr_perception/image_dataset_node.py

@@ -0,0 +1,113 @@
+#!/usr/bin/env python
+# Copyright 2020-2022 OpenDR European Project
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import argparse
+import os
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Image as ROS_Image
+from opendr_bridge import ROS2Bridge
+from opendr.engine.datasets import DatasetIterator
+from opendr.perception.object_tracking_2d import MotDataset, RawMotDatasetIterator
+
+
+class ImageDatasetNode(Node):
+    def __init__(
+            self,
+            dataset: DatasetIterator,
+            output_rgb_image_topic="/opendr/dataset_image",
+            data_fps=10,
+    ):
+        """
+        Creates a ROS2 Node for publishing dataset images
+        """
+
+        super().__init__('opendr_image_dataset_node')
+
+        self.dataset = dataset
+        self.bridge = ROS2Bridge()
+        self.timer = self.create_timer(1.0 / data_fps, self.timer_callback)
+        self.sample_index = 0
+
+        self.output_image_publisher = self.create_publisher(
+            ROS_Image, output_rgb_image_topic, 1
+        )
+        self.get_logger().info("Publishing images.")
+
+    def timer_callback(self):
+        image = self.dataset[self.sample_index % len(self.dataset)][0]
+        # Dataset should have an (Image, Target) pair as elements
+
+        message = self.bridge.to_ros_image(
+            image, encoding="bgr8"
+        )
+        self.output_image_publisher.publish(message)
+
+        self.sample_index += 1
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    parser = argparse.ArgumentParser()
+    parser.add_argument("-d", "--dataset_path", help="Path to a dataset",
+                        type=str, default="MOT")
+    parser.add_argument(
+        "-ks", "--mot20_subsets_path", help="Path to mot20 subsets",
+        type=str, default=os.path.join(
+            "..", "..", "src", "opendr", "perception", "object_tracking_2d",
+            "datasets", "splits", "nano_mot20.train"
+        )
+    )
+    parser.add_argument("-o", "--output_rgb_image_topic", help="Topic name to publish the data",
+                        type=str, default="/opendr/dataset_image")
+    parser.add_argument("-f", "--fps", help="Data FPS",
+                        type=float, default=30)
+    args = parser.parse_args()
+
+    dataset_path = args.dataset_path
+    mot20_subsets_path = args.mot20_subsets_path
+    output_rgb_image_topic = args.output_rgb_image_topic
+    data_fps = args.fps
+
+    if not os.path.exists(dataset_path):
+        dataset_path = MotDataset.download_nano_mot20(
+            "MOT", True
+        ).path
+
+    dataset = RawMotDatasetIterator(
+        dataset_path,
+        {
+            "mot20": mot20_subsets_path
+        },
+        scan_labels=False
+    )
+    dataset_node = ImageDatasetNode(
+        dataset,
+        output_rgb_image_topic=output_rgb_image_topic,
+        data_fps=data_fps,
+    )
+
+    rclpy.spin(dataset_node)
+
+    # Destroy the node explicitly
+    # (optional - otherwise it will be done automatically
+    # when the garbage collector destroys the node object)
+    dataset_node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()