From 855b176178b2dd814ed91fb9f5e3c7e4d7f54851 Mon Sep 17 00:00:00 2001 From: Illia Oleksiienko Date: Mon, 26 Sep 2022 08:42:19 +0000 Subject: [PATCH 01/44] Add ros2 point cloud dataset node --- projects/opendr_ws_2/.gitignore | 3 + .../point_cloud_dataset_node.py | 102 ++++++++++++++++++ .../src/opendr_perception/setup.py | 1 + .../opendr_ros2_bridge/bridge.py | 41 ++++++- .../second_detector/core/cc/nms/nms_cpu.h | 1 + 5 files changed, 145 insertions(+), 3 deletions(-) create mode 100644 projects/opendr_ws_2/.gitignore create mode 100644 projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py diff --git a/projects/opendr_ws_2/.gitignore b/projects/opendr_ws_2/.gitignore new file mode 100644 index 0000000000..e8b072e4b4 --- /dev/null +++ b/projects/opendr_ws_2/.gitignore @@ -0,0 +1,3 @@ +build +install +log \ No newline at end of file diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py new file mode 100644 index 0000000000..45b4b805d0 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py @@ -0,0 +1,102 @@ +#!/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 time +from sensor_msgs.msg import PointCloud as ROS_PointCloud +import rclpy +from rclpy.node import Node +from opendr_ros2_bridge import ROS2Bridge +from opendr.engine.datasets import DatasetIterator +from opendr.perception.object_detection_3d import KittiDataset, LabeledPointCloudsDatasetIterator + + +class PointCloudDatasetNode(Node): + def __init__( + self, + dataset: DatasetIterator, + output_point_cloud_topic="/opendr/dataset_point_cloud", + data_fps=10, + ): + """ + Creates a ROS Node for publishing dataset point clouds + """ + + super().__init__('point_cloud_dataset_node') + + # Initialize the face detector + self.dataset = dataset + # Initialize OpenDR ROSBridge object + self.bridge = ROS2Bridge() + self.timer = self.create_timer(1.0 / data_fps, self.timer_callback) + self.sample_index = 0 + + if output_point_cloud_topic is not None: + self.output_point_cloud_publisher = self.create_publisher( + ROS_PointCloud, output_point_cloud_topic, 1 + ) + + def timer_callback(self): + + point_cloud = self.dataset[self.sample_index % len(self.dataset)][0] # Dataset should have a (PointCloud, Target) pair as elements + + self.get_logger().info("Publishing point_cloud [" + str(self.sample_index) + "]") + message = self.bridge.to_ros_point_cloud( + point_cloud, self.get_clock().now().to_msg() + ) + self.output_point_cloud_publisher.publish(message) + + self.sample_index += 1 + +def main( + args=None, + dataset_path="KITTI/opendr_nano_kitti", + kitti_subsets_path="../../src/opendr/perception/object_detection_3d/datasets/nano_kitti_subsets", + # kitti_subsets_path Only used if a KITTI dataset is downloaded +): + rclpy.init(args=args) + parser = argparse.ArgumentParser() + parser.add_argument("-d", "--dataset_path", help="Path to a dataset. If does not exist, nano KITTI dataset will be downloaded there.", + type=str, default="image_raw") + parser.add_argument("-ks", "--kitti_subsets_path", help="Path to kitti subsets. Used only if a KITTI dataset is downloaded", + type=str, default="../../src/opendr/perception/object_detection_3d/datasets/nano_kitti_subsets") + args = parser.parse_args() + + if not os.path.exists(dataset_path): + dataset_path = KittiDataset.download_nano_kitti( + "KITTI", kitti_subsets_path=kitti_subsets_path, + create_dir=True, + ).path + + dataset = LabeledPointCloudsDatasetIterator( + dataset_path + "/training/velodyne_reduced", + dataset_path + "/training/label_2", + dataset_path + "/training/calib", + ) + + dataset_node = PointCloudDatasetNode(dataset) + + 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() diff --git a/projects/opendr_ws_2/src/opendr_perception/setup.py b/projects/opendr_ws_2/src/opendr_perception/setup.py index 8568b257e5..9e43b2b472 100644 --- a/projects/opendr_ws_2/src/opendr_perception/setup.py +++ b/projects/opendr_ws_2/src/opendr_perception/setup.py @@ -29,6 +29,7 @@ 'semantic_segmentation_bisenet = opendr_perception.semantic_segmentation_bisenet_node:main', 'face_recognition = opendr_perception.face_recognition_node:main', 'fall_detection = opendr_perception.fall_detection_node:main', + 'point_cloud_dataset = opendr_perception.point_cloud_dataset_node:main', ], }, ) diff --git a/projects/opendr_ws_2/src/opendr_ros2_bridge/opendr_ros2_bridge/bridge.py b/projects/opendr_ws_2/src/opendr_ros2_bridge/opendr_ros2_bridge/bridge.py index 369ca1f458..a8fb08594a 100644 --- a/projects/opendr_ws_2/src/opendr_ros2_bridge/opendr_ros2_bridge/bridge.py +++ b/projects/opendr_ws_2/src/opendr_ros2_bridge/opendr_ros2_bridge/bridge.py @@ -17,10 +17,10 @@ from opendr.engine.target import Pose, BoundingBox, BoundingBoxList, Category from cv_bridge import CvBridge -from std_msgs.msg import String -from sensor_msgs.msg import Image as ImageMsg +from std_msgs.msg import String, Header +from sensor_msgs.msg import Image as ImageMsg, PointCloud as PointCloudMsg, ChannelFloat32 as ChannelFloat32Msg from vision_msgs.msg import Detection2DArray, Detection2D, BoundingBox2D, ObjectHypothesisWithPose -from geometry_msgs.msg import Pose2D +from geometry_msgs.msg import Pose2D, Point32 as Point32Msg from opendr_ros2_messages.msg import OpenDRPose2D, OpenDRPose2DKeypoint @@ -243,3 +243,38 @@ def to_ros_face_id(self, category): result = String() result.data = category.description return result + + def to_ros_point_cloud(self, point_cloud, time_stamp): + """ + Converts an OpenDR PointCloud message into a ROS2 PointCloud + :param: OpenDR PointCloud + :type: engine.data.PointCloud + :return message: ROS PointCloud + :rtype message: 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 \ No newline at end of file diff --git a/src/opendr/perception/object_detection_3d/voxel_object_detection_3d/second_detector/core/cc/nms/nms_cpu.h b/src/opendr/perception/object_detection_3d/voxel_object_detection_3d/second_detector/core/cc/nms/nms_cpu.h index cbf644aac3..252d6c536f 100644 --- a/src/opendr/perception/object_detection_3d/voxel_object_detection_3d/second_detector/core/cc/nms/nms_cpu.h +++ b/src/opendr/perception/object_detection_3d/voxel_object_detection_3d/second_detector/core/cc/nms/nms_cpu.h @@ -7,6 +7,7 @@ #include #include #include +#include namespace py = pybind11; using namespace pybind11::literals; template inline py::array_t constant(ShapeContainer shape, DType value) { From 7b843034dc7aaafc987267e583ea55b21d7fc45f Mon Sep 17 00:00:00 2001 From: Illia Oleksiienko Date: Mon, 26 Sep 2022 09:03:13 +0000 Subject: [PATCH 02/44] Fix point cloude dataset node args --- .../opendr_perception/point_cloud_dataset_node.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py index 45b4b805d0..d376630dca 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py @@ -63,9 +63,6 @@ def timer_callback(self): def main( args=None, - dataset_path="KITTI/opendr_nano_kitti", - kitti_subsets_path="../../src/opendr/perception/object_detection_3d/datasets/nano_kitti_subsets", - # kitti_subsets_path Only used if a KITTI dataset is downloaded ): rclpy.init(args=args) parser = argparse.ArgumentParser() @@ -75,6 +72,9 @@ def main( type=str, default="../../src/opendr/perception/object_detection_3d/datasets/nano_kitti_subsets") args = parser.parse_args() + dataset_path = args.dataset_path + kitti_subsets_path = args.kitti_subsets_path + if not os.path.exists(dataset_path): dataset_path = KittiDataset.download_nano_kitti( "KITTI", kitti_subsets_path=kitti_subsets_path, From 82148ce76b9845ff2b01ce8f166d56f3a7b36a9e Mon Sep 17 00:00:00 2001 From: Illia Oleksiienko Date: Mon, 26 Sep 2022 09:06:25 +0000 Subject: [PATCH 03/44] Update default dataset path --- .../opendr_perception/point_cloud_dataset_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py index d376630dca..dc5958e957 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py @@ -67,7 +67,7 @@ def main( rclpy.init(args=args) parser = argparse.ArgumentParser() parser.add_argument("-d", "--dataset_path", help="Path to a dataset. If does not exist, nano KITTI dataset will be downloaded there.", - type=str, default="image_raw") + type=str, default="KITTI/opendr_nano_kitti") parser.add_argument("-ks", "--kitti_subsets_path", help="Path to kitti subsets. Used only if a KITTI dataset is downloaded", type=str, default="../../src/opendr/perception/object_detection_3d/datasets/nano_kitti_subsets") args = parser.parse_args() From fdd205e959fe98af2c3499e77bd8f7da187bd8df Mon Sep 17 00:00:00 2001 From: Illia Oleksiienko Date: Mon, 26 Sep 2022 09:56:25 +0000 Subject: [PATCH 04/44] Add voxel 3d detection node --- .../object_detection_3d_voxel_node.py | 136 ++++++++++++++++++ .../point_cloud_dataset_node.py | 10 +- .../src/opendr_perception/setup.py | 1 + .../opendr_ros2_bridge/bridge.py | 108 +++++++++++++- 4 files changed, 248 insertions(+), 7 deletions(-) create mode 100644 projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py new file mode 100644 index 0000000000..0e1f802cef --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py @@ -0,0 +1,136 @@ +#!/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 torch +import os +import rclpy +from rclpy.node import Node +from vision_msgs.msg import Detection3DArray +from sensor_msgs.msg import PointCloud as ROS_PointCloud +from opendr_ros2_bridge import ROS2Bridge +from opendr.perception.object_detection_3d import VoxelObjectDetection3DLearner + + +class ObjectDetection3DVoxelNode(Node): + def __init__( + self, + input_point_cloud_topic="/opendr/dataset_point_cloud", + output_detection3d_topic="/opendr/detection3d", + device="cuda:0", + model_name="tanet_car_xyres_16", + model_config_path=os.path.join( + "..", "..", "src", "opendr", "perception", "object_detection_3d", + "voxel_object_detection_3d", "second_detector", "configs", "tanet", + "ped_cycle", "test_short.proto" + ), + temp_dir="temp", + ): + """ + Creates a ROS2 Node for 3D object detection + :param input_point_cloud_topic: Topic from which we are reading the input point cloud + :type input_image_topic: str + :param output_detection3d_topic: Topic to which we are publishing the annotations + :type output_detection3d_topic: str + :param device: device on which we are running inference ('cpu' or 'cuda') + :type device: str + :param model_name: the pretrained model to download or a trained model in temp_dir + :type model_name: str + :param temp_dir: where to store models + :type temp_dir: str + """ + + super().__init__('object_detection_3d_voxel_node') + + self.get_logger().info("Using model_name: {}".format(model_name)) + + self.learner = VoxelObjectDetection3DLearner( + device=device, temp_path=temp_dir, model_config_path=model_config_path + ) + if not os.path.exists(os.path.join(temp_dir, model_name)): + VoxelObjectDetection3DLearner.download(model_name, temp_dir) + + self.learner.load(os.path.join(temp_dir, model_name), verbose=True) + + # Initialize OpenDR ROSBridge object + self.bridge = ROS2Bridge() + + self.detection_publisher = self.create_publisher( + Detection3DArray, output_detection3d_topic, 1 + ) + + self.create_subscription(ROS_PointCloud, input_point_cloud_topic, self.callback, 1) + + self.get_logger().info("Ready to listen") + + def callback(self, data): + """ + Callback that process the input data and publishes to the corresponding topics + :param data: input message + :type data: sensor_msgs.msg.Image + """ + + # Convert sensor_msgs.msg.Image into OpenDR Image + point_cloud = self.bridge.from_ros_point_cloud(data) + detection_boxes = self.learner.infer(point_cloud) + + # Convert detected boxes to ROS type and publish + ros_boxes = self.bridge.to_ros_boxes_3d(detection_boxes, classes=["Car", "Van", "Truck", "Pedestrian", "Cyclist"]) + if self.detection_publisher is not None: + self.detection_publisher.publish(ros_boxes) + self.get_logger().info("Published " + str(len(detection_boxes)) + "detection boxes") + +def main( + args=None, +): + rclpy.init(args=args) + parser = argparse.ArgumentParser() + parser.add_argument("-n", "--model_name", help="Name of the trained model", + type=str, default="tanet_car_xyres_16") + parser.add_argument( + "-c", "--model_config_path", help="Path to a model .proto config", + type=str, default=os.path.join( + "..", "..", "src", "opendr", "perception", "object_detection_3d", + "voxel_object_detection_3d", "second_detector", "configs", "tanet", + "car", "xyres_16.proto" + ) + ) + parser.add_argument("-t", "--temp_dir", help="Path to a temp dir with models", + type=str, default="temp") + parser.add_argument("-i", "--input_point_cloud_topic", + help="Point Cloud topic provdied by either a point_cloud_dataset_node or any other 3D Point Cloud Node", + type=str, default="/opendr/dataset_point_cloud") + parser.add_argument("--device", help="Device to use, either \"cpu\" or \"cuda\", defaults to \"cuda\"", + type=str, default="cuda", choices=["cuda", "cpu"]) + args = parser.parse_args() + + voxel_node = ObjectDetection3DVoxelNode( + device=args.device, + model_name=args.model_name, + model_config_path=args.model_config_path, + input_point_cloud_topic=args.input_point_cloud_topic, + temp_dir=args.temp_dir, + ) + + rclpy.spin(voxel_node) + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + voxel_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py index dc5958e957..ec4490b0ab 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py @@ -70,10 +70,16 @@ def main( type=str, default="KITTI/opendr_nano_kitti") parser.add_argument("-ks", "--kitti_subsets_path", help="Path to kitti subsets. Used only if a KITTI dataset is downloaded", type=str, default="../../src/opendr/perception/object_detection_3d/datasets/nano_kitti_subsets") + parser.add_argument("-o", "--output_point_cloud_topic", help="Topic name to upload the data", + type=str, default="/opendr/dataset_point_cloud") + parser.add_argument("-f", "--fps", help="Data FPS", + type=float, default=10) args = parser.parse_args() dataset_path = args.dataset_path kitti_subsets_path = args.kitti_subsets_path + output_point_cloud_topic = args.output_point_cloud_topic + data_fps = args.fps if not os.path.exists(dataset_path): dataset_path = KittiDataset.download_nano_kitti( @@ -87,7 +93,9 @@ def main( dataset_path + "/training/calib", ) - dataset_node = PointCloudDatasetNode(dataset) + dataset_node = PointCloudDatasetNode( + dataset, output_point_cloud_topic=output_point_cloud_topic, data_fps=data_fps + ) rclpy.spin(dataset_node) diff --git a/projects/opendr_ws_2/src/opendr_perception/setup.py b/projects/opendr_ws_2/src/opendr_perception/setup.py index 9e43b2b472..08d87f62a0 100644 --- a/projects/opendr_ws_2/src/opendr_perception/setup.py +++ b/projects/opendr_ws_2/src/opendr_perception/setup.py @@ -30,6 +30,7 @@ 'face_recognition = opendr_perception.face_recognition_node:main', 'fall_detection = opendr_perception.fall_detection_node:main', 'point_cloud_dataset = opendr_perception.point_cloud_dataset_node:main', + 'object_detection_3d_voxel = opendr_perception.object_detection_3d_voxel_node:main', ], }, ) diff --git a/projects/opendr_ws_2/src/opendr_ros2_bridge/opendr_ros2_bridge/bridge.py b/projects/opendr_ws_2/src/opendr_ros2_bridge/opendr_ros2_bridge/bridge.py index a8fb08594a..0607960c2c 100644 --- a/projects/opendr_ws_2/src/opendr_ros2_bridge/opendr_ros2_bridge/bridge.py +++ b/projects/opendr_ws_2/src/opendr_ros2_bridge/opendr_ros2_bridge/bridge.py @@ -13,14 +13,22 @@ # limitations under the License. import numpy as np -from opendr.engine.data import Image -from opendr.engine.target import Pose, BoundingBox, BoundingBoxList, Category - +from opendr.engine.data import Image, PointCloud +from opendr.engine.target import ( + Pose, BoundingBox, BoundingBoxList, Category, + BoundingBox3D, BoundingBox3DList +) from cv_bridge import CvBridge from std_msgs.msg import String, Header from sensor_msgs.msg import Image as ImageMsg, PointCloud as PointCloudMsg, ChannelFloat32 as ChannelFloat32Msg -from vision_msgs.msg import Detection2DArray, Detection2D, BoundingBox2D, ObjectHypothesisWithPose -from geometry_msgs.msg import Pose2D, Point32 as Point32Msg +from vision_msgs.msg import ( + Detection2DArray, Detection2D, BoundingBox2D, ObjectHypothesisWithPose, + Detection3D, Detection3DArray, BoundingBox3D as BoundingBox3DMsg +) +from geometry_msgs.msg import ( + Pose2D, Point32 as Point32Msg, + Quaternion as QuaternionMsg, Pose as Pose3D +) from opendr_ros2_messages.msg import OpenDRPose2D, OpenDRPose2DKeypoint @@ -244,6 +252,32 @@ 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 message: ROS PointCloud to be converted + :type message: 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 @@ -277,4 +311,66 @@ def to_ros_point_cloud(self, point_cloud, time_stamp): ros_point_cloud.points = points ros_point_cloud.channels = channels - return ros_point_cloud \ No newline at end of file + 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 + :param classes: The array of classes to transform from string name into an index. + :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 From 77818af45fcfae33d78f4a6fa577fb31d631440b Mon Sep 17 00:00:00 2001 From: Illia Oleksiienko Date: Mon, 26 Sep 2022 10:14:23 +0000 Subject: [PATCH 05/44] Add output topic to args --- .../opendr_perception/object_detection_3d_voxel_node.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py index 0e1f802cef..d738a2352b 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py @@ -87,10 +87,10 @@ def callback(self, data): detection_boxes = self.learner.infer(point_cloud) # Convert detected boxes to ROS type and publish - ros_boxes = self.bridge.to_ros_boxes_3d(detection_boxes, classes=["Car", "Van", "Truck", "Pedestrian", "Cyclist"]) + ros_boxes = self.bridge.to_ros_boxes_3d(detection_boxes) if self.detection_publisher is not None: self.detection_publisher.publish(ros_boxes) - self.get_logger().info("Published " + str(len(detection_boxes)) + "detection boxes") + self.get_logger().info("Published " + str(len(detection_boxes)) + " detection boxes") def main( args=None, @@ -112,6 +112,9 @@ def main( parser.add_argument("-i", "--input_point_cloud_topic", help="Point Cloud topic provdied by either a point_cloud_dataset_node or any other 3D Point Cloud Node", type=str, default="/opendr/dataset_point_cloud") + parser.add_argument("-o", "--output_detection3d_topic", + help="Output detections topic", + type=str, default="/opendr/detection3d") parser.add_argument("--device", help="Device to use, either \"cpu\" or \"cuda\", defaults to \"cuda\"", type=str, default="cuda", choices=["cuda", "cpu"]) args = parser.parse_args() @@ -122,6 +125,7 @@ def main( model_config_path=args.model_config_path, input_point_cloud_topic=args.input_point_cloud_topic, temp_dir=args.temp_dir, + output_detection3d_topic=args.output_detection3d_topic, ) rclpy.spin(voxel_node) From 1991e5be90cfbb8846186eb97ad349e2b37b16df Mon Sep 17 00:00:00 2001 From: Illia Oleksiienko Date: Mon, 26 Sep 2022 10:27:30 +0000 Subject: [PATCH 06/44] Add tracking 3d node --- .../object_tracking_3d_ab3dmot_node.py | 165 ++++++++++++++++++ .../src/opendr_perception/setup.py | 1 + 2 files changed, 166 insertions(+) create mode 100644 projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py new file mode 100644 index 0000000000..4169878fc3 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py @@ -0,0 +1,165 @@ +#!/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 torch +from opendr.engine.learners import Learner +import rclpy +from rclpy.node import Node +from vision_msgs.msg import Detection3DArray +from std_msgs.msg import Int32MultiArray +from sensor_msgs.msg import PointCloud as ROS_PointCloud +from opendr_ros2_bridge import ROS2Bridge +from opendr.perception.object_tracking_3d import ObjectTracking3DAb3dmotLearner +from opendr.perception.object_detection_3d import VoxelObjectDetection3DLearner + + +class ObjectTracking3DAb3dmotNode(Node): + def __init__( + self, + detector: Learner, + input_point_cloud_topic="/opendr/dataset_point_cloud", + output_detection3d_topic="/opendr/detection3d", + output_tracking3d_id_topic="/opendr/tracking3d_id", + device="cuda:0", + ): + """ + Creates a ROS2 Node for 3D object tracking + :param detector: Learner that proides 3D object detections + :type detector: Learner + :param input_point_cloud_topic: Topic from which we are reading the input point cloud + :type input_image_topic: str + :param output_detection3d_topic: Topic to which we are publishing the annotations + :type output_detection3d_topic: str + :param output_tracking3d_id_topic: Topic to which we are publishing the tracking ids + :type output_tracking3d_id_topic: str + :param device: device on which we are running inference ('cpu' or 'cuda') + :type device: str + """ + super().__init__('object_tracking_3d_ab3dmot_node') + + self.detector = detector + self.learner = ObjectTracking3DAb3dmotLearner( + device=device + ) + + # Initialize OpenDR ROSBridge object + self.bridge = ROS2Bridge() + + self.detection_publisher = self.create_publisher( + Detection3DArray, output_detection3d_topic, 1 + ) + self.tracking_id_publisher = self.create_publisher( + Int32MultiArray, output_tracking3d_id_topic, 1 + ) + + self.create_subscription(ROS_PointCloud, input_point_cloud_topic, self.callback, 1) + + self.get_logger().info("Ready to listen") + + def callback(self, data): + """ + Callback that process the input data and publishes to the corresponding topics + :param data: input message + :type data: sensor_msgs.msg.Image + """ + + # Convert sensor_msgs.msg.Image into OpenDR Image + point_cloud = self.bridge.from_ros_point_cloud(data) + detection_boxes = self.detector.infer(point_cloud) + tracking_boxes = self.learner.infer(detection_boxes) + ids = [tracking_box.id for tracking_box in tracking_boxes] + + # Convert detected boxes to ROS type and publish + ros_boxes = self.bridge.to_ros_boxes_3d(detection_boxes) + if self.detection_publisher is not None: + self.detection_publisher.publish(ros_boxes) + self.get_logger().info("Published " + str(len(detection_boxes)) + " detection boxes") + + ros_ids = Int32MultiArray() + ros_ids.data = ids + + if self.tracking_id_publisher is not None: + self.tracking_id_publisher.publish(ros_ids) + self.get_logger().info("Published " + str(len(ids)) + " tracking ids") + +def main( + args=None, +): + rclpy.init(args=args) + parser = argparse.ArgumentParser() + parser.add_argument("-dn", "--detector_model_name", help="Name of the trained model", + type=str, default="tanet_car_xyres_16") + parser.add_argument( + "-dc", "--detector_model_config_path", help="Path to a model .proto config", + type=str, default=os.path.join( + "..", "..", "src", "opendr", "perception", "object_detection_3d", + "voxel_object_detection_3d", "second_detector", "configs", "tanet", + "car", "xyres_16.proto" + ) + ) + parser.add_argument("-t", "--temp_dir", help="Path to a temp dir with models", + type=str, default="temp") + parser.add_argument("-i", "--input_point_cloud_topic", + help="Point Cloud topic provdied by either a point_cloud_dataset_node or any other 3D Point Cloud Node", + type=str, default="/opendr/dataset_point_cloud") + parser.add_argument("-od", "--output_detection3d_topic", + help="Output detections topic", + type=str, default="/opendr/detection3d") + parser.add_argument("-ot", "--output_tracking3d_id_topic", + help="Output tracking topic", + type=str, default="/opendr/tracking3d_id") + parser.add_argument("--device", help="Device to use, either \"cpu\" or \"cuda\", defaults to \"cuda\"", + type=str, default="cuda", choices=["cuda", "cpu"]) + args = parser.parse_args() + + input_point_cloud_topic = args.input_point_cloud_topic + detector_model_name = args.detector_model_name + temp_dir = args.temp_dir + device = args.device + detector_model_config_path = args.detector_model_config_path + output_detection3d_topic = args.output_detection3d_topic + output_tracking3d_id_topic = args.output_tracking3d_id_topic + + detector = VoxelObjectDetection3DLearner( + device=device, + temp_path=temp_dir, + model_config_path=detector_model_config_path + ) + if not os.path.exists(os.path.join(temp_dir, detector_model_name)): + VoxelObjectDetection3DLearner.download(detector_model_name, temp_dir) + + detector.load(os.path.join(temp_dir, detector_model_name), verbose=True) + + # created node object + ab3dmot_node = ObjectTracking3DAb3dmotNode( + detector=detector, + device=device, + input_point_cloud_topic=input_point_cloud_topic, + output_detection3d_topic=output_detection3d_topic, + output_tracking3d_id_topic=output_tracking3d_id_topic, + ) + + rclpy.spin(ab3dmot_node) + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + ab3dmot_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/projects/opendr_ws_2/src/opendr_perception/setup.py b/projects/opendr_ws_2/src/opendr_perception/setup.py index 08d87f62a0..c432b4c874 100644 --- a/projects/opendr_ws_2/src/opendr_perception/setup.py +++ b/projects/opendr_ws_2/src/opendr_perception/setup.py @@ -31,6 +31,7 @@ 'fall_detection = opendr_perception.fall_detection_node:main', 'point_cloud_dataset = opendr_perception.point_cloud_dataset_node:main', 'object_detection_3d_voxel = opendr_perception.object_detection_3d_voxel_node:main', + 'object_tracking_3d_ab3dmot = opendr_perception.object_tracking_3d_ab3dmot_node:main', ], }, ) From 12fa2cb054daedc6efb222b8aa1512d0f7c32674 Mon Sep 17 00:00:00 2001 From: Illia Oleksiienko Date: Mon, 26 Sep 2022 11:32:25 +0000 Subject: [PATCH 07/44] Add tracking 2d fairmot node --- .../opendr_perception/image_dataset_node.py | 115 +++++++++ .../object_detection_3d_voxel_node.py | 1 - .../object_tracking_2d_fair_mot_node.py | 225 ++++++++++++++++++ .../src/opendr_perception/setup.py | 2 + 4 files changed, 342 insertions(+), 1 deletion(-) create mode 100644 projects/opendr_ws_2/src/opendr_perception/opendr_perception/image_dataset_node.py create mode 100755 projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/image_dataset_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/image_dataset_node.py new file mode 100644 index 0000000000..a6735e0016 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/image_dataset_node.py @@ -0,0 +1,115 @@ +#!/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_ros2_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_image_topic="/opendr/dataset_image", + data_fps=10, + ): + """ + Creates a ROS2 Node for publishing dataset images + """ + + super().__init__('image_dataset_node') + + # Initialize the face detector + self.dataset = dataset + # Initialize OpenDR ROSBridge object + self.bridge = ROS2Bridge() + self.timer = self.create_timer(1.0 / data_fps, self.timer_callback) + self.sample_index = 0 + + if output_image_topic is not None: + self.output_image_publisher = self.create_publisher( + ROS_Image, output_image_topic, 1 + ) + + def timer_callback(self): + + image = self.dataset[self.sample_index % len(self.dataset)][0] # Dataset should have an (Image, Target) pair as elements + + self.get_logger().info("Publishing image [" + str(self.sample_index) + "]") + message = self.bridge.to_ros_image( + image, encoding="rgb8" + ) + 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="KITTI/opendr_nano_kitti") + 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_image_topic", help="Topic name to upload 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_image_topic = args.output_image_topic + data_fps = args.fps + + 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_image_topic=output_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() diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py index d738a2352b..2efe4f804a 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py @@ -14,7 +14,6 @@ # limitations under the License. import argparse -import torch import os import rclpy from rclpy.node import Node diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py new file mode 100755 index 0000000000..edec3b669f --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py @@ -0,0 +1,225 @@ +#!/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 cv2 +import torch +import os +from opendr.engine.target import TrackingAnnotation +import rclpy +from rclpy.node import Node +from vision_msgs.msg import Detection2DArray +from std_msgs.msg import Int32MultiArray +from sensor_msgs.msg import Image as ROS_Image +from opendr_ros2_bridge import ROS2Bridge +from opendr.perception.object_tracking_2d import ( + ObjectTracking2DFairMotLearner, +) +from opendr.engine.data import Image + + +class ObjectTracking2DFairMotNode(Node): + def __init__( + self, + input_image_topic="/usb_cam/image_raw", + output_detection_topic="/opendr/detection", + output_tracking_id_topic="/opendr/tracking_id", + output_image_topic="/opendr/image_annotated", + device="cuda:0", + model_name="fairmot_dla34", + temp_dir="temp", + ): + """ + Creates a ROS Node for 2D object tracking + :param input_image_topic: Topic from which we are reading the input image + :type input_image_topic: str + :param output_image_topic: Topic to which we are publishing the annotated image (if None, we are not publishing + annotated image) + :type output_image_topic: str + :param output_detection_topic: Topic to which we are publishing the detections + :type output_detection_topic: str + :param output_tracking_id_topic: Topic to which we are publishing the tracking ids + :type output_tracking_id_topic: str + :param device: device on which we are running inference ('cpu' or 'cuda') + :type device: str + :param model_name: the pretrained model to download or a saved model in temp_dir folder to use + :type model_name: str + :param temp_dir: the folder to download models + :type temp_dir: str + """ + + super().__init__('object_tracking_2d_fair_mot_node') + + # # Initialize the face detector + self.learner = ObjectTracking2DFairMotLearner( + device=device, temp_path=temp_dir, + ) + if not os.path.exists(os.path.join(temp_dir, model_name)): + ObjectTracking2DFairMotLearner.download(model_name, temp_dir) + + self.learner.load(os.path.join(temp_dir, model_name), verbose=True) + + # Initialize OpenDR ROSBridge object + self.bridge = ROS2Bridge() + + self.detection_publisher = self.create_publisher( + Detection2DArray, output_detection_topic, 1 + ) + self.tracking_id_publisher = self.create_publisher( + Int32MultiArray, output_tracking_id_topic, 1 + ) + + if output_image_topic is not None: + self.output_image_publisher = self.create_publisher( + ROS_Image, output_image_topic, 1 + ) + + self.create_subscription(ROS_Image, input_image_topic, self.callback, 1) + + def callback(self, data): + """ + Callback that process the input data and publishes to the corresponding topics + :param data: input message + :type data: sensor_msgs.msg.Image + """ + + # Convert sensor_msgs.msg.Image into OpenDR Image + image = self.bridge.from_ros_image(data, encoding="bgr8") + tracking_boxes = self.learner.infer(image) + + if self.output_image_publisher is not None: + frame = image.opencv() + draw_predictions(frame, tracking_boxes) + message = self.bridge.to_ros_image( + Image(frame), encoding="bgr8" + ) + self.output_image_publisher.publish(message) + self.get_logger().info("Published annotated image") + + detection_boxes = tracking_boxes.bounding_box_list() + ids = [tracking_box.id for tracking_box in tracking_boxes] + + # Convert detected boxes to ROS type and publish + ros_boxes = self.bridge.to_ros_boxes(detection_boxes) + if self.detection_publisher is not None: + self.detection_publisher.publish(ros_boxes) + self.get_logger().info("Published " + str(len(detection_boxes)) + " detection boxes") + + ros_ids = Int32MultiArray() + ros_ids.data = ids + + if self.tracking_id_publisher is not None: + self.tracking_id_publisher.publish(ros_ids) + self.get_logger().info("Published " + str(len(ids)) + " tracking ids") + + +colors = [ + (255, 0, 255), + (0, 0, 255), + (0, 255, 0), + (255, 0, 0), + (35, 69, 55), + (43, 63, 54), +] + + +def draw_predictions(frame, predictions: TrackingAnnotation, is_centered=False, is_flipped_xy=True): + global colors + w, h, _ = frame.shape + + for prediction in predictions.boxes: + prediction = prediction + + if not hasattr(prediction, "id"): + prediction.id = 0 + + color = colors[int(prediction.id) * 7 % len(colors)] + + x = prediction.left + y = prediction.top + + if is_flipped_xy: + x = prediction.top + y = prediction.left + + if is_centered: + x -= prediction.width + y -= prediction.height + + cv2.rectangle( + frame, + (int(x), int(y)), + ( + int(x + prediction.width), + int(y + prediction.height), + ), + color, + 2, + ) + + +def main( + args=None, +): + rclpy.init(args=args) + parser = argparse.ArgumentParser() + parser.add_argument("-n", "--model_name", help="Name of the trained model", + type=str, default="fairmot_dla34") + parser.add_argument( + "-c", "--model_config_path", help="Path to a model .proto config", + type=str, default=os.path.join( + "..", "..", "src", "opendr", "perception", "object_detection_3d", + "voxel_object_detection_3d", "second_detector", "configs", "tanet", + "car", "xyres_16.proto" + ) + ) + parser.add_argument("-t", "--temp_dir", help="Path to a temp dir with models", + type=str, default="temp") + parser.add_argument("-i", "--input_image_topic", + help="Input Image topic provdied by either an image_dataset_node, webcam or any other image node", + type=str, default="/opendr/dataset_image") + parser.add_argument("-od", "--output_detection_topic", + help="Output detections topic", + type=str, default="/opendr/detection") + parser.add_argument("-ot", "--output_tracking_id_topic", + help="Output detections topic", + type=str, default="/opendr/tracking_id") + parser.add_argument("-oi", "--output_image_topic", + help="Output detections topic", + type=str, default="/opendr/image_annotated") + parser.add_argument("--device", help="Device to use, either \"cpu\" or \"cuda\", defaults to \"cuda\"", + type=str, default="cuda", choices=["cuda", "cpu"]) + args = parser.parse_args() + + fair_mot_node = ObjectTracking2DFairMotNode( + device=args.device, + model_name=args.model_name, + input_image_topic=args.input_image_topic, + temp_dir=args.temp_dir, + output_detection_topic=args.output_detection_topic, + output_tracking_id_topic=args.output_tracking_id_topic, + output_image_topic=args.output_image_topic if args.output_image_topic is not "None" else None, + ) + + rclpy.spin(fair_mot_node) + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + fair_mot_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/projects/opendr_ws_2/src/opendr_perception/setup.py b/projects/opendr_ws_2/src/opendr_perception/setup.py index c432b4c874..8162fc09dd 100644 --- a/projects/opendr_ws_2/src/opendr_perception/setup.py +++ b/projects/opendr_ws_2/src/opendr_perception/setup.py @@ -30,8 +30,10 @@ 'face_recognition = opendr_perception.face_recognition_node:main', 'fall_detection = opendr_perception.fall_detection_node:main', 'point_cloud_dataset = opendr_perception.point_cloud_dataset_node:main', + 'image_dataset = opendr_perception.image_dataset_node:main', 'object_detection_3d_voxel = opendr_perception.object_detection_3d_voxel_node:main', 'object_tracking_3d_ab3dmot = opendr_perception.object_tracking_3d_ab3dmot_node:main', + 'object_tracking_2d_fair_mot = opendr_perception.object_tracking_2d_fair_mot_node:main', ], }, ) From 5fe25d456bd93825efccddc4cec9691017cbfbbc Mon Sep 17 00:00:00 2001 From: Illia Oleksiienko Date: Mon, 26 Sep 2022 12:48:08 +0000 Subject: [PATCH 08/44] Add deep sort ros2 node --- projects/opendr_ws_2/.gitignore | 4 +- .../object_tracking_2d_deep_sort_node.py | 233 ++++++++++++++++++ .../object_tracking_2d_fair_mot_node.py | 9 - .../src/opendr_perception/setup.py | 1 + .../deep_sort/algorithm/deep_sort_tracker.py | 2 +- 5 files changed, 238 insertions(+), 11 deletions(-) create mode 100644 projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py diff --git a/projects/opendr_ws_2/.gitignore b/projects/opendr_ws_2/.gitignore index e8b072e4b4..b4181cec9d 100644 --- a/projects/opendr_ws_2/.gitignore +++ b/projects/opendr_ws_2/.gitignore @@ -1,3 +1,5 @@ build install -log \ No newline at end of file +log +MOT +KITTI \ No newline at end of file diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py new file mode 100644 index 0000000000..2ec7367d15 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py @@ -0,0 +1,233 @@ +#!/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 cv2 +import torch +import os +from opendr.engine.target import TrackingAnnotation +import rclpy +from rclpy.node import Node +from vision_msgs.msg import Detection2DArray +from std_msgs.msg import Int32MultiArray +from sensor_msgs.msg import Image as ROS_Image +from opendr_ros2_bridge import ROS2Bridge +from opendr.engine.learners import Learner +from opendr.perception.object_tracking_2d import ( + ObjectTracking2DDeepSortLearner, + ObjectTracking2DFairMotLearner +) +from opendr.engine.data import Image, ImageWithDetections + + +class ObjectTracking2DDeepSortNode(Node): + def __init__( + self, + detector: Learner, + input_image_topic="/usb_cam/image_raw", + output_detection_topic="/opendr/detection", + output_tracking_id_topic="/opendr/tracking_id", + output_image_topic="/opendr/image_annotated", + device="cuda:0", + model_name="deep_sort", + temp_dir="temp", + ): + """ + Creates a ROS2 Node for 2D object tracking + :param detector: Learner to generate object detections + :type detector: Learner + :param input_image_topic: Topic from which we are reading the input image + :type input_image_topic: str + :param output_image_topic: Topic to which we are publishing the annotated image (if None, we are not publishing + annotated image) + :type output_image_topic: str + :param output_detection_topic: Topic to which we are publishing the detections + :type output_detection_topic: str + :param output_tracking_id_topic: Topic to which we are publishing the tracking ids + :type output_tracking_id_topic: str + :param device: device on which we are running inference ('cpu' or 'cuda') + :type device: str + :param model_name: the pretrained model to download or a saved model in temp_dir folder to use + :type model_name: str + :param temp_dir: the folder to download models + :type temp_dir: str + """ + + super().__init__('object_tracking_2d_deep_sort_node') + + self.get_logger().info("Using model_name: {}".format(model_name)) + + self.detector = detector + self.learner = ObjectTracking2DDeepSortLearner( + device=device, temp_path=temp_dir, + ) + if not os.path.exists(os.path.join(temp_dir, model_name)): + ObjectTracking2DDeepSortLearner.download(model_name, temp_dir) + + self.learner.load(os.path.join(temp_dir, model_name), verbose=True) + + # Initialize OpenDR ROSBridge object + self.bridge = ROS2Bridge() + self.tracking_id_publisher = self.create_publisher( + Int32MultiArray, output_tracking_id_topic, 1 + ) + + if output_image_topic is not None: + self.output_image_publisher = self.create_publisher( + ROS_Image, output_image_topic, 1 + ) + + self.detection_publisher = self.create_publisher( + Detection2DArray, output_detection_topic, 1 + ) + + self.create_subscription(ROS_Image, input_image_topic, self.callback, 1) + + def callback(self, data): + """ + Callback that process the input data and publishes to the corresponding topics + :param data: input message + :type data: sensor_msgs.msg.Image + """ + + # Convert sensor_msgs.msg.Image into OpenDR Image + image = self.bridge.from_ros_image(data, encoding="bgr8") + detection_boxes = self.detector.infer(image) + image_with_detections = ImageWithDetections(image.numpy(), detection_boxes) + tracking_boxes = self.learner.infer(image_with_detections) + + if self.output_image_publisher is not None: + frame = image.opencv() + draw_predictions(frame, tracking_boxes) + message = self.bridge.to_ros_image( + Image(frame), encoding="bgr8" + ) + self.output_image_publisher.publish(message) + self.get_logger().info("Published annotated image") + + ids = [int(tracking_box.id) for tracking_box in tracking_boxes] + + # Convert detected boxes to ROS type and publish + ros_boxes = self.bridge.to_ros_boxes(detection_boxes) + if self.detection_publisher is not None: + self.detection_publisher.publish(ros_boxes) + self.get_logger().info("Published " + str(len(detection_boxes)) + " detection boxes") + + ros_ids = Int32MultiArray() + ros_ids.data = ids + + if self.tracking_id_publisher is not None: + self.tracking_id_publisher.publish(ros_ids) + self.get_logger().info("Published " + str(len(ids)) + " tracking ids") + + +colors = [ + (255, 0, 255), + (0, 0, 255), + (0, 255, 0), + (255, 0, 0), + (35, 69, 55), + (43, 63, 54), +] + + +def draw_predictions(frame, predictions: TrackingAnnotation, is_centered=False, is_flipped_xy=True): + global colors + w, h, _ = frame.shape + + for prediction in predictions.boxes: + prediction = prediction + + if not hasattr(prediction, "id"): + prediction.id = 0 + + color = colors[int(prediction.id) * 7 % len(colors)] + + x = prediction.left + y = prediction.top + + if is_flipped_xy: + x = prediction.top + y = prediction.left + + if is_centered: + x -= prediction.width + y -= prediction.height + + cv2.rectangle( + frame, + (int(x), int(y)), + ( + int(x + prediction.width), + int(y + prediction.height), + ), + color, + 2, + ) + + +def main( + args=None, +): + rclpy.init(args=args) + parser = argparse.ArgumentParser() + parser.add_argument("-n", "--model_name", help="Name of the trained model", + type=str, default="deep_sort") + parser.add_argument("-t", "--temp_dir", help="Path to a temp dir with models", + type=str, default="temp") + parser.add_argument("-i", "--input_image_topic", + help="Input Image topic provdied by either an image_dataset_node, webcam or any other image node", + type=str, default="/opendr/dataset_image") + parser.add_argument("-od", "--output_detection_topic", + help="Output detections topic", + type=str, default="/opendr/detection") + parser.add_argument("-ot", "--output_tracking_id_topic", + help="Output detections topic", + type=str, default="/opendr/tracking_id") + parser.add_argument("-oi", "--output_image_topic", + help="Output detections topic", + type=str, default="/opendr/image_annotated") + parser.add_argument("--device", help="Device to use, either \"cpu\" or \"cuda\", defaults to \"cuda\"", + type=str, default="cuda", choices=["cuda", "cpu"]) + args = parser.parse_args() + + detection_learner = ObjectTracking2DFairMotLearner( + device=args.device, temp_path=args.temp_dir, + ) + if not os.path.exists(os.path.join(args.temp_dir, "fairmot_dla34")): + ObjectTracking2DFairMotLearner.download("fairmot_dla34", args.temp_dir) + + detection_learner.load(os.path.join(args.temp_dir, "fairmot_dla34"), verbose=True) + + deep_sort_node = ObjectTracking2DDeepSortNode( + detector=detection_learner, + device=args.device, + model_name=args.model_name, + input_image_topic=args.input_image_topic, + temp_dir=args.temp_dir, + output_detection_topic=args.output_detection_topic, + output_tracking_id_topic=args.output_tracking_id_topic, + output_image_topic=args.output_image_topic if args.output_image_topic is not "None" else None, + ) + rclpy.spin(deep_sort_node) + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + deep_sort_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py index edec3b669f..008fbf8491 100755 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py @@ -15,7 +15,6 @@ import argparse import cv2 -import torch import os from opendr.engine.target import TrackingAnnotation import rclpy @@ -177,14 +176,6 @@ def main( parser = argparse.ArgumentParser() parser.add_argument("-n", "--model_name", help="Name of the trained model", type=str, default="fairmot_dla34") - parser.add_argument( - "-c", "--model_config_path", help="Path to a model .proto config", - type=str, default=os.path.join( - "..", "..", "src", "opendr", "perception", "object_detection_3d", - "voxel_object_detection_3d", "second_detector", "configs", "tanet", - "car", "xyres_16.proto" - ) - ) parser.add_argument("-t", "--temp_dir", help="Path to a temp dir with models", type=str, default="temp") parser.add_argument("-i", "--input_image_topic", diff --git a/projects/opendr_ws_2/src/opendr_perception/setup.py b/projects/opendr_ws_2/src/opendr_perception/setup.py index 8162fc09dd..26776f929f 100644 --- a/projects/opendr_ws_2/src/opendr_perception/setup.py +++ b/projects/opendr_ws_2/src/opendr_perception/setup.py @@ -34,6 +34,7 @@ 'object_detection_3d_voxel = opendr_perception.object_detection_3d_voxel_node:main', 'object_tracking_3d_ab3dmot = opendr_perception.object_tracking_3d_ab3dmot_node:main', 'object_tracking_2d_fair_mot = opendr_perception.object_tracking_2d_fair_mot_node:main', + 'object_tracking_2d_deep_sort = opendr_perception.object_tracking_2d_deep_sort_node:main', ], }, ) diff --git a/src/opendr/perception/object_tracking_2d/deep_sort/algorithm/deep_sort_tracker.py b/src/opendr/perception/object_tracking_2d/deep_sort/algorithm/deep_sort_tracker.py index edd44325f2..7073b1e6ff 100644 --- a/src/opendr/perception/object_tracking_2d/deep_sort/algorithm/deep_sort_tracker.py +++ b/src/opendr/perception/object_tracking_2d/deep_sort/algorithm/deep_sort_tracker.py @@ -56,7 +56,7 @@ def infer(self, imageWithDetections: ImageWithDetections, frame_id=None): if frame_id is not None: self.frame = frame_id - image = imageWithDetections.numpy().transpose(2, 1, 0) + image = imageWithDetections.numpy().transpose(1, 2, 0) detections = imageWithDetections.boundingBoxList bbox_xywh = [] From 8433818edf803d9d3bfb31fa75774dd14b67d972 Mon Sep 17 00:00:00 2001 From: Illia Oleksiienko Date: Mon, 26 Sep 2022 13:16:00 +0000 Subject: [PATCH 09/44] Fix style errors --- .../opendr_perception/image_dataset_node.py | 4 +++- .../object_detection_3d_voxel_node.py | 5 +++-- .../object_tracking_2d_deep_sort_node.py | 5 ++--- .../object_tracking_2d_fair_mot_node.py | 4 ++-- .../object_tracking_3d_ab3dmot_node.py | 4 ++-- .../opendr_perception/point_cloud_dataset_node.py | 14 +++++++++----- .../opendr_ros2_bridge/bridge.py | 4 ++-- .../second_detector/core/cc/nms/nms_cpu.h | 1 + tests/test_license.py | 2 ++ tests/test_pep8.py | 1 + 10 files changed, 27 insertions(+), 17 deletions(-) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/image_dataset_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/image_dataset_node.py index a6735e0016..449e6cc6bc 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/image_dataset_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/image_dataset_node.py @@ -50,7 +50,8 @@ def __init__( def timer_callback(self): - image = self.dataset[self.sample_index % len(self.dataset)][0] # Dataset should have an (Image, Target) pair as elements + image = self.dataset[self.sample_index % len(self.dataset)][0] + # Dataset should have an (Image, Target) pair as elements self.get_logger().info("Publishing image [" + str(self.sample_index) + "]") message = self.bridge.to_ros_image( @@ -60,6 +61,7 @@ def timer_callback(self): self.sample_index += 1 + def main( args=None, ): diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py index 2efe4f804a..a55466326d 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py @@ -71,7 +71,7 @@ def __init__( ) self.create_subscription(ROS_PointCloud, input_point_cloud_topic, self.callback, 1) - + self.get_logger().info("Ready to listen") def callback(self, data): @@ -91,6 +91,7 @@ def callback(self, data): self.detection_publisher.publish(ros_boxes) self.get_logger().info("Published " + str(len(detection_boxes)) + " detection boxes") + def main( args=None, ): @@ -115,7 +116,7 @@ def main( help="Output detections topic", type=str, default="/opendr/detection3d") parser.add_argument("--device", help="Device to use, either \"cpu\" or \"cuda\", defaults to \"cuda\"", - type=str, default="cuda", choices=["cuda", "cpu"]) + type=str, default="cuda", choices=["cuda", "cpu"]) args = parser.parse_args() voxel_node = ObjectDetection3DVoxelNode( diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py index 2ec7367d15..981cb31bb8 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py @@ -15,7 +15,6 @@ import argparse import cv2 -import torch import os from opendr.engine.target import TrackingAnnotation import rclpy @@ -200,7 +199,7 @@ def main( help="Output detections topic", type=str, default="/opendr/image_annotated") parser.add_argument("--device", help="Device to use, either \"cpu\" or \"cuda\", defaults to \"cuda\"", - type=str, default="cuda", choices=["cuda", "cpu"]) + type=str, default="cuda", choices=["cuda", "cpu"]) args = parser.parse_args() detection_learner = ObjectTracking2DFairMotLearner( @@ -219,7 +218,7 @@ def main( temp_dir=args.temp_dir, output_detection_topic=args.output_detection_topic, output_tracking_id_topic=args.output_tracking_id_topic, - output_image_topic=args.output_image_topic if args.output_image_topic is not "None" else None, + output_image_topic=args.output_image_topic if args.output_image_topic != "None" else None, ) rclpy.spin(deep_sort_node) # Destroy the node explicitly diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py index 008fbf8491..0c565018f5 100755 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py @@ -191,7 +191,7 @@ def main( help="Output detections topic", type=str, default="/opendr/image_annotated") parser.add_argument("--device", help="Device to use, either \"cpu\" or \"cuda\", defaults to \"cuda\"", - type=str, default="cuda", choices=["cuda", "cpu"]) + type=str, default="cuda", choices=["cuda", "cpu"]) args = parser.parse_args() fair_mot_node = ObjectTracking2DFairMotNode( @@ -201,7 +201,7 @@ def main( temp_dir=args.temp_dir, output_detection_topic=args.output_detection_topic, output_tracking_id_topic=args.output_tracking_id_topic, - output_image_topic=args.output_image_topic if args.output_image_topic is not "None" else None, + output_image_topic=args.output_image_topic if args.output_image_topic != "None" else None, ) rclpy.spin(fair_mot_node) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py index 4169878fc3..32d55284e8 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py @@ -15,7 +15,6 @@ import argparse import os -import torch from opendr.engine.learners import Learner import rclpy from rclpy.node import Node @@ -96,6 +95,7 @@ def callback(self, data): self.tracking_id_publisher.publish(ros_ids) self.get_logger().info("Published " + str(len(ids)) + " tracking ids") + def main( args=None, ): @@ -123,7 +123,7 @@ def main( help="Output tracking topic", type=str, default="/opendr/tracking3d_id") parser.add_argument("--device", help="Device to use, either \"cpu\" or \"cuda\", defaults to \"cuda\"", - type=str, default="cuda", choices=["cuda", "cpu"]) + type=str, default="cuda", choices=["cuda", "cpu"]) args = parser.parse_args() input_point_cloud_topic = args.input_point_cloud_topic diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py index ec4490b0ab..5eb6a82e30 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py @@ -15,7 +15,6 @@ import argparse import os -import time from sensor_msgs.msg import PointCloud as ROS_PointCloud import rclpy from rclpy.node import Node @@ -51,7 +50,8 @@ def __init__( def timer_callback(self): - point_cloud = self.dataset[self.sample_index % len(self.dataset)][0] # Dataset should have a (PointCloud, Target) pair as elements + point_cloud = self.dataset[self.sample_index % len(self.dataset)][0] + # Dataset should have a (PointCloud, Target) pair as elements self.get_logger().info("Publishing point_cloud [" + str(self.sample_index) + "]") message = self.bridge.to_ros_point_cloud( @@ -61,15 +61,19 @@ def timer_callback(self): 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. If does not exist, nano KITTI dataset will be downloaded there.", + parser.add_argument("-d", "--dataset_path", + help="Path to a dataset. If does not exist, nano KITTI dataset will be downloaded there.", type=str, default="KITTI/opendr_nano_kitti") - parser.add_argument("-ks", "--kitti_subsets_path", help="Path to kitti subsets. Used only if a KITTI dataset is downloaded", - type=str, default="../../src/opendr/perception/object_detection_3d/datasets/nano_kitti_subsets") + parser.add_argument("-ks", "--kitti_subsets_path", + help="Path to kitti subsets. Used only if a KITTI dataset is downloaded", + type=str, + default="../../src/opendr/perception/object_detection_3d/datasets/nano_kitti_subsets") parser.add_argument("-o", "--output_point_cloud_topic", help="Topic name to upload the data", type=str, default="/opendr/dataset_point_cloud") parser.add_argument("-f", "--fps", help="Data FPS", diff --git a/projects/opendr_ws_2/src/opendr_ros2_bridge/opendr_ros2_bridge/bridge.py b/projects/opendr_ws_2/src/opendr_ros2_bridge/opendr_ros2_bridge/bridge.py index 0607960c2c..c4cc2b0ecb 100644 --- a/projects/opendr_ws_2/src/opendr_ros2_bridge/opendr_ros2_bridge/bridge.py +++ b/projects/opendr_ws_2/src/opendr_ros2_bridge/opendr_ros2_bridge/bridge.py @@ -290,7 +290,7 @@ def to_ros_point_cloud(self, point_cloud, time_stamp): ros_point_cloud = PointCloudMsg() header = Header() - + header.stamp = time_stamp ros_point_cloud.header = header @@ -312,7 +312,7 @@ def to_ros_point_cloud(self, point_cloud, time_stamp): 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. diff --git a/src/opendr/perception/object_detection_3d/voxel_object_detection_3d/second_detector/core/cc/nms/nms_cpu.h b/src/opendr/perception/object_detection_3d/voxel_object_detection_3d/second_detector/core/cc/nms/nms_cpu.h index 252d6c536f..773f2b684f 100644 --- a/src/opendr/perception/object_detection_3d/voxel_object_detection_3d/second_detector/core/cc/nms/nms_cpu.h +++ b/src/opendr/perception/object_detection_3d/voxel_object_detection_3d/second_detector/core/cc/nms/nms_cpu.h @@ -8,6 +8,7 @@ #include #include #include + namespace py = pybind11; using namespace pybind11::literals; template inline py::array_t constant(ShapeContainer shape, DType value) { diff --git a/tests/test_license.py b/tests/test_license.py index a82525a927..89b6fcac59 100755 --- a/tests/test_license.py +++ b/tests/test_license.py @@ -108,6 +108,8 @@ def setUp(self): 'src/opendr/perception/facial_expression_recognition/landmark_based_facial_expression_recognition', 'projects/opendr_ws_2/src/opendr_perception/test', 'projects/opendr_ws_2/src/opendr_ros2_bridge/test', + 'projects/opendr_ws_2/src/vision_opencv', + 'projects/opendr_ws_2/install', ] skippedFilePaths = [ diff --git a/tests/test_pep8.py b/tests/test_pep8.py index 800c6064c4..52ac8f8b6b 100755 --- a/tests/test_pep8.py +++ b/tests/test_pep8.py @@ -33,6 +33,7 @@ 'lib', 'src/opendr/perception/panoptic_segmentation/efficient_ps/algorithm/EfficientPS', 'projects/python/control/eagerx', + 'projects/opendr_ws_2/src/vision_opencv', 'venv', 'build', ] From b5e33f4c3bfd2526926d30449f0efcc50c8b1f5b Mon Sep 17 00:00:00 2001 From: Illia Oleksiienko Date: Mon, 26 Sep 2022 13:42:45 +0000 Subject: [PATCH 10/44] Fix C++ style error --- .../second_detector/core/cc/nms/nms_cpu.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/opendr/perception/object_detection_3d/voxel_object_detection_3d/second_detector/core/cc/nms/nms_cpu.h b/src/opendr/perception/object_detection_3d/voxel_object_detection_3d/second_detector/core/cc/nms/nms_cpu.h index 773f2b684f..cc7a9f235a 100644 --- a/src/opendr/perception/object_detection_3d/voxel_object_detection_3d/second_detector/core/cc/nms/nms_cpu.h +++ b/src/opendr/perception/object_detection_3d/voxel_object_detection_3d/second_detector/core/cc/nms/nms_cpu.h @@ -6,8 +6,8 @@ #include #include #include -#include #include +#include namespace py = pybind11; using namespace pybind11::literals; From 804dafdcbbca98470b51f57ec9b7555f316d548a Mon Sep 17 00:00:00 2001 From: Illia Oleksiienko Date: Mon, 26 Sep 2022 21:26:06 +0000 Subject: [PATCH 11/44] Add device parsing --- .../object_detection_3d_voxel_node.py | 16 +++++++++++++++- .../object_tracking_2d_deep_sort_node.py | 18 ++++++++++++++++-- .../object_tracking_2d_fair_mot_node.py | 16 +++++++++++++++- .../object_tracking_3d_ab3dmot_node.py | 14 ++++++++++++++ 4 files changed, 60 insertions(+), 4 deletions(-) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py index a55466326d..8b1ffb332a 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py @@ -13,6 +13,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +import torch import argparse import os import rclpy @@ -119,8 +120,21 @@ def main( type=str, default="cuda", choices=["cuda", "cpu"]) args = parser.parse_args() + try: + if args.device == "cuda" and torch.cuda.is_available(): + device = "cuda" + elif args.device == "cuda": + print("GPU not found. Using CPU instead.") + device = "cpu" + else: + print("Using CPU.") + device = "cpu" + except: + print("Using CPU.") + device = "cpu" + voxel_node = ObjectDetection3DVoxelNode( - device=args.device, + device=device, model_name=args.model_name, model_config_path=args.model_config_path, input_point_cloud_topic=args.input_point_cloud_topic, diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py index 981cb31bb8..dace246853 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py @@ -13,6 +13,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +import torch import argparse import cv2 import os @@ -202,8 +203,21 @@ def main( type=str, default="cuda", choices=["cuda", "cpu"]) args = parser.parse_args() + try: + if args.device == "cuda" and torch.cuda.is_available(): + device = "cuda" + elif args.device == "cuda": + print("GPU not found. Using CPU instead.") + device = "cpu" + else: + print("Using CPU.") + device = "cpu" + except: + print("Using CPU.") + device = "cpu" + detection_learner = ObjectTracking2DFairMotLearner( - device=args.device, temp_path=args.temp_dir, + device=device, temp_path=args.temp_dir, ) if not os.path.exists(os.path.join(args.temp_dir, "fairmot_dla34")): ObjectTracking2DFairMotLearner.download("fairmot_dla34", args.temp_dir) @@ -212,7 +226,7 @@ def main( deep_sort_node = ObjectTracking2DDeepSortNode( detector=detection_learner, - device=args.device, + device=device, model_name=args.model_name, input_image_topic=args.input_image_topic, temp_dir=args.temp_dir, diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py index 0c565018f5..28616e4178 100755 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py @@ -13,6 +13,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +import torch import argparse import cv2 import os @@ -194,8 +195,21 @@ def main( type=str, default="cuda", choices=["cuda", "cpu"]) args = parser.parse_args() + try: + if args.device == "cuda" and torch.cuda.is_available(): + device = "cuda" + elif args.device == "cuda": + print("GPU not found. Using CPU instead.") + device = "cpu" + else: + print("Using CPU.") + device = "cpu" + except: + print("Using CPU.") + device = "cpu" + fair_mot_node = ObjectTracking2DFairMotNode( - device=args.device, + device=device, model_name=args.model_name, input_image_topic=args.input_image_topic, temp_dir=args.temp_dir, diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py index 32d55284e8..203fa35723 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py @@ -13,6 +13,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +import torch import argparse import os from opendr.engine.learners import Learner @@ -134,6 +135,19 @@ def main( output_detection3d_topic = args.output_detection3d_topic output_tracking3d_id_topic = args.output_tracking3d_id_topic + try: + if args.device == "cuda" and torch.cuda.is_available(): + device = "cuda" + elif args.device == "cuda": + print("GPU not found. Using CPU instead.") + device = "cpu" + else: + print("Using CPU.") + device = "cpu" + except: + print("Using CPU.") + device = "cpu" + detector = VoxelObjectDetection3DLearner( device=device, temp_path=temp_dir, From d362a54d5c422f046ef0f6620d571e283d304d49 Mon Sep 17 00:00:00 2001 From: Illia Oleksiienko Date: Wed, 9 Nov 2022 13:53:58 +0000 Subject: [PATCH 12/44] Move ros2 gitignore to global --- .gitignore | 6 ++++++ projects/opendr_ws_2/.gitignore | 5 ----- 2 files changed, 6 insertions(+), 5 deletions(-) delete mode 100644 projects/opendr_ws_2/.gitignore diff --git a/.gitignore b/.gitignore index 58ae399224..e5f6454b35 100644 --- a/.gitignore +++ b/.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 \ No newline at end of file diff --git a/projects/opendr_ws_2/.gitignore b/projects/opendr_ws_2/.gitignore deleted file mode 100644 index b4181cec9d..0000000000 --- a/projects/opendr_ws_2/.gitignore +++ /dev/null @@ -1,5 +0,0 @@ -build -install -log -MOT -KITTI \ No newline at end of file From b38a5a97daa57e20e102d00d13f5ff4a68b507a1 Mon Sep 17 00:00:00 2001 From: Illia Oleksiienko Date: Wed, 9 Nov 2022 14:00:07 +0000 Subject: [PATCH 13/44] Fix image dataset conditions --- .../opendr_perception/image_dataset_node.py | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/image_dataset_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/image_dataset_node.py index 449e6cc6bc..07e863a0f9 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/image_dataset_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/image_dataset_node.py @@ -36,17 +36,14 @@ def __init__( super().__init__('image_dataset_node') - # Initialize the face detector self.dataset = dataset - # Initialize OpenDR ROSBridge object self.bridge = ROS2Bridge() self.timer = self.create_timer(1.0 / data_fps, self.timer_callback) self.sample_index = 0 - if output_image_topic is not None: - self.output_image_publisher = self.create_publisher( - ROS_Image, output_image_topic, 1 - ) + self.output_image_publisher = self.create_publisher( + ROS_Image, output_image_topic, 1 + ) def timer_callback(self): @@ -55,7 +52,7 @@ def timer_callback(self): self.get_logger().info("Publishing image [" + str(self.sample_index) + "]") message = self.bridge.to_ros_image( - image, encoding="rgb8" + image, encoding="bgr8" ) self.output_image_publisher.publish(message) @@ -87,9 +84,10 @@ def main( output_image_topic = args.output_image_topic data_fps = args.fps - dataset_path = MotDataset.download_nano_mot20( - "MOT", True - ).path + if not os.path.exists(dataset_path): + dataset_path = MotDataset.download_nano_mot20( + "MOT", True + ).path dataset = RawMotDatasetIterator( dataset_path, From 96cfb180e6fe8c7c2cf070aabd34a5d11486af98 Mon Sep 17 00:00:00 2001 From: Illia Oleksiienko Date: Wed, 9 Nov 2022 14:15:32 +0000 Subject: [PATCH 14/44] Fix docstrings --- .../opendr_ros2_bridge/bridge.py | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/projects/opendr_ws_2/src/opendr_ros2_bridge/opendr_ros2_bridge/bridge.py b/projects/opendr_ws_2/src/opendr_ros2_bridge/opendr_ros2_bridge/bridge.py index c4cc2b0ecb..302c914769 100644 --- a/projects/opendr_ws_2/src/opendr_ros2_bridge/opendr_ros2_bridge/bridge.py +++ b/projects/opendr_ws_2/src/opendr_ros2_bridge/opendr_ros2_bridge/bridge.py @@ -255,8 +255,8 @@ def to_ros_face_id(self, category): def from_ros_point_cloud(self, point_cloud: PointCloudMsg): """ Converts a ROS PointCloud message into an OpenDR PointCloud - :param message: ROS PointCloud to be converted - :type message: sensor_msgs.msg.PointCloud + :param point_cloud: ROS PointCloud to be converted + :type point_cloud: sensor_msgs.msg.PointCloud :return: OpenDR PointCloud :rtype: engine.data.PointCloud """ @@ -281,10 +281,12 @@ def from_ros_point_cloud(self, point_cloud: PointCloudMsg): def to_ros_point_cloud(self, point_cloud, time_stamp): """ Converts an OpenDR PointCloud message into a ROS2 PointCloud - :param: OpenDR PointCloud - :type: engine.data.PointCloud - :return message: ROS PointCloud - :rtype message: sensor_msgs.msg.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() @@ -353,7 +355,6 @@ 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 - :param classes: The array of classes to transform from string name into an index. :return: ROS message with the boxes :rtype: vision_msgs.msg.Detection3DArray """ From e8977b39dabf9b3058458bc6d16e4d27db9d8713 Mon Sep 17 00:00:00 2001 From: Illia Oleksiienko Date: Wed, 9 Nov 2022 14:38:53 +0000 Subject: [PATCH 15/44] Fix pc dataset comments and conditions --- .../opendr_perception/point_cloud_dataset_node.py | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py index 5eb6a82e30..aa1d6d710a 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py @@ -36,17 +36,14 @@ def __init__( super().__init__('point_cloud_dataset_node') - # Initialize the face detector self.dataset = dataset - # Initialize OpenDR ROSBridge object self.bridge = ROS2Bridge() self.timer = self.create_timer(1.0 / data_fps, self.timer_callback) self.sample_index = 0 - if output_point_cloud_topic is not None: - self.output_point_cloud_publisher = self.create_publisher( - ROS_PointCloud, output_point_cloud_topic, 1 - ) + self.output_point_cloud_publisher = self.create_publisher( + ROS_PointCloud, output_point_cloud_topic, 1 + ) def timer_callback(self): From 8e9dacd65f859ae93d11eefb56f8ec733e0a15c3 Mon Sep 17 00:00:00 2001 From: Illia Oleksiienko Date: Wed, 9 Nov 2022 14:44:17 +0000 Subject: [PATCH 16/44] Fix voxel 3d arguments and conditions --- .../opendr_perception/object_detection_3d_voxel_node.py | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py index 8b1ffb332a..6ca4aaeafe 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py @@ -41,7 +41,7 @@ def __init__( """ Creates a ROS2 Node for 3D object detection :param input_point_cloud_topic: Topic from which we are reading the input point cloud - :type input_image_topic: str + :type input_point_cloud_topic: str :param output_detection3d_topic: Topic to which we are publishing the annotations :type output_detection3d_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') @@ -88,9 +88,8 @@ def callback(self, data): # Convert detected boxes to ROS type and publish ros_boxes = self.bridge.to_ros_boxes_3d(detection_boxes) - if self.detection_publisher is not None: - self.detection_publisher.publish(ros_boxes) - self.get_logger().info("Published " + str(len(detection_boxes)) + " detection boxes") + self.detection_publisher.publish(ros_boxes) + self.get_logger().info("Published " + str(len(detection_boxes)) + " detection boxes") def main( @@ -111,7 +110,7 @@ def main( parser.add_argument("-t", "--temp_dir", help="Path to a temp dir with models", type=str, default="temp") parser.add_argument("-i", "--input_point_cloud_topic", - help="Point Cloud topic provdied by either a point_cloud_dataset_node or any other 3D Point Cloud Node", + help="Point Cloud topic provided by either a point_cloud_dataset_node or any other 3D Point Cloud Node", type=str, default="/opendr/dataset_point_cloud") parser.add_argument("-o", "--output_detection3d_topic", help="Output detections topic", From 7aef6505e94cf529725c7009c62322d125eff345 Mon Sep 17 00:00:00 2001 From: Illia Oleksiienko Date: Wed, 9 Nov 2022 14:49:50 +0000 Subject: [PATCH 17/44] Fix ab3dmot conditions --- .../object_tracking_3d_ab3dmot_node.py | 30 ++++++++++--------- 1 file changed, 16 insertions(+), 14 deletions(-) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py index 203fa35723..380b393ee7 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py @@ -59,12 +59,15 @@ def __init__( # Initialize OpenDR ROSBridge object self.bridge = ROS2Bridge() - self.detection_publisher = self.create_publisher( - Detection3DArray, output_detection3d_topic, 1 - ) - self.tracking_id_publisher = self.create_publisher( - Int32MultiArray, output_tracking3d_id_topic, 1 - ) + if output_detection3d_topic is not None: + self.detection_publisher = self.create_publisher( + Detection3DArray, output_detection3d_topic, 1 + ) + + if output_tracking3d_id_topic is not None: + self.tracking_id_publisher = self.create_publisher( + Int32MultiArray, output_tracking3d_id_topic, 1 + ) self.create_subscription(ROS_PointCloud, input_point_cloud_topic, self.callback, 1) @@ -81,18 +84,17 @@ def callback(self, data): point_cloud = self.bridge.from_ros_point_cloud(data) detection_boxes = self.detector.infer(point_cloud) tracking_boxes = self.learner.infer(detection_boxes) - ids = [tracking_box.id for tracking_box in tracking_boxes] # Convert detected boxes to ROS type and publish - ros_boxes = self.bridge.to_ros_boxes_3d(detection_boxes) if self.detection_publisher is not None: + ros_boxes = self.bridge.to_ros_boxes_3d(detection_boxes) self.detection_publisher.publish(ros_boxes) self.get_logger().info("Published " + str(len(detection_boxes)) + " detection boxes") - ros_ids = Int32MultiArray() - ros_ids.data = ids - if self.tracking_id_publisher is not None: + ids = [tracking_box.id for tracking_box in tracking_boxes] + ros_ids = Int32MultiArray() + ros_ids.data = ids self.tracking_id_publisher.publish(ros_ids) self.get_logger().info("Published " + str(len(ids)) + " tracking ids") @@ -115,7 +117,7 @@ def main( parser.add_argument("-t", "--temp_dir", help="Path to a temp dir with models", type=str, default="temp") parser.add_argument("-i", "--input_point_cloud_topic", - help="Point Cloud topic provdied by either a point_cloud_dataset_node or any other 3D Point Cloud Node", + help="Point Cloud topic provided by either a point_cloud_dataset_node or any other 3D Point Cloud Node", type=str, default="/opendr/dataset_point_cloud") parser.add_argument("-od", "--output_detection3d_topic", help="Output detections topic", @@ -163,8 +165,8 @@ def main( detector=detector, device=device, input_point_cloud_topic=input_point_cloud_topic, - output_detection3d_topic=output_detection3d_topic, - output_tracking3d_id_topic=output_tracking3d_id_topic, + output_detection3d_topic=output_detection3d_topic if output_detection3d_topic != "None" else None, + output_tracking3d_id_topic=output_tracking3d_id_topic if output_tracking3d_id_topic != "None" else None, ) rclpy.spin(ab3dmot_node) From 80117fa9119aad0b34027cdf56a07cb644faba7c Mon Sep 17 00:00:00 2001 From: Illia Oleksiienko Date: Wed, 9 Nov 2022 14:52:30 +0000 Subject: [PATCH 18/44] Fix unused device var --- .../opendr_perception/object_tracking_2d_deep_sort_node.py | 2 +- .../opendr_perception/object_tracking_2d_fair_mot_node.py | 2 +- .../opendr_perception/object_tracking_3d_ab3dmot_node.py | 1 - 3 files changed, 2 insertions(+), 3 deletions(-) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py index dace246853..586aeec313 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py @@ -188,7 +188,7 @@ def main( parser.add_argument("-t", "--temp_dir", help="Path to a temp dir with models", type=str, default="temp") parser.add_argument("-i", "--input_image_topic", - help="Input Image topic provdied by either an image_dataset_node, webcam or any other image node", + help="Input Image topic provided by either an image_dataset_node, webcam or any other image node", type=str, default="/opendr/dataset_image") parser.add_argument("-od", "--output_detection_topic", help="Output detections topic", diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py index 28616e4178..83c20c537a 100755 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py @@ -180,7 +180,7 @@ def main( parser.add_argument("-t", "--temp_dir", help="Path to a temp dir with models", type=str, default="temp") parser.add_argument("-i", "--input_image_topic", - help="Input Image topic provdied by either an image_dataset_node, webcam or any other image node", + help="Input Image topic provided by either an image_dataset_node, webcam or any other image node", type=str, default="/opendr/dataset_image") parser.add_argument("-od", "--output_detection_topic", help="Output detections topic", diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py index 380b393ee7..474afcf173 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py @@ -132,7 +132,6 @@ def main( input_point_cloud_topic = args.input_point_cloud_topic detector_model_name = args.detector_model_name temp_dir = args.temp_dir - device = args.device detector_model_config_path = args.detector_model_config_path output_detection3d_topic = args.output_detection3d_topic output_tracking3d_id_topic = args.output_tracking3d_id_topic From 21995df421e728384efc7ee7dd1aa6383ec719d8 Mon Sep 17 00:00:00 2001 From: Illia Oleksiienko Date: Wed, 9 Nov 2022 15:01:06 +0000 Subject: [PATCH 19/44] Fix deep sort conditions --- .../object_tracking_2d_deep_sort_node.py | 47 +++++++++---------- 1 file changed, 23 insertions(+), 24 deletions(-) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py index 586aeec313..dc26028c06 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py @@ -17,7 +17,7 @@ import argparse import cv2 import os -from opendr.engine.target import TrackingAnnotation +from opendr.engine.target import TrackingAnnotationList import rclpy from rclpy.node import Node from vision_msgs.msg import Detection2DArray @@ -37,9 +37,9 @@ def __init__( self, detector: Learner, input_image_topic="/usb_cam/image_raw", - output_detection_topic="/opendr/detection", - output_tracking_id_topic="/opendr/tracking_id", - output_image_topic="/opendr/image_annotated", + output_detection_topic="/opendr/deep_sort_detection", + output_tracking_id_topic="/opendr/deep_sort_tracking_id", + output_image_topic="/opendr/deep_sort_image_annotated", device="cuda:0", model_name="deep_sort", temp_dir="temp", @@ -78,20 +78,22 @@ def __init__( self.learner.load(os.path.join(temp_dir, model_name), verbose=True) - # Initialize OpenDR ROSBridge object self.bridge = ROS2Bridge() - self.tracking_id_publisher = self.create_publisher( - Int32MultiArray, output_tracking_id_topic, 1 - ) + + if output_tracking_id_topic is not None: + self.tracking_id_publisher = self.create_publisher( + Int32MultiArray, output_tracking_id_topic, 1 + ) if output_image_topic is not None: self.output_image_publisher = self.create_publisher( ROS_Image, output_image_topic, 1 ) - self.detection_publisher = self.create_publisher( - Detection2DArray, output_detection_topic, 1 - ) + if output_detection_topic is not None: + self.detection_publisher = self.create_publisher( + Detection2DArray, output_detection_topic, 1 + ) self.create_subscription(ROS_Image, input_image_topic, self.callback, 1) @@ -117,18 +119,15 @@ def callback(self, data): self.output_image_publisher.publish(message) self.get_logger().info("Published annotated image") - ids = [int(tracking_box.id) for tracking_box in tracking_boxes] - - # Convert detected boxes to ROS type and publish - ros_boxes = self.bridge.to_ros_boxes(detection_boxes) if self.detection_publisher is not None: + ros_boxes = self.bridge.to_ros_boxes(detection_boxes) self.detection_publisher.publish(ros_boxes) self.get_logger().info("Published " + str(len(detection_boxes)) + " detection boxes") - ros_ids = Int32MultiArray() - ros_ids.data = ids - if self.tracking_id_publisher is not None: + ids = [int(tracking_box.id) for tracking_box in tracking_boxes] + ros_ids = Int32MultiArray() + ros_ids.data = ids self.tracking_id_publisher.publish(ros_ids) self.get_logger().info("Published " + str(len(ids)) + " tracking ids") @@ -143,7 +142,7 @@ def callback(self, data): ] -def draw_predictions(frame, predictions: TrackingAnnotation, is_centered=False, is_flipped_xy=True): +def draw_predictions(frame, predictions: TrackingAnnotationList, is_centered=False, is_flipped_xy=True): global colors w, h, _ = frame.shape @@ -192,13 +191,13 @@ def main( type=str, default="/opendr/dataset_image") parser.add_argument("-od", "--output_detection_topic", help="Output detections topic", - type=str, default="/opendr/detection") + type=str, default="/opendr/deep_sort_detection") parser.add_argument("-ot", "--output_tracking_id_topic", help="Output detections topic", - type=str, default="/opendr/tracking_id") + type=str, default="/opendr/deep_sort_tracking_id") parser.add_argument("-oi", "--output_image_topic", help="Output detections topic", - type=str, default="/opendr/image_annotated") + type=str, default="/opendr/deep_sort_image_annotated") parser.add_argument("--device", help="Device to use, either \"cpu\" or \"cuda\", defaults to \"cuda\"", type=str, default="cuda", choices=["cuda", "cpu"]) args = parser.parse_args() @@ -230,8 +229,8 @@ def main( model_name=args.model_name, input_image_topic=args.input_image_topic, temp_dir=args.temp_dir, - output_detection_topic=args.output_detection_topic, - output_tracking_id_topic=args.output_tracking_id_topic, + output_detection_topic=args.output_detection_topic if args.output_detection_topic != "None" else None, + output_tracking_id_topic=args.output_tracking_id_topic if args.output_tracking_id_topic != "None" else None, output_image_topic=args.output_image_topic if args.output_image_topic != "None" else None, ) rclpy.spin(deep_sort_node) From b7d2340c7a1eab5199972a278ba1758a9c9a0325 Mon Sep 17 00:00:00 2001 From: Illia Oleksiienko Date: Wed, 9 Nov 2022 15:07:13 +0000 Subject: [PATCH 20/44] Fix fairmot conditions --- .../object_tracking_2d_fair_mot_node.py | 48 +++++++++---------- 1 file changed, 24 insertions(+), 24 deletions(-) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py index 83c20c537a..4410d56d92 100755 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py @@ -17,7 +17,7 @@ import argparse import cv2 import os -from opendr.engine.target import TrackingAnnotation +from opendr.engine.target import TrackingAnnotationList import rclpy from rclpy.node import Node from vision_msgs.msg import Detection2DArray @@ -34,9 +34,9 @@ class ObjectTracking2DFairMotNode(Node): def __init__( self, input_image_topic="/usb_cam/image_raw", - output_detection_topic="/opendr/detection", - output_tracking_id_topic="/opendr/tracking_id", - output_image_topic="/opendr/image_annotated", + output_detection_topic="/opendr/fairmot_detection", + output_tracking_id_topic="/opendr/fairmot_tracking_id", + output_image_topic="/opendr/fairmot_image_annotated", device="cuda:0", model_name="fairmot_dla34", temp_dir="temp", @@ -74,12 +74,15 @@ def __init__( # Initialize OpenDR ROSBridge object self.bridge = ROS2Bridge() - self.detection_publisher = self.create_publisher( - Detection2DArray, output_detection_topic, 1 - ) - self.tracking_id_publisher = self.create_publisher( - Int32MultiArray, output_tracking_id_topic, 1 - ) + if output_detection_topic is not None: + self.detection_publisher = self.create_publisher( + Detection2DArray, output_detection_topic, 1 + ) + + if output_tracking_id_topic is not None: + self.tracking_id_publisher = self.create_publisher( + Int32MultiArray, output_tracking_id_topic, 1 + ) if output_image_topic is not None: self.output_image_publisher = self.create_publisher( @@ -108,19 +111,16 @@ def callback(self, data): self.output_image_publisher.publish(message) self.get_logger().info("Published annotated image") - detection_boxes = tracking_boxes.bounding_box_list() - ids = [tracking_box.id for tracking_box in tracking_boxes] - - # Convert detected boxes to ROS type and publish - ros_boxes = self.bridge.to_ros_boxes(detection_boxes) if self.detection_publisher is not None: + detection_boxes = tracking_boxes.bounding_box_list() + ros_boxes = self.bridge.to_ros_boxes(detection_boxes) self.detection_publisher.publish(ros_boxes) self.get_logger().info("Published " + str(len(detection_boxes)) + " detection boxes") - ros_ids = Int32MultiArray() - ros_ids.data = ids - if self.tracking_id_publisher is not None: + ids = [tracking_box.id for tracking_box in tracking_boxes] + ros_ids = Int32MultiArray() + ros_ids.data = ids self.tracking_id_publisher.publish(ros_ids) self.get_logger().info("Published " + str(len(ids)) + " tracking ids") @@ -135,7 +135,7 @@ def callback(self, data): ] -def draw_predictions(frame, predictions: TrackingAnnotation, is_centered=False, is_flipped_xy=True): +def draw_predictions(frame, predictions: TrackingAnnotationList, is_centered=False, is_flipped_xy=True): global colors w, h, _ = frame.shape @@ -184,13 +184,13 @@ def main( type=str, default="/opendr/dataset_image") parser.add_argument("-od", "--output_detection_topic", help="Output detections topic", - type=str, default="/opendr/detection") + type=str, default="/opendr/fairmot_detection") parser.add_argument("-ot", "--output_tracking_id_topic", help="Output detections topic", - type=str, default="/opendr/tracking_id") + type=str, default="/opendr/fairmot_tracking_id") parser.add_argument("-oi", "--output_image_topic", help="Output detections topic", - type=str, default="/opendr/image_annotated") + type=str, default="/opendr/fairmot_image_annotated") parser.add_argument("--device", help="Device to use, either \"cpu\" or \"cuda\", defaults to \"cuda\"", type=str, default="cuda", choices=["cuda", "cpu"]) args = parser.parse_args() @@ -213,8 +213,8 @@ def main( model_name=args.model_name, input_image_topic=args.input_image_topic, temp_dir=args.temp_dir, - output_detection_topic=args.output_detection_topic, - output_tracking_id_topic=args.output_tracking_id_topic, + output_detection_topic=args.output_detection_topic if args.output_detection_topic != "None" else None, + output_tracking_id_topic=args.output_tracking_id_topic if args.output_tracking_id_topic != "None" else None, output_image_topic=args.output_image_topic if args.output_image_topic != "None" else None, ) From 2cf5be4f1a2cd50ce067ed250d30f672bd3aad65 Mon Sep 17 00:00:00 2001 From: Illia Oleksiienko Date: Wed, 9 Nov 2022 15:16:18 +0000 Subject: [PATCH 21/44] Fix ab3dmot docstrings --- .../opendr_perception/object_tracking_3d_ab3dmot_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py index 474afcf173..230aaa48f2 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py @@ -41,7 +41,7 @@ def __init__( :param detector: Learner that proides 3D object detections :type detector: Learner :param input_point_cloud_topic: Topic from which we are reading the input point cloud - :type input_image_topic: str + :type input_point_cloud_topic: str :param output_detection3d_topic: Topic to which we are publishing the annotations :type output_detection3d_topic: str :param output_tracking3d_id_topic: Topic to which we are publishing the tracking ids From c26449bbd5171844f3a2ab072da78615280c3d32 Mon Sep 17 00:00:00 2001 From: Illia Oleksiienko Date: Wed, 9 Nov 2022 18:10:04 +0000 Subject: [PATCH 22/44] Fix style errors --- .../opendr_perception/object_tracking_2d_deep_sort_node.py | 2 +- .../src/opendr_ros2_bridge/opendr_ros2_bridge/bridge.py | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py index dc26028c06..12f5fa1032 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py @@ -79,7 +79,7 @@ def __init__( self.learner.load(os.path.join(temp_dir, model_name), verbose=True) self.bridge = ROS2Bridge() - + if output_tracking_id_topic is not None: self.tracking_id_publisher = self.create_publisher( Int32MultiArray, output_tracking_id_topic, 1 diff --git a/projects/opendr_ws_2/src/opendr_ros2_bridge/opendr_ros2_bridge/bridge.py b/projects/opendr_ws_2/src/opendr_ros2_bridge/opendr_ros2_bridge/bridge.py index bd3f147020..b60c572334 100644 --- a/projects/opendr_ws_2/src/opendr_ros2_bridge/opendr_ros2_bridge/bridge.py +++ b/projects/opendr_ws_2/src/opendr_ros2_bridge/opendr_ros2_bridge/bridge.py @@ -24,13 +24,13 @@ from vision_msgs.msg import ( Detection2DArray, Detection2D, BoundingBox2D, ObjectHypothesisWithPose, Detection3D, Detection3DArray, BoundingBox3D as BoundingBox3DMsg, - Classification2D + Classification2D, ObjectHypothesis ) from shape_msgs.msg import Mesh, MeshTriangle from geometry_msgs.msg import ( Pose2D, Point32 as Point32Msg, Quaternion as QuaternionMsg, Pose as Pose3D, - Point, Pose2D + Point ) from opendr_ros2_messages.msg import OpenDRPose2D, OpenDRPose2DKeypoint, OpenDRPose3D, OpenDRPose3DKeypoint @@ -378,6 +378,7 @@ def to_ros_boxes_3d(self, boxes_3d): 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 From 417080786ec4c7f4fe6b647e6eadd140d44eb3cf Mon Sep 17 00:00:00 2001 From: Illia Oleksiienko Date: Tue, 22 Nov 2022 12:30:35 +0100 Subject: [PATCH 23/44] Apply suggestions from code review Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> --- .../object_tracking_2d_deep_sort_node.py | 20 +++++++------- .../object_tracking_2d_fair_mot_node.py | 26 +++++++++---------- .../object_tracking_3d_ab3dmot_node.py | 2 +- 3 files changed, 24 insertions(+), 24 deletions(-) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py index 12f5fa1032..4436042d26 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py @@ -48,11 +48,11 @@ def __init__( Creates a ROS2 Node for 2D object tracking :param detector: Learner to generate object detections :type detector: Learner - :param input_image_topic: Topic from which we are reading the input image - :type input_image_topic: str - :param output_image_topic: Topic to which we are publishing the annotated image (if None, we are not publishing + :param input_rgb_image_topic: Topic from which we are reading the input image + :type input_rgb_image_topic: str + :param output_rgb_image_topic: Topic to which we are publishing the annotated image (if None, we are not publishing annotated image) - :type output_image_topic: str + :type output_rgb_image_topic: str :param output_detection_topic: Topic to which we are publishing the detections :type output_detection_topic: str :param output_tracking_id_topic: Topic to which we are publishing the tracking ids @@ -87,7 +87,7 @@ def __init__( if output_image_topic is not None: self.output_image_publisher = self.create_publisher( - ROS_Image, output_image_topic, 1 + ROS_Image, output_rgb_image_topic, 1 ) if output_detection_topic is not None: @@ -95,7 +95,7 @@ def __init__( Detection2DArray, output_detection_topic, 1 ) - self.create_subscription(ROS_Image, input_image_topic, self.callback, 1) + self.create_subscription(ROS_Image, input_rgb_image_topic, self.callback, 1) def callback(self, data): """ @@ -186,7 +186,7 @@ def main( type=str, default="deep_sort") parser.add_argument("-t", "--temp_dir", help="Path to a temp dir with models", type=str, default="temp") - parser.add_argument("-i", "--input_image_topic", + parser.add_argument("-i", "--input_rgb_image_topic", help="Input Image topic provided by either an image_dataset_node, webcam or any other image node", type=str, default="/opendr/dataset_image") parser.add_argument("-od", "--output_detection_topic", @@ -195,7 +195,7 @@ def main( parser.add_argument("-ot", "--output_tracking_id_topic", help="Output detections topic", type=str, default="/opendr/deep_sort_tracking_id") - parser.add_argument("-oi", "--output_image_topic", + parser.add_argument("-oi", "--output_rgb_image_topic", help="Output detections topic", type=str, default="/opendr/deep_sort_image_annotated") parser.add_argument("--device", help="Device to use, either \"cpu\" or \"cuda\", defaults to \"cuda\"", @@ -227,11 +227,11 @@ def main( detector=detection_learner, device=device, model_name=args.model_name, - input_image_topic=args.input_image_topic, + input_rgb_image_topic=args.input_rgb_image_topic, temp_dir=args.temp_dir, output_detection_topic=args.output_detection_topic if args.output_detection_topic != "None" else None, output_tracking_id_topic=args.output_tracking_id_topic if args.output_tracking_id_topic != "None" else None, - output_image_topic=args.output_image_topic if args.output_image_topic != "None" else None, + output_rgb_image_topic=args.output_rgb_image_topic if args.output_rgb_image_topic != "None" else None, ) rclpy.spin(deep_sort_node) # Destroy the node explicitly diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py index 4410d56d92..7f92fa8335 100755 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py @@ -33,21 +33,21 @@ class ObjectTracking2DFairMotNode(Node): def __init__( self, - input_image_topic="/usb_cam/image_raw", + input_rgb_image_topic="image_raw", output_detection_topic="/opendr/fairmot_detection", output_tracking_id_topic="/opendr/fairmot_tracking_id", - output_image_topic="/opendr/fairmot_image_annotated", + output_rgb_image_topic="/opendr/fairmot_image_annotated", device="cuda:0", model_name="fairmot_dla34", temp_dir="temp", ): """ Creates a ROS Node for 2D object tracking - :param input_image_topic: Topic from which we are reading the input image - :type input_image_topic: str - :param output_image_topic: Topic to which we are publishing the annotated image (if None, we are not publishing + :param input_rgb_image_topic: Topic from which we are reading the input image + :type input_rgb_image_topic: str + :param output_rgb_image_topic: Topic to which we are publishing the annotated image (if None, we are not publishing annotated image) - :type output_image_topic: str + :type output_rgb_image_topic: str :param output_detection_topic: Topic to which we are publishing the detections :type output_detection_topic: str :param output_tracking_id_topic: Topic to which we are publishing the tracking ids @@ -84,12 +84,12 @@ def __init__( Int32MultiArray, output_tracking_id_topic, 1 ) - if output_image_topic is not None: + if output_rgb_image_topic is not None: self.output_image_publisher = self.create_publisher( - ROS_Image, output_image_topic, 1 + ROS_Image, output_rgb_image_topic, 1 ) - self.create_subscription(ROS_Image, input_image_topic, self.callback, 1) + self.create_subscription(ROS_Image, input_rgb_image_topic, self.callback, 1) def callback(self, data): """ @@ -179,7 +179,7 @@ def main( type=str, default="fairmot_dla34") parser.add_argument("-t", "--temp_dir", help="Path to a temp dir with models", type=str, default="temp") - parser.add_argument("-i", "--input_image_topic", + parser.add_argument("-i", "--input_rgb_image_topic", help="Input Image topic provided by either an image_dataset_node, webcam or any other image node", type=str, default="/opendr/dataset_image") parser.add_argument("-od", "--output_detection_topic", @@ -188,7 +188,7 @@ def main( parser.add_argument("-ot", "--output_tracking_id_topic", help="Output detections topic", type=str, default="/opendr/fairmot_tracking_id") - parser.add_argument("-oi", "--output_image_topic", + parser.add_argument("-oi", "--output_rgb_image_topic", help="Output detections topic", type=str, default="/opendr/fairmot_image_annotated") parser.add_argument("--device", help="Device to use, either \"cpu\" or \"cuda\", defaults to \"cuda\"", @@ -211,11 +211,11 @@ def main( fair_mot_node = ObjectTracking2DFairMotNode( device=device, model_name=args.model_name, - input_image_topic=args.input_image_topic, + input_rgb_image_topic=args.input_rgb_image_topic, temp_dir=args.temp_dir, output_detection_topic=args.output_detection_topic if args.output_detection_topic != "None" else None, output_tracking_id_topic=args.output_tracking_id_topic if args.output_tracking_id_topic != "None" else None, - output_image_topic=args.output_image_topic if args.output_image_topic != "None" else None, + output_rgb_image_topic=args.output_rgb_image_topic if args.output_rgb_image_topic != "None" else None, ) rclpy.spin(fair_mot_node) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py index 230aaa48f2..8ac6a93e2f 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py @@ -38,7 +38,7 @@ def __init__( ): """ Creates a ROS2 Node for 3D object tracking - :param detector: Learner that proides 3D object detections + :param detector: Learner that provides 3D object detections :type detector: Learner :param input_point_cloud_topic: Topic from which we are reading the input point cloud :type input_point_cloud_topic: str From f915282a35ee23a7c957fe491741ecc3bbcd8d76 Mon Sep 17 00:00:00 2001 From: Illia Oleksiienko Date: Tue, 22 Nov 2022 14:31:37 +0100 Subject: [PATCH 24/44] Update projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> --- .../opendr_perception/object_tracking_2d_deep_sort_node.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py index 4436042d26..f941058bf3 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py @@ -36,10 +36,10 @@ class ObjectTracking2DDeepSortNode(Node): def __init__( self, detector: Learner, - input_image_topic="/usb_cam/image_raw", + input_rgb_image_topic="image_raw", output_detection_topic="/opendr/deep_sort_detection", output_tracking_id_topic="/opendr/deep_sort_tracking_id", - output_image_topic="/opendr/deep_sort_image_annotated", + output_rgb_image_topic="/opendr/deep_sort_image_annotated", device="cuda:0", model_name="deep_sort", temp_dir="temp", From a67f72ebcf87834b26a62e0fa691ac2866e68f5d Mon Sep 17 00:00:00 2001 From: Illia Oleksiienko Date: Tue, 22 Nov 2022 13:31:55 +0000 Subject: [PATCH 25/44] Fix parameter names --- .../opendr_perception/image_dataset_node.py | 10 +++++----- .../object_tracking_2d_deep_sort_node.py | 6 +++--- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/image_dataset_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/image_dataset_node.py index 07e863a0f9..e788dbbec9 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/image_dataset_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/image_dataset_node.py @@ -27,7 +27,7 @@ class ImageDatasetNode(Node): def __init__( self, dataset: DatasetIterator, - output_image_topic="/opendr/dataset_image", + output_rgb_image_topic="/opendr/dataset_image", data_fps=10, ): """ @@ -42,7 +42,7 @@ def __init__( self.sample_index = 0 self.output_image_publisher = self.create_publisher( - ROS_Image, output_image_topic, 1 + ROS_Image, output_rgb_image_topic, 1 ) def timer_callback(self): @@ -73,7 +73,7 @@ def main( "datasets", "splits", "nano_mot20.train" ) ) - parser.add_argument("-o", "--output_image_topic", help="Topic name to upload the data", + parser.add_argument("-o", "--output_rgb_image_topic", help="Topic name to upload the data", type=str, default="/opendr/dataset_image") parser.add_argument("-f", "--fps", help="Data FPS", type=float, default=30) @@ -81,7 +81,7 @@ def main( dataset_path = args.dataset_path mot20_subsets_path = args.mot20_subsets_path - output_image_topic = args.output_image_topic + output_rgb_image_topic = args.output_rgb_image_topic data_fps = args.fps if not os.path.exists(dataset_path): @@ -98,7 +98,7 @@ def main( ) dataset_node = ImageDatasetNode( dataset, - output_image_topic=output_image_topic, + output_rgb_image_topic=output_rgb_image_topic, data_fps=data_fps, ) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py index 4436042d26..673f313cff 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py @@ -36,10 +36,10 @@ class ObjectTracking2DDeepSortNode(Node): def __init__( self, detector: Learner, - input_image_topic="/usb_cam/image_raw", + input_rgb_image_topic="/usb_cam/image_raw", output_detection_topic="/opendr/deep_sort_detection", output_tracking_id_topic="/opendr/deep_sort_tracking_id", - output_image_topic="/opendr/deep_sort_image_annotated", + output_rgb_image_topic="/opendr/deep_sort_image_annotated", device="cuda:0", model_name="deep_sort", temp_dir="temp", @@ -85,7 +85,7 @@ def __init__( Int32MultiArray, output_tracking_id_topic, 1 ) - if output_image_topic is not None: + if output_rgb_image_topic is not None: self.output_image_publisher = self.create_publisher( ROS_Image, output_rgb_image_topic, 1 ) From 093e7049d7f437e2343893758ac6de05596f9374 Mon Sep 17 00:00:00 2001 From: Illia Oleksiienko Date: Mon, 19 Dec 2022 15:11:47 +0000 Subject: [PATCH 26/44] Fix deep sort inference --- .../opendr_perception/object_tracking_2d_deep_sort_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py index afa955e8ff..b98181ba45 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py @@ -108,7 +108,7 @@ def callback(self, data): image = self.bridge.from_ros_image(data, encoding="bgr8") detection_boxes = self.detector.infer(image) image_with_detections = ImageWithDetections(image.numpy(), detection_boxes) - tracking_boxes = self.learner.infer(image_with_detections) + tracking_boxes = self.learner.infer(image_with_detections, swap_left_top=False) if self.output_image_publisher is not None: frame = image.opencv() From e5083cc8ef27fab8094063268d5a292d7c7739d3 Mon Sep 17 00:00:00 2001 From: Illia Oleksiienko Date: Mon, 19 Dec 2022 15:29:25 +0000 Subject: [PATCH 27/44] Fix deep sort no detections fail --- .../opendr_perception/object_tracking_2d_deep_sort_node.py | 1 + .../object_tracking_2d/deep_sort/algorithm/deep_sort_tracker.py | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py index b98181ba45..0a53229035 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py @@ -109,6 +109,7 @@ def callback(self, data): detection_boxes = self.detector.infer(image) image_with_detections = ImageWithDetections(image.numpy(), detection_boxes) tracking_boxes = self.learner.infer(image_with_detections, swap_left_top=False) + detection_boxes = tracking_boxes.bounding_box_list() if self.output_image_publisher is not None: frame = image.opencv() diff --git a/src/opendr/perception/object_tracking_2d/deep_sort/algorithm/deep_sort_tracker.py b/src/opendr/perception/object_tracking_2d/deep_sort/algorithm/deep_sort_tracker.py index e3b67314c2..559139c597 100644 --- a/src/opendr/perception/object_tracking_2d/deep_sort/algorithm/deep_sort_tracker.py +++ b/src/opendr/perception/object_tracking_2d/deep_sort/algorithm/deep_sort_tracker.py @@ -73,7 +73,7 @@ def infer(self, imageWithDetections: ImageWithDetections, frame_id=None, swap_le cls_conf.append(detection.confidence) cls_ids.append(detection.name) - bbox_xywh = np.array(bbox_xywh) + bbox_xywh = np.array(bbox_xywh).reshape(-1, 4) cls_conf = np.array(cls_conf) cls_ids = np.array(cls_ids) From 8c45aa2c7aac8aafd800f5683e22a9add3d01303 Mon Sep 17 00:00:00 2001 From: tsampazk <27914645+tsampazk@users.noreply.github.com> Date: Tue, 20 Dec 2022 12:29:14 +0200 Subject: [PATCH 28/44] Removed depend on rclcpp for data generation package --- projects/opendr_ws_2/src/opendr_data_generation/package.xml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/projects/opendr_ws_2/src/opendr_data_generation/package.xml b/projects/opendr_ws_2/src/opendr_data_generation/package.xml index 24193dd55b..a7f13acc60 100644 --- a/projects/opendr_ws_2/src/opendr_data_generation/package.xml +++ b/projects/opendr_ws_2/src/opendr_data_generation/package.xml @@ -6,8 +6,7 @@ OpenDR's ROS2 nodes for data generation package tefas Apache License v2.0 - - rclcpp> + sensor_msgs rclpy From d571eef01317613aa63965cfc810ca5f60b33850 Mon Sep 17 00:00:00 2001 From: tsampazk <27914645+tsampazk@users.noreply.github.com> Date: Tue, 20 Dec 2022 12:29:23 +0200 Subject: [PATCH 29/44] Fix bridge imports --- .../opendr_perception/opendr_perception/image_dataset_node.py | 2 +- .../opendr_perception/object_detection_3d_voxel_node.py | 2 +- .../opendr_perception/object_tracking_2d_deep_sort_node.py | 2 +- .../opendr_perception/object_tracking_2d_fair_mot_node.py | 2 +- .../opendr_perception/object_tracking_3d_ab3dmot_node.py | 2 +- .../opendr_perception/point_cloud_dataset_node.py | 2 +- 6 files changed, 6 insertions(+), 6 deletions(-) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/image_dataset_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/image_dataset_node.py index e788dbbec9..0835e959e7 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/image_dataset_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/image_dataset_node.py @@ -18,7 +18,7 @@ import rclpy from rclpy.node import Node from sensor_msgs.msg import Image as ROS_Image -from opendr_ros2_bridge import ROS2Bridge +from opendr_bridge import ROS2Bridge from opendr.engine.datasets import DatasetIterator from opendr.perception.object_tracking_2d import MotDataset, RawMotDatasetIterator diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py index 6ca4aaeafe..4e63d236f7 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py @@ -20,7 +20,7 @@ from rclpy.node import Node from vision_msgs.msg import Detection3DArray from sensor_msgs.msg import PointCloud as ROS_PointCloud -from opendr_ros2_bridge import ROS2Bridge +from opendr_bridge import ROS2Bridge from opendr.perception.object_detection_3d import VoxelObjectDetection3DLearner diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py index 0a53229035..ac1572a9a8 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py @@ -23,7 +23,7 @@ from vision_msgs.msg import Detection2DArray from std_msgs.msg import Int32MultiArray from sensor_msgs.msg import Image as ROS_Image -from opendr_ros2_bridge import ROS2Bridge +from opendr_bridge import ROS2Bridge from opendr.engine.learners import Learner from opendr.perception.object_tracking_2d import ( ObjectTracking2DDeepSortLearner, diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py index 7f92fa8335..fdc6da3d81 100755 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py @@ -23,7 +23,7 @@ from vision_msgs.msg import Detection2DArray from std_msgs.msg import Int32MultiArray from sensor_msgs.msg import Image as ROS_Image -from opendr_ros2_bridge import ROS2Bridge +from opendr_bridge import ROS2Bridge from opendr.perception.object_tracking_2d import ( ObjectTracking2DFairMotLearner, ) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py index 8ac6a93e2f..d5947e8e98 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py @@ -22,7 +22,7 @@ from vision_msgs.msg import Detection3DArray from std_msgs.msg import Int32MultiArray from sensor_msgs.msg import PointCloud as ROS_PointCloud -from opendr_ros2_bridge import ROS2Bridge +from opendr_bridge import ROS2Bridge from opendr.perception.object_tracking_3d import ObjectTracking3DAb3dmotLearner from opendr.perception.object_detection_3d import VoxelObjectDetection3DLearner diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py index aa1d6d710a..116b9b149f 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py @@ -18,7 +18,7 @@ from sensor_msgs.msg import PointCloud as ROS_PointCloud import rclpy from rclpy.node import Node -from opendr_ros2_bridge import ROS2Bridge +from opendr_bridge import ROS2Bridge from opendr.engine.datasets import DatasetIterator from opendr.perception.object_detection_3d import KittiDataset, LabeledPointCloudsDatasetIterator From a3858c4b62b84dd3462845935176598938d971cb Mon Sep 17 00:00:00 2001 From: tsampazk <27914645+tsampazk@users.noreply.github.com> Date: Tue, 20 Dec 2022 12:54:00 +0200 Subject: [PATCH 30/44] Fix interface import --- projects/opendr_ws_2/src/opendr_bridge/opendr_bridge/bridge.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/projects/opendr_ws_2/src/opendr_bridge/opendr_bridge/bridge.py b/projects/opendr_ws_2/src/opendr_bridge/opendr_bridge/bridge.py index be6b32d95c..43ae87b836 100644 --- a/projects/opendr_ws_2/src/opendr_bridge/opendr_bridge/bridge.py +++ b/projects/opendr_ws_2/src/opendr_bridge/opendr_bridge/bridge.py @@ -32,7 +32,7 @@ Quaternion as QuaternionMsg, Pose as Pose3D, Point ) -from opendr_ros2_messages.msg import OpenDRPose2D, OpenDRPose2DKeypoint, OpenDRPose3D, OpenDRPose3DKeypoint +from opendr_interface.msg import OpenDRPose2D, OpenDRPose2DKeypoint, OpenDRPose3D, OpenDRPose3DKeypoint class ROS2Bridge: From f4e313d153699e9f72a3665811308f171c49c215 Mon Sep 17 00:00:00 2001 From: tsampazk <27914645+tsampazk@users.noreply.github.com> Date: Tue, 20 Dec 2022 14:21:49 +0200 Subject: [PATCH 31/44] Minor fixes on voxel node based on ros1 voxel node --- .../object_detection_3d_voxel_node.py | 28 +++++++++---------- 1 file changed, 13 insertions(+), 15 deletions(-) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py index 4e63d236f7..06a8c89f3e 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py @@ -28,7 +28,7 @@ class ObjectDetection3DVoxelNode(Node): def __init__( self, input_point_cloud_topic="/opendr/dataset_point_cloud", - output_detection3d_topic="/opendr/detection3d", + output_detection3d_topic="/opendr/objects3d", device="cuda:0", model_name="tanet_car_xyres_16", model_config_path=os.path.join( @@ -52,7 +52,7 @@ def __init__( :type temp_dir: str """ - super().__init__('object_detection_3d_voxel_node') + super().__init__('opendr_object_detection_3d_voxel_node') self.get_logger().info("Using model_name: {}".format(model_name)) @@ -73,7 +73,7 @@ def __init__( self.create_subscription(ROS_PointCloud, input_point_cloud_topic, self.callback, 1) - self.get_logger().info("Ready to listen") + self.get_logger().info("Object Detection 3D Voxel Node initialized.") def callback(self, data): """ @@ -89,14 +89,20 @@ def callback(self, data): # Convert detected boxes to ROS type and publish ros_boxes = self.bridge.to_ros_boxes_3d(detection_boxes) self.detection_publisher.publish(ros_boxes) - self.get_logger().info("Published " + str(len(detection_boxes)) + " detection boxes") -def main( - args=None, -): +def main(args=None): rclpy.init(args=args) + parser = argparse.ArgumentParser() + parser.add_argument("-i", "--input_point_cloud_topic", + help="Point Cloud topic provided by either a point_cloud_dataset_node or any other 3D Point Cloud Node", + type=str, default="/opendr/dataset_point_cloud") + parser.add_argument("-o", "--output_detection3d_topic", + help="Output detections topic", + type=str, default="/opendr/objects3d") + parser.add_argument("--device", help="Device to use, either \"cpu\" or \"cuda\", defaults to \"cuda\"", + type=str, default="cuda", choices=["cuda", "cpu"]) parser.add_argument("-n", "--model_name", help="Name of the trained model", type=str, default="tanet_car_xyres_16") parser.add_argument( @@ -109,14 +115,6 @@ def main( ) parser.add_argument("-t", "--temp_dir", help="Path to a temp dir with models", type=str, default="temp") - parser.add_argument("-i", "--input_point_cloud_topic", - help="Point Cloud topic provided by either a point_cloud_dataset_node or any other 3D Point Cloud Node", - type=str, default="/opendr/dataset_point_cloud") - parser.add_argument("-o", "--output_detection3d_topic", - help="Output detections topic", - type=str, default="/opendr/detection3d") - parser.add_argument("--device", help="Device to use, either \"cpu\" or \"cuda\", defaults to \"cuda\"", - type=str, default="cuda", choices=["cuda", "cpu"]) args = parser.parse_args() try: From cddf597e866c01caec7f49bc333a516588c20efc Mon Sep 17 00:00:00 2001 From: tsampazk <27914645+tsampazk@users.noreply.github.com> Date: Tue, 20 Dec 2022 14:23:45 +0200 Subject: [PATCH 32/44] Future fix for embedded devices --- .../opendr_perception/object_detection_3d_voxel_node.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py index 06a8c89f3e..67a28d1d97 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py @@ -32,7 +32,7 @@ def __init__( device="cuda:0", model_name="tanet_car_xyres_16", model_config_path=os.path.join( - "..", "..", "src", "opendr", "perception", "object_detection_3d", + "$OPENDR_HOME", "src", "opendr", "perception", "object_detection_3d", "voxel_object_detection_3d", "second_detector", "configs", "tanet", "ped_cycle", "test_short.proto" ), @@ -108,7 +108,7 @@ def main(args=None): parser.add_argument( "-c", "--model_config_path", help="Path to a model .proto config", type=str, default=os.path.join( - "..", "..", "src", "opendr", "perception", "object_detection_3d", + "$OPENDR_HOME", "src", "opendr", "perception", "object_detection_3d", "voxel_object_detection_3d", "second_detector", "configs", "tanet", "car", "xyres_16.proto" ) From c69bc09cff5e07b0f855025db58feb06d5e6142f Mon Sep 17 00:00:00 2001 From: tsampazk <27914645+tsampazk@users.noreply.github.com> Date: Tue, 20 Dec 2022 14:52:09 +0200 Subject: [PATCH 33/44] Fixes from ros1 --- .../object_tracking_2d_deep_sort_node.py | 44 ++++++++++--------- 1 file changed, 23 insertions(+), 21 deletions(-) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py index ac1572a9a8..5d840d94ef 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py @@ -24,7 +24,6 @@ from std_msgs.msg import Int32MultiArray from sensor_msgs.msg import Image as ROS_Image from opendr_bridge import ROS2Bridge -from opendr.engine.learners import Learner from opendr.perception.object_tracking_2d import ( ObjectTracking2DDeepSortLearner, ObjectTracking2DFairMotLearner @@ -35,11 +34,11 @@ class ObjectTracking2DDeepSortNode(Node): def __init__( self, - detector: Learner, + detector=None, input_rgb_image_topic="image_raw", - output_detection_topic="/opendr/deep_sort_detection", - output_tracking_id_topic="/opendr/deep_sort_tracking_id", - output_rgb_image_topic="/opendr/deep_sort_image_annotated", + output_detection_topic="/opendr/objects", + output_tracking_id_topic="/opendr/objects_tracking_id", + output_rgb_image_topic="/opendr/image_objects_annotated", device="cuda:0", model_name="deep_sort", temp_dir="temp", @@ -183,24 +182,27 @@ def main( ): rclpy.init(args=args) parser = argparse.ArgumentParser() - parser.add_argument("-n", "--model_name", help="Name of the trained model", - type=str, default="deep_sort") - parser.add_argument("-t", "--temp_dir", help="Path to a temp dir with models", - type=str, default="temp") parser.add_argument("-i", "--input_rgb_image_topic", help="Input Image topic provided by either an image_dataset_node, webcam or any other image node", - type=str, default="/opendr/dataset_image") - parser.add_argument("-od", "--output_detection_topic", - help="Output detections topic", - type=str, default="/opendr/deep_sort_detection") - parser.add_argument("-ot", "--output_tracking_id_topic", + type=str, default="/image_raw") + parser.add_argument("-o", "--output_rgb_image_topic", + help="Output annotated image topic with a visualization of detections and their ids", + type=lambda value: value if value.lower() != "none" else None, + default="/opendr/image_objects_annotated") + parser.add_argument("-d", "--detections_topic", help="Output detections topic", - type=str, default="/opendr/deep_sort_tracking_id") - parser.add_argument("-oi", "--output_rgb_image_topic", - help="Output detections topic", - type=str, default="/opendr/deep_sort_image_annotated") + type=lambda value: value if value.lower() != "none" else None, + default="/opendr/objects") + parser.add_argument("-t", "--tracking_id_topic", + help="Output tracking ids topic with the same element count as in output_detection_topic", + type=lambda value: value if value.lower() != "none" else None, + default="/opendr/objects_tracking_id") parser.add_argument("--device", help="Device to use, either \"cpu\" or \"cuda\", defaults to \"cuda\"", type=str, default="cuda", choices=["cuda", "cpu"]) + parser.add_argument("-n", "--model_name", help="Name of the trained model", + type=str, default="deep_sort", choices=["deep_sort"]) + parser.add_argument("-td", "--temp_dir", help="Path to a temporary directory with models", + type=str, default="temp") args = parser.parse_args() try: @@ -230,9 +232,9 @@ def main( model_name=args.model_name, input_rgb_image_topic=args.input_rgb_image_topic, temp_dir=args.temp_dir, - output_detection_topic=args.output_detection_topic if args.output_detection_topic != "None" else None, - output_tracking_id_topic=args.output_tracking_id_topic if args.output_tracking_id_topic != "None" else None, - output_rgb_image_topic=args.output_rgb_image_topic if args.output_rgb_image_topic != "None" else None, + output_detection_topic=args.detections_topic, + output_tracking_id_topic=args.tracking_id_topic, + output_rgb_image_topic=args.output_rgb_image_topic, ) rclpy.spin(deep_sort_node) # Destroy the node explicitly From 14fecc8fc900ca92e59cdfa65fbdb633d8bfa924 Mon Sep 17 00:00:00 2001 From: tsampazk <27914645+tsampazk@users.noreply.github.com> Date: Tue, 20 Dec 2022 14:53:14 +0200 Subject: [PATCH 34/44] Minor formatting --- .../opendr_perception/object_tracking_2d_deep_sort_node.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py index 5d840d94ef..9a2fe0f80d 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py @@ -177,10 +177,9 @@ def draw_predictions(frame, predictions: TrackingAnnotationList, is_centered=Fal ) -def main( - args=None, -): +def main(args=None): rclpy.init(args=args) + parser = argparse.ArgumentParser() parser.add_argument("-i", "--input_rgb_image_topic", help="Input Image topic provided by either an image_dataset_node, webcam or any other image node", From 0b1e12c13cc23fe7fb7b0e7a99734b5e55575ec7 Mon Sep 17 00:00:00 2001 From: tsampazk <27914645+tsampazk@users.noreply.github.com> Date: Tue, 20 Dec 2022 14:55:09 +0200 Subject: [PATCH 35/44] Matched arguments with ros1 --- .../object_detection_3d_voxel_node.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py index 67a28d1d97..fc9660db67 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py @@ -28,7 +28,7 @@ class ObjectDetection3DVoxelNode(Node): def __init__( self, input_point_cloud_topic="/opendr/dataset_point_cloud", - output_detection3d_topic="/opendr/objects3d", + detections_topic="/opendr/objects3d", device="cuda:0", model_name="tanet_car_xyres_16", model_config_path=os.path.join( @@ -42,8 +42,8 @@ def __init__( Creates a ROS2 Node for 3D object detection :param input_point_cloud_topic: Topic from which we are reading the input point cloud :type input_point_cloud_topic: str - :param output_detection3d_topic: Topic to which we are publishing the annotations - :type output_detection3d_topic: str + :param detections_topic: Topic to which we are publishing the annotations + :type detections_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str :param model_name: the pretrained model to download or a trained model in temp_dir @@ -68,7 +68,7 @@ def __init__( self.bridge = ROS2Bridge() self.detection_publisher = self.create_publisher( - Detection3DArray, output_detection3d_topic, 1 + Detection3DArray, detections_topic, 1 ) self.create_subscription(ROS_PointCloud, input_point_cloud_topic, self.callback, 1) @@ -98,7 +98,7 @@ def main(args=None): parser.add_argument("-i", "--input_point_cloud_topic", help="Point Cloud topic provided by either a point_cloud_dataset_node or any other 3D Point Cloud Node", type=str, default="/opendr/dataset_point_cloud") - parser.add_argument("-o", "--output_detection3d_topic", + parser.add_argument("-d", "--detections_topic", help="Output detections topic", type=str, default="/opendr/objects3d") parser.add_argument("--device", help="Device to use, either \"cpu\" or \"cuda\", defaults to \"cuda\"", @@ -136,7 +136,7 @@ def main(args=None): model_config_path=args.model_config_path, input_point_cloud_topic=args.input_point_cloud_topic, temp_dir=args.temp_dir, - output_detection3d_topic=args.output_detection3d_topic, + detections_topic=args.detections_topic, ) rclpy.spin(voxel_node) From 2a184302b0b0dbcc027f9d5dfcaa2cdff132f301 Mon Sep 17 00:00:00 2001 From: tsampazk <27914645+tsampazk@users.noreply.github.com> Date: Tue, 20 Dec 2022 14:56:53 +0200 Subject: [PATCH 36/44] Fixed node name --- .../opendr_perception/object_tracking_2d_deep_sort_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py index 9a2fe0f80d..2465d5f3d3 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py @@ -64,7 +64,7 @@ def __init__( :type temp_dir: str """ - super().__init__('object_tracking_2d_deep_sort_node') + super().__init__('opendr_object_tracking_2d_deep_sort_node') self.get_logger().info("Using model_name: {}".format(model_name)) From f3d9772808e0053cc2fc4fc9e982a3cd0333ad0c Mon Sep 17 00:00:00 2001 From: tsampazk <27914645+tsampazk@users.noreply.github.com> Date: Tue, 20 Dec 2022 14:58:54 +0200 Subject: [PATCH 37/44] Fixed docstring --- .../opendr_perception/object_detection_3d_voxel_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py index fc9660db67..b757ccf1dd 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py @@ -77,7 +77,7 @@ def __init__( def callback(self, data): """ - Callback that process the input data and publishes to the corresponding topics + Callback that processes the input data and publishes to the corresponding topics. :param data: input message :type data: sensor_msgs.msg.Image """ From 7fc8f24a2b2c6f6e57375856ed1a74d09a87ed0a Mon Sep 17 00:00:00 2001 From: tsampazk <27914645+tsampazk@users.noreply.github.com> Date: Tue, 20 Dec 2022 15:01:14 +0200 Subject: [PATCH 38/44] Some autoformatting --- .../object_detection_3d_voxel_node.py | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py index b757ccf1dd..4c3b883905 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py @@ -26,17 +26,17 @@ class ObjectDetection3DVoxelNode(Node): def __init__( - self, - input_point_cloud_topic="/opendr/dataset_point_cloud", - detections_topic="/opendr/objects3d", - device="cuda:0", - model_name="tanet_car_xyres_16", - model_config_path=os.path.join( - "$OPENDR_HOME", "src", "opendr", "perception", "object_detection_3d", - "voxel_object_detection_3d", "second_detector", "configs", "tanet", - "ped_cycle", "test_short.proto" - ), - temp_dir="temp", + self, + input_point_cloud_topic="/opendr/dataset_point_cloud", + detections_topic="/opendr/objects3d", + device="cuda:0", + model_name="tanet_car_xyres_16", + model_config_path=os.path.join( + "$OPENDR_HOME", "src", "opendr", "perception", "object_detection_3d", + "voxel_object_detection_3d", "second_detector", "configs", "tanet", + "ped_cycle", "test_short.proto" + ), + temp_dir="temp", ): """ Creates a ROS2 Node for 3D object detection From 0ade0b35adf236b854ba100e7bd1cb162a91a2f6 Mon Sep 17 00:00:00 2001 From: tsampazk <27914645+tsampazk@users.noreply.github.com> Date: Tue, 20 Dec 2022 15:01:28 +0200 Subject: [PATCH 39/44] Some fixes --- .../object_tracking_2d_deep_sort_node.py | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py index 2465d5f3d3..30b83a8b75 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py @@ -33,15 +33,15 @@ class ObjectTracking2DDeepSortNode(Node): def __init__( - self, - detector=None, - input_rgb_image_topic="image_raw", - output_detection_topic="/opendr/objects", - output_tracking_id_topic="/opendr/objects_tracking_id", - output_rgb_image_topic="/opendr/image_objects_annotated", - device="cuda:0", - model_name="deep_sort", - temp_dir="temp", + self, + detector=None, + input_rgb_image_topic="image_raw", + output_detection_topic="/opendr/objects", + output_tracking_id_topic="/opendr/objects_tracking_id", + output_rgb_image_topic="/opendr/image_objects_annotated", + device="cuda:0", + model_name="deep_sort", + temp_dir="temp", ): """ Creates a ROS2 Node for 2D object tracking @@ -98,7 +98,7 @@ def __init__( def callback(self, data): """ - Callback that process the input data and publishes to the corresponding topics + Callback that processes the input data and publishes to the corresponding topics. :param data: input message :type data: sensor_msgs.msg.Image """ @@ -108,7 +108,6 @@ def callback(self, data): detection_boxes = self.detector.infer(image) image_with_detections = ImageWithDetections(image.numpy(), detection_boxes) tracking_boxes = self.learner.infer(image_with_detections, swap_left_top=False) - detection_boxes = tracking_boxes.bounding_box_list() if self.output_image_publisher is not None: frame = image.opencv() @@ -120,6 +119,7 @@ def callback(self, data): self.get_logger().info("Published annotated image") if self.detection_publisher is not None: + detection_boxes = tracking_boxes.bounding_box_list() ros_boxes = self.bridge.to_ros_boxes(detection_boxes) self.detection_publisher.publish(ros_boxes) self.get_logger().info("Published " + str(len(detection_boxes)) + " detection boxes") From b1da73fa333111dab270ee04bde9ee5851d940cc Mon Sep 17 00:00:00 2001 From: tsampazk <27914645+tsampazk@users.noreply.github.com> Date: Tue, 20 Dec 2022 15:02:42 +0200 Subject: [PATCH 40/44] Various fixes to match ros1 node --- .../object_tracking_2d_fair_mot_node.py | 63 ++++++++++--------- 1 file changed, 32 insertions(+), 31 deletions(-) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py index fdc6da3d81..bcd30f68ac 100755 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py @@ -32,17 +32,17 @@ class ObjectTracking2DFairMotNode(Node): def __init__( - self, - input_rgb_image_topic="image_raw", - output_detection_topic="/opendr/fairmot_detection", - output_tracking_id_topic="/opendr/fairmot_tracking_id", - output_rgb_image_topic="/opendr/fairmot_image_annotated", - device="cuda:0", - model_name="fairmot_dla34", - temp_dir="temp", + self, + input_rgb_image_topic="image_raw", + output_rgb_image_topic="/opendr/image_objects_annotated", + output_detection_topic="/opendr/objects", + output_tracking_id_topic="/opendr/objects_tracking_id", + device="cuda:0", + model_name="fairmot_dla34", + temp_dir="temp", ): """ - Creates a ROS Node for 2D object tracking + Creates a ROS2 Node for 2D object tracking :param input_rgb_image_topic: Topic from which we are reading the input image :type input_rgb_image_topic: str :param output_rgb_image_topic: Topic to which we are publishing the annotated image (if None, we are not publishing @@ -60,9 +60,8 @@ def __init__( :type temp_dir: str """ - super().__init__('object_tracking_2d_fair_mot_node') + super().__init__('opendr_object_tracking_2d_fair_mot_node') - # # Initialize the face detector self.learner = ObjectTracking2DFairMotLearner( device=device, temp_path=temp_dir, ) @@ -93,7 +92,7 @@ def __init__( def callback(self, data): """ - Callback that process the input data and publishes to the corresponding topics + Callback that processes the input data and publishes to the corresponding topics. :param data: input message :type data: sensor_msgs.msg.Image """ @@ -170,29 +169,31 @@ def draw_predictions(frame, predictions: TrackingAnnotationList, is_centered=Fal ) -def main( - args=None, -): +def main(args=None): rclpy.init(args=args) + parser = argparse.ArgumentParser() - parser.add_argument("-n", "--model_name", help="Name of the trained model", - type=str, default="fairmot_dla34") - parser.add_argument("-t", "--temp_dir", help="Path to a temp dir with models", - type=str, default="temp") parser.add_argument("-i", "--input_rgb_image_topic", help="Input Image topic provided by either an image_dataset_node, webcam or any other image node", - type=str, default="/opendr/dataset_image") - parser.add_argument("-od", "--output_detection_topic", - help="Output detections topic", - type=str, default="/opendr/fairmot_detection") - parser.add_argument("-ot", "--output_tracking_id_topic", + type=str, default="/image_raw") + parser.add_argument("-o", "--output_rgb_image_topic", + help="Output annotated image topic with a visualization of detections and their ids", + type=lambda value: value if value.lower() != "none" else None, + default="/opendr/image_objects_annotated") + parser.add_argument("-d", "--detections_topic", help="Output detections topic", - type=str, default="/opendr/fairmot_tracking_id") - parser.add_argument("-oi", "--output_rgb_image_topic", - help="Output detections topic", - type=str, default="/opendr/fairmot_image_annotated") + type=lambda value: value if value.lower() != "none" else None, + default="/opendr/objects") + parser.add_argument("-t", "--tracking_id_topic", + help="Output tracking ids topic with the same element count as in output_detection_topic", + type=lambda value: value if value.lower() != "none" else None, + default="/opendr/objects_tracking_id") parser.add_argument("--device", help="Device to use, either \"cpu\" or \"cuda\", defaults to \"cuda\"", type=str, default="cuda", choices=["cuda", "cpu"]) + parser.add_argument("-n", "--model_name", help="Name of the trained model", + type=str, default="fairmot_dla34", choices=["fairmot_dla34"]) + parser.add_argument("-td", "--temp_dir", help="Path to a temporary directory with models", + type=str, default="temp") args = parser.parse_args() try: @@ -213,9 +214,9 @@ def main( model_name=args.model_name, input_rgb_image_topic=args.input_rgb_image_topic, temp_dir=args.temp_dir, - output_detection_topic=args.output_detection_topic if args.output_detection_topic != "None" else None, - output_tracking_id_topic=args.output_tracking_id_topic if args.output_tracking_id_topic != "None" else None, - output_rgb_image_topic=args.output_rgb_image_topic if args.output_rgb_image_topic != "None" else None, + output_detection_topic=args.detections_topic, + output_tracking_id_topic=args.tracking_id_topic, + output_rgb_image_topic=args.output_rgb_image_topic, ) rclpy.spin(fair_mot_node) From 01deac14f167ee044d2f5016661c3735ce45b486 Mon Sep 17 00:00:00 2001 From: tsampazk <27914645+tsampazk@users.noreply.github.com> Date: Tue, 20 Dec 2022 15:25:02 +0200 Subject: [PATCH 41/44] Fixes from ros1 --- .../object_tracking_3d_ab3dmot_node.py | 53 +++++++++---------- 1 file changed, 25 insertions(+), 28 deletions(-) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py index d5947e8e98..d79a229392 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py @@ -16,7 +16,6 @@ import torch import argparse import os -from opendr.engine.learners import Learner import rclpy from rclpy.node import Node from vision_msgs.msg import Detection3DArray @@ -30,7 +29,7 @@ class ObjectTracking3DAb3dmotNode(Node): def __init__( self, - detector: Learner, + detector=None, input_point_cloud_topic="/opendr/dataset_point_cloud", output_detection3d_topic="/opendr/detection3d", output_tracking3d_id_topic="/opendr/tracking3d_id", @@ -49,7 +48,7 @@ def __init__( :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str """ - super().__init__('object_tracking_3d_ab3dmot_node') + super().__init__('opendr_object_tracking_3d_ab3dmot_node') self.detector = detector self.learner = ObjectTracking3DAb3dmotLearner( @@ -71,11 +70,11 @@ def __init__( self.create_subscription(ROS_PointCloud, input_point_cloud_topic, self.callback, 1) - self.get_logger().info("Ready to listen") + self.get_logger().info("Object Tracking 3D Ab3dmot Node initialized.") def callback(self, data): """ - Callback that process the input data and publishes to the corresponding topics + Callback that processes the input data and publishes to the corresponding topics. :param data: input message :type data: sensor_msgs.msg.Image """ @@ -83,7 +82,6 @@ def callback(self, data): # Convert sensor_msgs.msg.Image into OpenDR Image point_cloud = self.bridge.from_ros_point_cloud(data) detection_boxes = self.detector.infer(point_cloud) - tracking_boxes = self.learner.infer(detection_boxes) # Convert detected boxes to ROS type and publish if self.detection_publisher is not None: @@ -92,6 +90,7 @@ def callback(self, data): self.get_logger().info("Published " + str(len(detection_boxes)) + " detection boxes") if self.tracking_id_publisher is not None: + tracking_boxes = self.learner.infer(detection_boxes) ids = [tracking_box.id for tracking_box in tracking_boxes] ros_ids = Int32MultiArray() ros_ids.data = ids @@ -99,42 +98,41 @@ def callback(self, data): self.get_logger().info("Published " + str(len(ids)) + " tracking ids") -def main( - args=None, -): +def main(args=None): rclpy.init(args=args) + parser = argparse.ArgumentParser() + parser.add_argument("-i", "--input_point_cloud_topic", + help="Point Cloud topic provided by either a point_cloud_dataset_node or any other 3D Point Cloud Node", + type=str, default="/opendr/dataset_point_cloud") + parser.add_argument("-d", "--detections_topic", + help="Output detections topic", + type=lambda value: value if value.lower() != "none" else None, default="/opendr/objects3d") + parser.add_argument("-t", "--tracking3d_id_topic", + help="Output tracking ids topic with the same element count as in output_detection_topic", + type=lambda value: value if value.lower() != "none" else None, default="/opendr/objects_tracking_id") + parser.add_argument("--device", help="Device to use, either \"cpu\" or \"cuda\", defaults to \"cuda\"", + type=str, default="cuda", choices=["cuda", "cpu"]) parser.add_argument("-dn", "--detector_model_name", help="Name of the trained model", - type=str, default="tanet_car_xyres_16") + type=str, default="tanet_car_xyres_16", choices=["tanet_car_xyres_16"]) parser.add_argument( "-dc", "--detector_model_config_path", help="Path to a model .proto config", type=str, default=os.path.join( - "..", "..", "src", "opendr", "perception", "object_detection_3d", + "$OPENDR_HOME", "src", "opendr", "perception", "object_detection_3d", "voxel_object_detection_3d", "second_detector", "configs", "tanet", "car", "xyres_16.proto" ) ) - parser.add_argument("-t", "--temp_dir", help="Path to a temp dir with models", + parser.add_argument("-td", "--temp_dir", help="Path to a temporary directory with models", type=str, default="temp") - parser.add_argument("-i", "--input_point_cloud_topic", - help="Point Cloud topic provided by either a point_cloud_dataset_node or any other 3D Point Cloud Node", - type=str, default="/opendr/dataset_point_cloud") - parser.add_argument("-od", "--output_detection3d_topic", - help="Output detections topic", - type=str, default="/opendr/detection3d") - parser.add_argument("-ot", "--output_tracking3d_id_topic", - help="Output tracking topic", - type=str, default="/opendr/tracking3d_id") - parser.add_argument("--device", help="Device to use, either \"cpu\" or \"cuda\", defaults to \"cuda\"", - type=str, default="cuda", choices=["cuda", "cpu"]) args = parser.parse_args() input_point_cloud_topic = args.input_point_cloud_topic detector_model_name = args.detector_model_name temp_dir = args.temp_dir detector_model_config_path = args.detector_model_config_path - output_detection3d_topic = args.output_detection3d_topic - output_tracking3d_id_topic = args.output_tracking3d_id_topic + output_detection3d_topic = args.detections_topic + output_tracking3d_id_topic = args.tracking3d_id_topic try: if args.device == "cuda" and torch.cuda.is_available(): @@ -159,13 +157,12 @@ def main( detector.load(os.path.join(temp_dir, detector_model_name), verbose=True) - # created node object ab3dmot_node = ObjectTracking3DAb3dmotNode( detector=detector, device=device, input_point_cloud_topic=input_point_cloud_topic, - output_detection3d_topic=output_detection3d_topic if output_detection3d_topic != "None" else None, - output_tracking3d_id_topic=output_tracking3d_id_topic if output_tracking3d_id_topic != "None" else None, + output_detection3d_topic=output_detection3d_topic, + output_tracking3d_id_topic=output_tracking3d_id_topic, ) rclpy.spin(ab3dmot_node) From 2db187028aee3e3193e1cd059425cd3c4c642cdc Mon Sep 17 00:00:00 2001 From: tsampazk <27914645+tsampazk@users.noreply.github.com> Date: Tue, 20 Dec 2022 15:32:58 +0200 Subject: [PATCH 42/44] Autoformatting --- .../object_tracking_3d_ab3dmot_node.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py index d79a229392..c0cfb95124 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py @@ -28,12 +28,12 @@ class ObjectTracking3DAb3dmotNode(Node): def __init__( - self, - detector=None, - input_point_cloud_topic="/opendr/dataset_point_cloud", - output_detection3d_topic="/opendr/detection3d", - output_tracking3d_id_topic="/opendr/tracking3d_id", - device="cuda:0", + self, + detector=None, + input_point_cloud_topic="/opendr/dataset_point_cloud", + output_detection3d_topic="/opendr/detection3d", + output_tracking3d_id_topic="/opendr/tracking3d_id", + device="cuda:0", ): """ Creates a ROS2 Node for 3D object tracking From 61feb316a77eb3146c1f66ca690310284918cb3c Mon Sep 17 00:00:00 2001 From: tsampazk <27914645+tsampazk@users.noreply.github.com> Date: Tue, 20 Dec 2022 15:39:26 +0200 Subject: [PATCH 43/44] Some fixes from ros1 --- .../opendr_perception/image_dataset_node.py | 22 +++++++++---------- 1 file changed, 10 insertions(+), 12 deletions(-) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/image_dataset_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/image_dataset_node.py index 0835e959e7..3587e37aef 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/image_dataset_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/image_dataset_node.py @@ -25,16 +25,16 @@ class ImageDatasetNode(Node): def __init__( - self, - dataset: DatasetIterator, - output_rgb_image_topic="/opendr/dataset_image", - data_fps=10, + self, + dataset: DatasetIterator, + output_rgb_image_topic="/opendr/dataset_image", + data_fps=10, ): """ Creates a ROS2 Node for publishing dataset images """ - super().__init__('image_dataset_node') + super().__init__('opendr_image_dataset_node') self.dataset = dataset self.bridge = ROS2Bridge() @@ -44,13 +44,12 @@ def __init__( 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 - self.get_logger().info("Publishing image [" + str(self.sample_index) + "]") message = self.bridge.to_ros_image( image, encoding="bgr8" ) @@ -59,13 +58,12 @@ def timer_callback(self): self.sample_index += 1 -def main( - args=None, -): +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="KITTI/opendr_nano_kitti") + type=str, default="MOT") parser.add_argument( "-ks", "--mot20_subsets_path", help="Path to mot20 subsets", type=str, default=os.path.join( @@ -73,7 +71,7 @@ def main( "datasets", "splits", "nano_mot20.train" ) ) - parser.add_argument("-o", "--output_rgb_image_topic", help="Topic name to upload the data", + 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) From af14715236f5e12a3ef1dd91f6e339d47f5b6ca4 Mon Sep 17 00:00:00 2001 From: tsampazk <27914645+tsampazk@users.noreply.github.com> Date: Tue, 20 Dec 2022 15:39:32 +0200 Subject: [PATCH 44/44] Some fixes from ros1 --- .../opendr_perception/point_cloud_dataset_node.py | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py index 116b9b149f..5ea7f129ff 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py @@ -15,9 +15,9 @@ import argparse import os -from sensor_msgs.msg import PointCloud as ROS_PointCloud import rclpy from rclpy.node import Node +from sensor_msgs.msg import PointCloud as ROS_PointCloud from opendr_bridge import ROS2Bridge from opendr.engine.datasets import DatasetIterator from opendr.perception.object_detection_3d import KittiDataset, LabeledPointCloudsDatasetIterator @@ -34,7 +34,7 @@ def __init__( Creates a ROS Node for publishing dataset point clouds """ - super().__init__('point_cloud_dataset_node') + super().__init__('opendr_point_cloud_dataset_node') self.dataset = dataset self.bridge = ROS2Bridge() @@ -44,13 +44,13 @@ def __init__( self.output_point_cloud_publisher = self.create_publisher( ROS_PointCloud, output_point_cloud_topic, 1 ) + self.get_logger().info("Publishing point_cloud images.") def timer_callback(self): point_cloud = self.dataset[self.sample_index % len(self.dataset)][0] # Dataset should have a (PointCloud, Target) pair as elements - self.get_logger().info("Publishing point_cloud [" + str(self.sample_index) + "]") message = self.bridge.to_ros_point_cloud( point_cloud, self.get_clock().now().to_msg() ) @@ -59,10 +59,9 @@ def timer_callback(self): self.sample_index += 1 -def main( - args=None, -): +def main(args=None): rclpy.init(args=args) + parser = argparse.ArgumentParser() parser.add_argument("-d", "--dataset_path", help="Path to a dataset. If does not exist, nano KITTI dataset will be downloaded there.", @@ -71,7 +70,7 @@ def main( help="Path to kitti subsets. Used only if a KITTI dataset is downloaded", type=str, default="../../src/opendr/perception/object_detection_3d/datasets/nano_kitti_subsets") - parser.add_argument("-o", "--output_point_cloud_topic", help="Topic name to upload the data", + parser.add_argument("-o", "--output_point_cloud_topic", help="Topic name to publish the data", type=str, default="/opendr/dataset_point_cloud") parser.add_argument("-f", "--fps", help="Data FPS", type=float, default=10)