From 54e2d30687ae86bfa7b8502e5ed1d8e9e76216ef Mon Sep 17 00:00:00 2001 From: Illia Oleksiienko Date: Fri, 25 Nov 2022 15:22:57 +0100 Subject: [PATCH] Ros1 fixes for 3D Detection and 2D/3D tracking (#320) * Fix ros1 3d detection * Fix ros1 tracking 3d node * Fix ros1 tracking 2d fairmot node * Fix ros1 tracking 2d deep sort node * Fix ros1 point cloud dataset node * Fix ros1 image dataset node * Fix style errors * Fix point cloud dataset init * Fix image dataset init node * Fix fair mot init * Fix image format rgb8 to bgr8 * Fix 3d detection init node * Fix re-download of the dataset * Fix fairmot conditional computing and typing * Fix deepsort init node * Fix ab3dmot init node * Add point cloud dataset anonymous node * Fix typo * Fix phrasing * Remove extra print * Apply suggestions from code review Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Fix sources * Fix names * Add topic comments * Move fairmot to rgb-based * Fix deep sort direction incompatibility * Fix bounding box frame reference * Exlude opendr_ws devel from source check * Optimize deep sort topic computations * Fixes * More fixes Co-authored-by: Illia Oleksiienko Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> Co-authored-by: ad-daniel <44834743+ad-daniel@users.noreply.github.com> Co-authored-by: ad-daniel --- projects/opendr_ws/README.md | 6 +- projects/opendr_ws/src/perception/README.md | 22 +-- .../src/perception/scripts/image_dataset.py | 69 ++++++--- .../scripts/object_detection_3d_voxel.py | 87 +++++++---- .../scripts/object_tracking_2d_deep_sort.py | 143 +++++++++++------- .../scripts/object_tracking_2d_fair_mot.py | 134 ++++++++++------ .../scripts/object_tracking_3d_ab3dmot.py | 112 ++++++++++---- .../perception/scripts/point_cloud_dataset.py | 52 +++++-- src/opendr/engine/target.py | 2 +- .../deep_sort/algorithm/deep_sort_tracker.py | 10 +- .../object_tracking_2d_deep_sort_learner.py | 4 +- tests/test_license.py | 1 + tests/test_pep8.py | 1 + 13 files changed, 420 insertions(+), 223 deletions(-) diff --git a/projects/opendr_ws/README.md b/projects/opendr_ws/README.md index 2fabf14d5d..0fe9f9c010 100755 --- a/projects/opendr_ws/README.md +++ b/projects/opendr_ws/README.md @@ -50,12 +50,12 @@ Currently, apart from tools, opendr_ws contains the following ROS nodes (categor 7. [Semantic Segmentation](src/perception/README.md#semantic-segmentation-ros-node) 8. [Video Human Activity Recognition](src/perception/README.md#human-action-recognition-ros-node) 9. [Landmark-based Facial Expression Recognition](src/perception/README.md#landmark-based-facial-expression-recognition-ros-node) -10. [Deep Sort Object Tracking 2D](src/perception/README.md#deep-sort-object-tracking-2d-ros-node) -11. [Skeleton-based Human Action Recognition](src/perception/README.md#skeleton-based-human-action-recognition-ros-node) +10. [FairMOT Object Tracking 2D](src/perception/README.md#fairmot-object-tracking-2d-ros-node) +11. [Deep Sort Object Tracking 2D](src/perception/README.md#deep-sort-object-tracking-2d-ros-node) +12. [Skeleton-based Human Action Recognition](src/perception/README.md#skeleton-based-human-action-recognition-ros-node) ## Point cloud input 1. [Voxel Object Detection 3D](src/perception/README.md#voxel-object-detection-3d-ros-node) 2. [AB3DMOT Object Tracking 3D](src/perception/README.md#ab3dmot-object-tracking-3d-ros-node) -3. [FairMOT Object Tracking 2D](src/perception/README.md#fairmot-object-tracking-2d-ros-node) ## RGB + Infrared input 1. [End-to-End Multi-Modal Object Detection (GEM)](src/perception/README.md#gem-ros-node) ## RGBD input nodes diff --git a/projects/opendr_ws/src/perception/README.md b/projects/opendr_ws/src/perception/README.md index eebb580b86..5b31864fd7 100755 --- a/projects/opendr_ws/src/perception/README.md +++ b/projects/opendr_ws/src/perception/README.md @@ -38,11 +38,11 @@ rosrun perception pose_estimation.py ``` 3. You can examine the annotated image stream using `rqt_image_view` (select the topic `/opendr/image_pose_annotated`) or - `rostopic echo /opendr/poses`. + `rostopic echo /opendr/poses`. Note that to use the pose messages properly, you need to create an appropriate subscriber that will convert the ROS pose messages back to OpenDR poses which you can access as described in the [documentation](https://github.com/opendr-eu/opendr/blob/master/docs/reference/engine-target.md#posekeypoints-confidence): ```python - ... + ... rospy.Subscriber("opendr/poses", Detection2DArray, self.callback) ... def callback(self, data): @@ -212,7 +212,7 @@ A ROS node for performing heart anomaly (atrial fibrillation) detection from ecg ```shell rosrun perception heart_anomaly_detection.py ECG_TOPIC MODEL ``` -with `ECG_TOPIC` specifying the ROS topic to which the node will subscribe, and `MODEL` set to either *gru* or *anbof*. The predictied classes are published to the topic `/opendr/heartanomaly`. +with `ECG_TOPIC` specifying the ROS topic to which the node will subscribe, and `MODEL` set to either *gru* or *anbof*. The predicted classes are published to the topic `/opendr/heartanomaly`. ## Human Action Recognition ROS Node @@ -221,7 +221,7 @@ Assuming the drivers have been installed and OpenDR catkin workspace has been so ```shell rosrun perception video_activity_recognition.py ``` -The predictied class id and confidence is published under the topic name `/opendr/human_activity_recognition`, and the human-readable class name under `/opendr/human_activity_recognition_description`. +The predicted class id and confidence is published under the topic name `/opendr/human_activity_recognition`, and the human-readable class name under `/opendr/human_activity_recognition_description`. ## Landmark-based Facial Expression Recognition ROS Node @@ -230,16 +230,16 @@ Assuming the drivers have been installed and OpenDR catkin workspace has been so ```shell rosrun perception landmark_based_facial_expression_recognition.py ``` -The predictied class id and confidence is published under the topic name `/opendr/landmark_based_expression_recognition`, and the human-readable class name under `/opendr/landmark_based_expression_recognition_description`. +The predicted class id and confidence is published under the topic name `/opendr/landmark_based_expression_recognition`, and the human-readable class name under `/opendr/landmark_based_expression_recognition_description`. ## Skeleton-based Human Action Recognition ROS Node -A ROS node for performing Skeleton-based Human Action Recognition using either ST-GCN or PST-GCN models pretrained on NTU-RGBD-60 dataset. The human body poses of the image are first extracted by the light-weight Openpose method which is implemented in the toolkit, and they are passed to the skeleton-based action recognition method to be categorized. +A ROS node for performing Skeleton-based Human Action Recognition using either ST-GCN or PST-GCN models pretrained on NTU-RGBD-60 dataset. The human body poses of the image are first extracted by the light-weight Open-pose method which is implemented in the toolkit, and they are passed to the skeleton-based action recognition method to be categorized. Assuming the drivers have been installed and OpenDR catkin workspace has been sourced, the node can be started as: ```shell rosrun perception skeleton_based_action_recognition.py ``` -The predictied class id and confidence is published under the topic name `/opendr/skeleton_based_action_recognition`, and the human-readable class name under `/opendr/skeleton_based_action_recognition_description`. +The predicted class id and confidence is published under the topic name `/opendr/skeleton_based_action_recognition`, and the human-readable class name under `/opendr/skeleton_based_action_recognition_description`. Besides, the annotated image is published in `/opendr/image_pose_annotated` as well as the corresponding poses in `/opendr/poses`. ## Speech Command Recognition ROS Node @@ -270,7 +270,7 @@ To get a point cloud from a dataset on the disk, you can start a `point_cloud_da ```shell rosrun perception point_cloud_dataset.py ``` -This will pulbish the dataset point clouds to a `/opendr/dataset_point_cloud` topic by default, which means that the `input_point_cloud_topic` should be set to `/opendr/dataset_point_cloud`. +This will publish the dataset point clouds to a `/opendr/dataset_point_cloud` topic by default, which means that the `input_point_cloud_topic` should be set to `/opendr/dataset_point_cloud`. ## AB3DMOT Object Tracking 3D ROS Node @@ -286,7 +286,7 @@ To get a point cloud from a dataset on the disk, you can start a `point_cloud_da ```shell rosrun perception point_cloud_dataset.py ``` -This will pulbish the dataset point clouds to a `/opendr/dataset_point_cloud` topic by default, which means that the `input_point_cloud_topic` should be set to `/opendr/dataset_point_cloud`. +This will publish the dataset point clouds to a `/opendr/dataset_point_cloud` topic by default, which means that the `input_point_cloud_topic` should be set to `/opendr/dataset_point_cloud`. ## FairMOT Object Tracking 2D ROS Node @@ -305,7 +305,7 @@ If you want to use a dataset from the disk, you can start a `image_dataset.py` n ```shell rosrun perception image_dataset.py ``` -This will pulbish the dataset images to an `/opendr/dataset_image` topic by default, which means that the `input_image_topic` should be set to `/opendr/dataset_image`. +This will publish the dataset images to an `/opendr/dataset_image` topic by default, which means that the `input_image_topic` should be set to `/opendr/dataset_image`. ## Deep Sort Object Tracking 2D ROS Node @@ -323,5 +323,5 @@ If you want to use a dataset from the disk, you can start an `image_dataset.py` ```shell rosrun perception image_dataset.py ``` -This will pulbish the dataset images to an `/opendr/dataset_image` topic by default, which means that the `input_image_topic` should be set to `/opendr/dataset_image`. +This will publish the dataset images to an `/opendr/dataset_image` topic by default, which means that the `input_image_topic` should be set to `/opendr/dataset_image`. diff --git a/projects/opendr_ws/src/perception/scripts/image_dataset.py b/projects/opendr_ws/src/perception/scripts/image_dataset.py index 0ce4ee3850..c4af25488e 100755 --- a/projects/opendr_ws/src/perception/scripts/image_dataset.py +++ b/projects/opendr_ws/src/perception/scripts/image_dataset.py @@ -13,6 +13,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +import argparse import os import rospy import time @@ -26,21 +27,21 @@ class ImageDatasetNode: def __init__( self, dataset: DatasetIterator, - output_image_topic="/opendr/dataset_image", + output_rgb_image_topic="/opendr/dataset_image", + data_fps=30, ): """ Creates a ROS Node for publishing dataset images """ - # Initialize the face detector self.dataset = dataset # Initialize OpenDR ROSBridge object self.bridge = ROSBridge() + self.delay = 1.0 / data_fps - if output_image_topic is not None: - self.output_image_publisher = rospy.Publisher( - output_image_topic, ROS_Image, queue_size=10 - ) + self.output_image_publisher = rospy.Publisher( + output_rgb_image_topic, ROS_Image, queue_size=10 + ) def start(self): rospy.loginfo("Timing images") @@ -53,32 +54,60 @@ def start(self): rospy.loginfo("Publishing image [" + str(i) + "]") message = self.bridge.to_ros_image( - image, encoding="rgb8" + image, encoding="bgr8" ) self.output_image_publisher.publish(message) - time.sleep(0.1) + time.sleep(self.delay) i += 1 -if __name__ == "__main__": - - rospy.init_node('opendr_image_dataset') - - dataset_path = MotDataset.download_nano_mot20( - "MOT", True - ).path +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("-d", "--dataset_path", help="Path to a dataset", + type=str, default="MOT") + parser.add_argument( + "-ks", "--mot20_subsets_path", help="Path to mot20 subsets", + type=str, default=os.path.join( + "..", "..", "src", "opendr", "perception", "object_tracking_2d", + "datasets", "splits", "nano_mot20.train" + ) + ) + parser.add_argument("-o", "--output_rgb_image_topic", help="Topic name to publish the data", + type=str, default="/opendr/dataset_image") + parser.add_argument("-f", "--fps", help="Data FPS", + type=float, default=30) + args = parser.parse_args() + + dataset_path = args.dataset_path + mot20_subsets_path = args.mot20_subsets_path + output_rgb_image_topic = args.output_rgb_image_topic + data_fps = args.fps + + if not os.path.exists(dataset_path): + dataset_path = MotDataset.download_nano_mot20( + "MOT", True + ).path dataset = RawMotDatasetIterator( dataset_path, { - "mot20": os.path.join( - "..", "..", "src", "opendr", "perception", "object_tracking_2d", - "datasets", "splits", "nano_mot20.train" - ) + "mot20": mot20_subsets_path }, scan_labels=False ) - dataset_node = ImageDatasetNode(dataset) + + rospy.init_node("image_dataset", anonymous=True) + + dataset_node = ImageDatasetNode( + dataset, + output_rgb_image_topic=output_rgb_image_topic, + data_fps=data_fps, + ) + dataset_node.start() + + +if __name__ == '__main__': + main() diff --git a/projects/opendr_ws/src/perception/scripts/object_detection_3d_voxel.py b/projects/opendr_ws/src/perception/scripts/object_detection_3d_voxel.py index 6d6b74015a..1716c2aa9a 100755 --- a/projects/opendr_ws/src/perception/scripts/object_detection_3d_voxel.py +++ b/projects/opendr_ws/src/perception/scripts/object_detection_3d_voxel.py @@ -13,6 +13,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +import argparse import torch import os import rospy @@ -39,7 +40,7 @@ def __init__( """ Creates a ROS 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') @@ -58,15 +59,13 @@ def __init__( self.learner.load(os.path.join(temp_dir, model_name), verbose=True) - # Initialize OpenDR ROSBridge object + self.input_point_cloud_topic = input_point_cloud_topic self.bridge = ROSBridge() self.detection_publisher = rospy.Publisher( - output_detection3d_topic, Detection3DArray, queue_size=10 + output_detection3d_topic, Detection3DArray, queue_size=1 ) - rospy.Subscriber(input_point_cloud_topic, ROS_PointCloud, self.callback) - def callback(self, data): """ Callback that process the input data and publishes to the corresponding topics @@ -80,39 +79,67 @@ def callback(self, data): # 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) - rospy.loginfo("Published detection boxes") + self.detection_publisher.publish(ros_boxes) + rospy.loginfo("Published detection boxes") + + def listen(self): + """ + Start the node and begin processing input data. + """ + rospy.init_node('voxel_detection_3d', anonymous=True) + rospy.Subscriber(self.input_point_cloud_topic, ROS_PointCloud, self.callback, queue_size=1, buff_size=10000000) -if __name__ == "__main__": - # Automatically run on GPU/CPU - device = "cuda:0" if torch.cuda.is_available() else "cpu" + rospy.loginfo("Object Detection 3D Voxel Node started.") + rospy.spin() - # initialize ROS node - rospy.init_node("opendr_voxel_detection_3d", anonymous=True) - rospy.loginfo("Voxel Detection 3D node started") - model_name = rospy.get_param("~model_name", "tanet_car_xyres_16") - model_config_path = rospy.get_param( - "~model_config_path", os.path.join( +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("-n", "--model_name", help="Name of the trained model", + type=str, default="tanet_car_xyres_16", choices=["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", "test_short.proto" + "car", "xyres_16.proto" ) ) - temp_dir = rospy.get_param("~temp_dir", "temp") - input_point_cloud_topic = rospy.get_param( - "~input_point_cloud_topic", "/opendr/dataset_point_cloud" - ) - rospy.loginfo("Using model_name: {}".format(model_name)) + parser.add_argument("-t", "--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("-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: + 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" - # created node object voxel_node = ObjectDetection3DVoxelNode( device=device, - model_name=model_name, - model_config_path=model_config_path, - input_point_cloud_topic=input_point_cloud_topic, - temp_dir=temp_dir, + 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, + output_detection3d_topic=args.output_detection3d_topic, ) - # begin ROS communications - rospy.spin() + + voxel_node.listen() + + +if __name__ == '__main__': + main() diff --git a/projects/opendr_ws/src/perception/scripts/object_tracking_2d_deep_sort.py b/projects/opendr_ws/src/perception/scripts/object_tracking_2d_deep_sort.py index 70d66c69a8..3e083be118 100755 --- a/projects/opendr_ws/src/perception/scripts/object_tracking_2d_deep_sort.py +++ b/projects/opendr_ws/src/perception/scripts/object_tracking_2d_deep_sort.py @@ -13,10 +13,11 @@ # 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 +from opendr.engine.target import TrackingAnnotationList import rospy from vision_msgs.msg import Detection2DArray from std_msgs.msg import Int32MultiArray @@ -34,10 +35,10 @@ class ObjectTracking2DDeepSortNode: 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", + 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_rgb_image_topic="/opendr/deep_sort_image_annotated", device="cuda:0", model_name="deep_sort", temp_dir="temp", @@ -46,11 +47,11 @@ def __init__( Creates a ROS 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 @@ -63,7 +64,6 @@ def __init__( :type temp_dir: str """ - # # Initialize the face detector self.detector = detector self.learner = ObjectTracking2DDeepSortLearner( device=device, temp_path=temp_dir, @@ -73,22 +73,23 @@ def __init__( self.learner.load(os.path.join(temp_dir, model_name), verbose=True) - # Initialize OpenDR ROSBridge object self.bridge = ROSBridge() - self.tracking_id_publisher = rospy.Publisher( - output_tracking_id_topic, Int32MultiArray, queue_size=10 - ) + self.input_rgb_image_topic = input_rgb_image_topic - if output_image_topic is not None: - self.output_image_publisher = rospy.Publisher( - output_image_topic, ROS_Image, queue_size=10 + if output_tracking_id_topic is not None: + self.tracking_id_publisher = rospy.Publisher( + output_tracking_id_topic, Int32MultiArray, queue_size=10 ) - self.detection_publisher = rospy.Publisher( - output_detection_topic, Detection2DArray, queue_size=10 - ) + if output_rgb_image_topic is not None: + self.output_image_publisher = rospy.Publisher( + output_rgb_image_topic, ROS_Image, queue_size=10 + ) - rospy.Subscriber(input_image_topic, ROS_Image, self.callback) + if output_detection_topic is not None: + self.detection_publisher = rospy.Publisher( + output_detection_topic, Detection2DArray, queue_size=10 + ) def callback(self, data): """ @@ -101,8 +102,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) - print(image_with_detections.data.shape) - tracking_boxes = self.learner.infer(image_with_detections) + tracking_boxes = self.learner.infer(image_with_detections, swap_left_top=True) if self.output_image_publisher is not None: frame = image.opencv() @@ -113,21 +113,28 @@ def callback(self, data): self.output_image_publisher.publish(message) rospy.loginfo("Published annotated image") - 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: + ros_boxes = self.bridge.to_ros_boxes(detection_boxes) self.detection_publisher.publish(ros_boxes) rospy.loginfo("Published 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) rospy.loginfo("Published tracking ids") + def listen(self): + """ + Start the node and begin processing input data. + """ + rospy.init_node('object_tracking_2d_deep_sort_node', anonymous=True) + rospy.Subscriber(self.input_rgb_image_topic, ROS_Image, self.callback, queue_size=1, buff_size=10000000) + + rospy.loginfo("Object Tracking 2D Deep Sort Node started.") + rospy.spin() + colors = [ (255, 0, 255), @@ -139,7 +146,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 @@ -174,36 +181,62 @@ def draw_predictions(frame, predictions: TrackingAnnotation, is_centered=False, ) -if __name__ == "__main__": - # Automatically run on GPU/CPU - device = "cuda:0" if torch.cuda.is_available() else "cpu" - - # initialize ROS node - rospy.init_node("opendr_deep_sort", anonymous=True) - rospy.loginfo("Deep Sort node started") - - model_name = rospy.get_param("~model_name", "deep_sort") - temp_dir = rospy.get_param("~temp_dir", "temp") - input_image_topic = rospy.get_param( - "~input_image_topic", "/opendr/dataset_image" - ) - rospy.loginfo("Using model_name: {}".format(model_name)) +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("-n", "--model_name", help="Name of the trained model", + type=str, default="deep_sort", choices=["deep_sort"]) + parser.add_argument("-t", "--temp_dir", help="Path to a temporary directory 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", + help="Output tracking ids topic with the same element count as in output_detection_topic", + type=str, default="/opendr/deep_sort_tracking_id") + parser.add_argument("-oi", "--output_rgb_image_topic", + help="Output annotated image topic with a visualization of detections and their ids", + 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() + + 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=device, temp_path=temp_dir, + device=device, temp_path=args.temp_dir, ) - if not os.path.exists(os.path.join(temp_dir, "fairmot_dla34")): - ObjectTracking2DFairMotLearner.download("fairmot_dla34", 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(temp_dir, "fairmot_dla34"), verbose=True) + detection_learner.load(os.path.join(args.temp_dir, "fairmot_dla34"), verbose=True) - # created node object deep_sort_node = ObjectTracking2DDeepSortNode( detector=detection_learner, device=device, - model_name=model_name, - input_image_topic=input_image_topic, - temp_dir=temp_dir, + 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, ) - # begin ROS communications - rospy.spin() + + deep_sort_node.listen() + + +if __name__ == '__main__': + main() diff --git a/projects/opendr_ws/src/perception/scripts/object_tracking_2d_fair_mot.py b/projects/opendr_ws/src/perception/scripts/object_tracking_2d_fair_mot.py index 0f8d3a7373..7c72a2d976 100755 --- a/projects/opendr_ws/src/perception/scripts/object_tracking_2d_fair_mot.py +++ b/projects/opendr_ws/src/perception/scripts/object_tracking_2d_fair_mot.py @@ -13,10 +13,11 @@ # 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 +from opendr.engine.target import TrackingAnnotationList import rospy from vision_msgs.msg import Detection2DArray from std_msgs.msg import Int32MultiArray @@ -31,21 +32,21 @@ class ObjectTracking2DFairMotNode: 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", + input_rgb_image_topic="/usb_cam/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", ): """ 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 @@ -58,7 +59,6 @@ def __init__( :type temp_dir: str """ - # # Initialize the face detector self.learner = ObjectTracking2DFairMotLearner( device=device, temp_path=temp_dir, ) @@ -67,22 +67,23 @@ def __init__( self.learner.load(os.path.join(temp_dir, model_name), verbose=True) - # Initialize OpenDR ROSBridge object self.bridge = ROSBridge() + self.input_rgb_image_topic = input_rgb_image_topic - self.detection_publisher = rospy.Publisher( - output_detection_topic, Detection2DArray, queue_size=10 - ) - self.tracking_id_publisher = rospy.Publisher( - output_tracking_id_topic, Int32MultiArray, queue_size=10 - ) + if output_detection_topic is not None: + self.detection_publisher = rospy.Publisher( + output_detection_topic, Detection2DArray, queue_size=10 + ) - if output_image_topic is not None: - self.output_image_publisher = rospy.Publisher( - output_image_topic, ROS_Image, queue_size=10 + if output_tracking_id_topic is not None: + self.tracking_id_publisher = rospy.Publisher( + output_tracking_id_topic, Int32MultiArray, queue_size=10 ) - rospy.Subscriber(input_image_topic, ROS_Image, self.callback) + if output_rgb_image_topic is not None: + self.output_image_publisher = rospy.Publisher( + output_rgb_image_topic, ROS_Image, queue_size=10 + ) def callback(self, data): """ @@ -104,22 +105,29 @@ def callback(self, data): self.output_image_publisher.publish(message) rospy.loginfo("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) rospy.loginfo("Published 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) rospy.loginfo("Published tracking ids") + def listen(self): + """ + Start the node and begin processing input data. + """ + rospy.init_node('object_tracking_2d_fair_mot_node', anonymous=True) + rospy.Subscriber(self.input_rgb_image_topic, ROS_Image, self.callback, queue_size=1, buff_size=10000000) + + rospy.loginfo("Object Tracking 2D Fair Mot Node started.") + rospy.spin() + colors = [ (255, 0, 255), @@ -131,7 +139,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 @@ -166,27 +174,53 @@ def draw_predictions(frame, predictions: TrackingAnnotation, is_centered=False, ) -if __name__ == "__main__": - # Automatically run on GPU/CPU - device = "cuda:0" if torch.cuda.is_available() else "cpu" +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("-n", "--model_name", help="Name of the trained model", + type=str, default="fairmot_dla34", choices=["fairmot_dla34"]) + parser.add_argument("-t", "--temp_dir", help="Path to a temporary directory 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", + help="Output tracking ids topic with the same element count as in output_detection_topic", + type=str, default="/opendr/fairmot_tracking_id") + parser.add_argument("-oi", "--output_rgb_image_topic", + help="Output annotated image topic with a visualization of detections and their ids", + 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() + + 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" - # initialize ROS node - rospy.init_node("opendr_fair_mot", anonymous=True) - rospy.loginfo("FairMOT node started") - - model_name = rospy.get_param("~model_name", "fairmot_dla34") - temp_dir = rospy.get_param("~temp_dir", "temp") - input_image_topic = rospy.get_param( - "~input_image_topic", "/opendr/dataset_image" - ) - rospy.loginfo("Using model_name: {}".format(model_name)) - - # created node object fair_mot_node = ObjectTracking2DFairMotNode( device=device, - model_name=model_name, - input_image_topic=input_image_topic, - temp_dir=temp_dir, + 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, ) - # begin ROS communications - rospy.spin() + + fair_mot_node.listen() + + +if __name__ == '__main__': + main() diff --git a/projects/opendr_ws/src/perception/scripts/object_tracking_3d_ab3dmot.py b/projects/opendr_ws/src/perception/scripts/object_tracking_3d_ab3dmot.py index b9927182ce..6c565bc577 100755 --- a/projects/opendr_ws/src/perception/scripts/object_tracking_3d_ab3dmot.py +++ b/projects/opendr_ws/src/perception/scripts/object_tracking_3d_ab3dmot.py @@ -13,6 +13,7 @@ # 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 @@ -36,10 +37,10 @@ def __init__( ): """ Creates a ROS 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_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 @@ -53,15 +54,18 @@ def __init__( device=device ) - # Initialize OpenDR ROSBridge object self.bridge = ROSBridge() + self.input_point_cloud_topic = input_point_cloud_topic - self.detection_publisher = rospy.Publisher( - output_detection3d_topic, Detection3DArray, queue_size=10 - ) - self.tracking_id_publisher = rospy.Publisher( - output_tracking3d_id_topic, Int32MultiArray, queue_size=10 - ) + if output_detection3d_topic is not None: + self.detection_publisher = rospy.Publisher( + output_detection3d_topic, Detection3DArray, queue_size=10 + ) + + if output_tracking3d_id_topic is not None: + self.tracking_id_publisher = rospy.Publisher( + output_tracking3d_id_topic, Int32MultiArray, queue_size=10 + ) rospy.Subscriber(input_point_cloud_topic, ROS_PointCloud, self.callback) @@ -76,44 +80,82 @@ 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, classes=["Car", "Van", "Truck", "Pedestrian", "Cyclist"]) if self.detection_publisher is not None: + # Convert detected boxes to ROS type and publish + ros_boxes = self.bridge.to_ros_boxes_3d(detection_boxes, classes=["Car", "Van", "Truck", "Pedestrian", "Cyclist"]) self.detection_publisher.publish(ros_boxes) rospy.loginfo("Published 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) rospy.loginfo("Published tracking ids") -if __name__ == "__main__": - # Automatically run on GPU/CPU - device = "cuda:0" if torch.cuda.is_available() else "cpu" + def listen(self): + """ + Start the node and begin processing input data. + """ + rospy.init_node('ab3dmot_tracking_3d', anonymous=True) + rospy.Subscriber(self.input_point_cloud_topic, ROS_PointCloud, self.callback, queue_size=1, buff_size=10000000) + + rospy.loginfo("Object Tracking 3D Ab3dmot Node started.") + rospy.spin() - # initialize ROS node - rospy.init_node("opendr_voxel_detection_3d", anonymous=True) - rospy.loginfo("AB3DMOT node started") - input_point_cloud_topic = rospy.get_param( - "~input_point_cloud_topic", "/opendr/dataset_point_cloud" - ) - temp_dir = rospy.get_param("~temp_dir", "temp") - detector_model_name = rospy.get_param("~detector_model_name", "tanet_car_xyres_16") - detector_model_config_path = rospy.get_param( - "~detector_model_config_path", os.path.join( +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("-dn", "--detector_model_name", help="Name of the trained model", + 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", "voxel_object_detection_3d", "second_detector", "configs", "tanet", - "car", "test_short.proto" + "car", "xyres_16.proto" ) ) + parser.add_argument("-t", "--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 id 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 + + 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, model_config_path=detector_model_config_path + 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) @@ -125,6 +167,12 @@ def callback(self, data): 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, ) - # begin ROS communications - rospy.spin() + + ab3dmot_node.listen() + + +if __name__ == '__main__': + main() diff --git a/projects/opendr_ws/src/perception/scripts/point_cloud_dataset.py b/projects/opendr_ws/src/perception/scripts/point_cloud_dataset.py index 0701e1005e..2859e8db47 100755 --- a/projects/opendr_ws/src/perception/scripts/point_cloud_dataset.py +++ b/projects/opendr_ws/src/perception/scripts/point_cloud_dataset.py @@ -13,6 +13,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +import argparse import os import rospy import time @@ -27,20 +28,19 @@ 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 """ - # Initialize the face detector self.dataset = dataset - # Initialize OpenDR ROSBridge object self.bridge = ROSBridge() + self.delay = 1.0 / data_fps - if output_point_cloud_topic is not None: - self.output_point_cloud_publisher = rospy.Publisher( - output_point_cloud_topic, ROS_PointCloud, queue_size=10 - ) + self.output_point_cloud_publisher = rospy.Publisher( + output_point_cloud_topic, ROS_PointCloud, queue_size=10 + ) def start(self): i = 0 @@ -55,20 +55,34 @@ def start(self): ) self.output_point_cloud_publisher.publish(message) - time.sleep(0.1) + time.sleep(self.delay) i += 1 -if __name__ == "__main__": - - rospy.init_node('opendr_point_cloud_dataset') - - dataset_path = "KITTI/opendr_nano_kitti" +def main(): + 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="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 publish 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( - "KITTI", kitti_subsets_path="../../src/opendr/perception/object_detection_3d/datasets/nano_kitti_subsets", + "KITTI", kitti_subsets_path=kitti_subsets_path, create_dir=True, ).path @@ -78,5 +92,15 @@ def start(self): dataset_path + "/training/calib", ) - dataset_node = PointCloudDatasetNode(dataset) + rospy.init_node('point_cloud_dataset', anonymous=True) + + dataset_node = PointCloudDatasetNode( + dataset, output_point_cloud_topic=output_point_cloud_topic, data_fps=data_fps + ) + dataset_node.start() + rospy.spin() + + +if __name__ == '__main__': + main() diff --git a/src/opendr/engine/target.py b/src/opendr/engine/target.py index c00187e882..4b44543731 100644 --- a/src/opendr/engine/target.py +++ b/src/opendr/engine/target.py @@ -361,7 +361,7 @@ def mot(self, with_confidence=True, frame=-1): ], dtype=np.float32) else: result = np.array([ - self.frame, + frame, self.left, self.top, self.width, 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..178f933078 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 @@ -51,7 +51,7 @@ def __init__( self.deepsort = self.build_tracker() self.frame = 0 - def infer(self, imageWithDetections: ImageWithDetections, frame_id=None): + def infer(self, imageWithDetections: ImageWithDetections, frame_id=None, swap_left_top=False): if frame_id is not None: self.frame = frame_id @@ -65,8 +65,8 @@ def infer(self, imageWithDetections: ImageWithDetections, frame_id=None): for detection in detections: bbox_xywh.append(np.array([ - detection.left, - detection.top, + detection.top if swap_left_top else detection.left, + detection.left if swap_left_top else detection.top, detection.width, detection.height, ])) @@ -96,8 +96,8 @@ def infer(self, imageWithDetections: ImageWithDetections, frame_id=None): bb_tlwh = self.deepsort._xyxy_to_tlwh(bb_xyxy) results.append(TrackingAnnotation( cls_id, - bb_tlwh[0], - bb_tlwh[1], + (bb_tlwh[1] + bb_tlwh[3] / 2 if swap_left_top else bb_tlwh[0] + bb_tlwh[2] / 2), + (bb_tlwh[0] + bb_tlwh[2] / 2 if swap_left_top else bb_tlwh[1] + bb_tlwh[3] / 2), bb_tlwh[2], bb_tlwh[3], id, diff --git a/src/opendr/perception/object_tracking_2d/deep_sort/object_tracking_2d_deep_sort_learner.py b/src/opendr/perception/object_tracking_2d/deep_sort/object_tracking_2d_deep_sort_learner.py index d0cd3f36a4..a5f0d020fa 100644 --- a/src/opendr/perception/object_tracking_2d/deep_sort/object_tracking_2d_deep_sort_learner.py +++ b/src/opendr/perception/object_tracking_2d/deep_sort/object_tracking_2d_deep_sort_learner.py @@ -262,7 +262,7 @@ def eval( return result - def infer(self, batch, frame_ids=None): + def infer(self, batch, frame_ids=None, swap_left_top=False): if self.tracker is None: raise ValueError("No model loaded or created") @@ -286,7 +286,7 @@ def infer(self, batch, frame_ids=None): t0 = time.time() - result = self.tracker.infer(image, frame_id) + result = self.tracker.infer(image, frame_id, swap_left_top=swap_left_top) results.append(result) t0 = time.time() - t0 diff --git a/tests/test_license.py b/tests/test_license.py index e8d39d4b37..6452095449 100755 --- a/tests/test_license.py +++ b/tests/test_license.py @@ -99,6 +99,7 @@ def setUp(self): 'src/opendr/perception/multimodal_human_centric/rgbd_hand_gesture_learner/algorithm/architectures', 'src/opendr/perception/skeleton_based_action_recognition/algorithm', 'projects/python/simulation/synthetic_multi_view_facial_image_generation/algorithm', + 'projects/opendr_ws/devel', 'src/opendr/perception/semantic_segmentation/bisenet/algorithm', 'src/opendr/perception/object_detection_2d/retinaface/algorithm', 'src/opendr/perception/object_detection_2d/gem/algorithm', diff --git a/tests/test_pep8.py b/tests/test_pep8.py index 800c6064c4..e4a445a9af 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/devel', 'venv', 'build', ]