From 640747fee2fa5ad1f850547749fb44b53a9f79ff Mon Sep 17 00:00:00 2001 From: topguns837 Date: Sun, 11 Feb 2024 15:31:23 +0530 Subject: [PATCH] Add FPS calculations and added License and description to setup.cfg --- object_detection/config/params.yaml | 2 +- .../object_detection/ObjectDetection.py | 25 +++++++++++++++++++ object_detection/package.xml | 2 +- object_detection/setup.py | 4 +-- 4 files changed, 29 insertions(+), 4 deletions(-) diff --git a/object_detection/config/params.yaml b/object_detection/config/params.yaml index eb69547..681c2b9 100644 --- a/object_detection/config/params.yaml +++ b/object_detection/config/params.yaml @@ -7,6 +7,6 @@ object_detection: model_params: detector_type: YOLOv5 model_dir_path: /root/percep_ws/models/yolov5 - weight_file_name: yolov5.onnx + weight_file_name: yolov5s.pt confidence_threshold : 0.5 show_fps : 1 \ No newline at end of file diff --git a/object_detection/object_detection/ObjectDetection.py b/object_detection/object_detection/ObjectDetection.py index b2ff1e0..5ac3b48 100644 --- a/object_detection/object_detection/ObjectDetection.py +++ b/object_detection/object_detection/ObjectDetection.py @@ -16,6 +16,7 @@ import importlib import os +import time import cv2 @@ -77,6 +78,12 @@ def __init__(self): self.bridge = CvBridge() + if self.show_fps: + self.start_time = time.time() + self.frame_count = 0 + self.total_elapsed_time = 0 + self.average_fps = 0 + def discover_detectors(self): curr_dir = os.path.dirname(__file__) dir_contents = os.listdir(curr_dir + "/Detectors") @@ -101,6 +108,8 @@ def load_detector(self): def detection_cb(self, img_msg): cv_image = self.bridge.imgmsg_to_cv2(img_msg, "bgr8") + start_time = time.time() + predictions = self.detector.get_predictions(cv_image=cv_image) if predictions is None: @@ -120,6 +129,22 @@ def detection_cb(self, img_msg): cv_image = cv2.putText(cv_image, label, (x1, y1 - 5), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1) + if self.show_fps: + elapsed_time = time.time() - start_time + self.frame_count += 1 + self.total_elapsed_time += elapsed_time + + # Write FPS on the frame + cv_image = cv2.putText(cv_image, f"FPS: {self.average_fps:.2f}", (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2) + + if time.time() - self.start_time >= 1.0: + self.average_fps = self.frame_count / self.total_elapsed_time + + self.frame_count = 0 + self.total_elapsed_time = 0 + self.start_time = time.time() + output = self.bridge.cv2_to_imgmsg(cv_image, "bgr8") self.img_pub.publish(output) diff --git a/object_detection/package.xml b/object_detection/package.xml index fc13de3..1bd237b 100644 --- a/object_detection/package.xml +++ b/object_detection/package.xml @@ -3,7 +3,7 @@ object_detection 0.0.0 - This is a ROS 2 package aimed at providing an interfacing between Deep Learning models and the ROS architecture using a plug-and-play modular arhitecture + This is a ROS 2 package aimed at providing an interfacing between Deep Learning models and the ROS architecture using a plug-and-play modular architecture singh Apache 2.0 diff --git a/object_detection/setup.py b/object_detection/setup.py index c1fcbea..5602255 100644 --- a/object_detection/setup.py +++ b/object_detection/setup.py @@ -21,8 +21,8 @@ zip_safe=True, maintainer='singh', maintainer_email='jasmeet0915@gmail.com', - description='TODO: Package description', - license='TODO: License declaration', + description='Plug-and-Play ROS 2 package for Perception in Robotics', + license='Apache 2.0', tests_require=['pytest'], entry_points={ 'console_scripts': [