diff --git a/projects/opendr_ws/src/opendr_perception/CMakeLists.txt b/projects/opendr_ws/src/opendr_perception/CMakeLists.txt index 9f58ca357d..92f2bfbb11 100644 --- a/projects/opendr_ws/src/opendr_perception/CMakeLists.txt +++ b/projects/opendr_ws/src/opendr_perception/CMakeLists.txt @@ -28,6 +28,7 @@ include_directories( ############# catkin_install_python(PROGRAMS + scripts/performance_node.py scripts/pose_estimation_node.py scripts/hr_pose_estimation_node.py scripts/fall_detection_node.py diff --git a/projects/opendr_ws/src/opendr_perception/README.md b/projects/opendr_ws/src/opendr_perception/README.md index e225c9cfd2..fd80f63c01 100644 --- a/projects/opendr_ws/src/opendr_perception/README.md +++ b/projects/opendr_ws/src/opendr_perception/README.md @@ -46,6 +46,11 @@ Before you can run any of the package's ROS nodes, some prerequisites need to be _An example would be to disable the output annotated image topic in a node when visualization is not needed and only use the detection message in another node, thus eliminating the OpenCV operations._ +- ### Logging the node performance in the console + OpenDR provides the utility [performance node](#performance-ros-node) to log performance messages in the console for the running node. + You can set the `performance_topic` of the node you are using and also run the performance node to get the time it takes for the + node to process a single input and its average speed expressed in frames per second. + - ### An example diagram of OpenDR nodes running ![Pose Estimation ROS node running diagram](../../images/opendr_node_diagram.png) - On the left, the `usb_cam` node can be seen, which is using a system camera to publish images on the `/usb_cam/image_raw` topic. @@ -79,6 +84,7 @@ The node publishes the detected poses in [OpenDR's 2D pose message format](../op - `-i or --input_rgb_image_topic INPUT_RGB_IMAGE_TOPIC`: topic name for input RGB image (default=`/usb_cam/image_raw`) - `-o or --output_rgb_image_topic OUTPUT_RGB_IMAGE_TOPIC`: topic name for output annotated RGB image, `None` to stop the node from publishing on this topic (default=`/opendr/image_pose_annotated`) - `-d or --detections_topic DETECTIONS_TOPIC`: topic name for detection messages, `None` to stop the node from publishing on this topic (default=`/opendr/poses`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) - `--accelerate`: acceleration flag that causes pose estimation to run faster but with less accuracy @@ -107,6 +113,7 @@ The node publishes the detected poses in [OpenDR's 2D pose message format](../op - `-i or --input_rgb_image_topic INPUT_RGB_IMAGE_TOPIC`: topic name for input RGB image (default=`/usb_cam/image_raw`) - `-o or --output_rgb_image_topic OUTPUT_RGB_IMAGE_TOPIC`: topic name for output annotated RGB image, `None` to stop the node from publishing on this topic (default=`/opendr/image_pose_annotated`) - `-d or --detections_topic DETECTIONS_TOPIC`: topic name for detection messages, `None` to stop the node from publishing on this topic (default=`/opendr/poses`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: Device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) - `--accelerate`: Acceleration flag that causes pose estimation to run faster but with less accuracy @@ -138,6 +145,7 @@ Fall detection uses the toolkit's pose estimation tool internally. - `-i or --input_rgb_image_topic INPUT_RGB_IMAGE_TOPIC`: topic name for input RGB image (default=`/usb_cam/image_raw`) - `-o or --output_rgb_image_topic OUTPUT_RGB_IMAGE_TOPIC`: topic name for output annotated RGB image, `None` to stop the node from publishing on this topic (default=`/opendr/image_fallen_annotated`) - `-d or --detections_topic DETECTIONS_TOPIC`: topic name for detection messages, `None` to stop the node from publishing on this topic (default=`/opendr/fallen`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) - `--accelerate`: acceleration flag that causes pose estimation that runs internally to run faster but with less accuracy @@ -168,6 +176,7 @@ The node makes use of the toolkit's [face detection tool](../../../../src/opendr - `-i or --input_rgb_image_topic INPUT_RGB_IMAGE_TOPIC`: topic name for input RGB image (default=`/usb_cam/image_raw`) - `-o or --output_rgb_image_topic OUTPUT_RGB_IMAGE_TOPIC`: topic name for output annotated RGB image, `None` to stop the node from publishing on this topic (default=`/opendr/image_faces_annotated`) - `-d or --detections_topic DETECTIONS_TOPIC`: topic name for detection messages, `None` to stop the node from publishing on this topic (default=`/opendr/faces`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) - `--backbone BACKBONE`: retinaface backbone, options are either `mnet` or `resnet`, where `mnet` detects masked faces as well (default=`resnet`) @@ -197,6 +206,7 @@ The node makes use of the toolkit's [face recognition tool](../../../../src/open - `-o or --output_rgb_image_topic OUTPUT_RGB_IMAGE_TOPIC`: topic name for output annotated RGB image, `None` to stop the node from publishing on this topic (default=`/opendr/image_face_reco_annotated`) - `-d or --detections_topic DETECTIONS_TOPIC`: topic name for detection messages, `None` to stop the node from publishing on this topic (default=`/opendr/face_recognition`) - `-id or --detections_id_topic DETECTIONS_ID_TOPIC`: topic name for detection ID messages, `None` to stop the node from publishing on this topic (default=`/opendr/face_recognition_id`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) - `--backbone BACKBONE`: backbone network (default=`mobilefacenet`) - `--dataset_path DATASET_PATH`: path of the directory where the images of the faces to be recognized are stored (default=`./database`) @@ -289,6 +299,7 @@ whose documentation can be found here: - `-i or --input_rgb_image_topic INPUT_RGB_IMAGE_TOPIC`: topic name for input RGB image (default=`/usb_cam/image_raw`) - `-o or --output_rgb_image_topic OUTPUT_RGB_IMAGE_TOPIC`: topic name for output annotated RGB image, `None` to stop the node from publishing on this topic (default=`/opendr/image_objects_annotated`) - `-d or --detections_topic DETECTIONS_TOPIC`: topic name for detection messages, `None` to stop the node from publishing on this topic (default=`/opendr/objects`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: Device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) 3. Default output topics: @@ -317,6 +328,7 @@ The node makes use of the toolkit's [single object tracking 2D SiamRPN tool](../ - `-i or --input_rgb_image_topic INPUT_RGB_IMAGE_TOPIC` : listen to RGB images on this topic (default=`/usb_cam/image_raw`) - `-o or --output_rgb_image_topic OUTPUT_RGB_IMAGE_TOPIC`: topic name for output annotated RGB image, `None` to stop the node from publishing on this topic (default=`/opendr/image_tracking_annotated`) - `-t or --tracker_topic TRACKER_TOPIC`: topic name for tracker messages, `None` to stop the node from publishing on this topic (default=`/opendr/tracked_object`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: Device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) 3. Default output topics: @@ -367,6 +379,7 @@ whose documentation can be found here: [Deep Sort docs](../../../../docs/referen - `-o or --output_rgb_image_topic OUTPUT_RGB_IMAGE_TOPIC`: topic name for output annotated RGB image, `None` to stop the node from publishing on this topic (default=`/opendr/image_objects_annotated`) - `-d or --detections_topic DETECTIONS_TOPIC`: topic name for detection messages, `None` to stop the node from publishing on this topic (default=`/opendr/objects`) - `-t or --tracking_id_topic TRACKING_ID_TOPIC`: topic name for tracking ID messages, `None` to stop the node from publishing on this topic (default=`/opendr/objects_tracking_id`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) - `-td --temp_dir TEMP_DIR`: path to a temporary directory with models (default=`temp`) @@ -406,6 +419,7 @@ and additional information about EfficientPS [here](../../../../src/opendr/perce - `-oh or --output_heatmap_topic OUTPUT_RGB_IMAGE_TOPIC`: publish the semantic and instance maps on this topic as `OUTPUT_HEATMAP_TOPIC/semantic` and `OUTPUT_HEATMAP_TOPIC/instance` (default=`/opendr/panoptic`) - `-ov or --output_rgb_image_topic OUTPUT_RGB_IMAGE_TOPIC`: publish the panoptic segmentation map as an RGB image on `VISUALIZATION_TOPIC` or a more detailed overview if using the `--detailed_visualization` flag (default=`/opendr/panoptic/rgb_visualization`) - `--detailed_visualization`: generate a combined overview of the input RGB image and the semantic, instance, and panoptic segmentation maps and publish it on `OUTPUT_RGB_IMAGE_TOPIC` (default=deactivated) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) 3. Default output topics: - Output images: `/opendr/panoptic/semantic`, `/opendr/panoptic/instance`, `/opendr/panoptic/rgb_visualization` @@ -430,6 +444,7 @@ The node makes use of the toolkit's [semantic segmentation tool](../../../../src - `-i or --input_rgb_image_topic INPUT_RGB_IMAGE_TOPIC`: topic name for input RGB image (default=`/usb_cam/image_raw`) - `-o or --output_heatmap_topic OUTPUT_HEATMAP_TOPIC`: topic to which we are publishing the heatmap in the form of a ROS image containing class IDs, `None` to stop the node from publishing on this topic (default=`/opendr/heatmap`) - `-ov or --output_rgb_image_topic OUTPUT_RGB_IMAGE_TOPIC`: topic to which we are publishing the heatmap image blended with the input image and a class legend for visualization purposes, `None` to stop the node from publishing on this topic (default=`/opendr/heatmap_visualization`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) 3. Default output topics: @@ -473,6 +488,7 @@ The node makes use of the toolkit's [binary high resolution tool](../../../../sr - `-ov or --output_rgb_image_topic OUTPUT_RGB_IMAGE_TOPIC`: topic to which we are publishing the heatmap image blended with the input image and a class legend for visualization purposes, `None` to stop the node from publishing on this topic (default=`/opendr/binary_hr_heatmap_visualization`) - `-m or --model_path MODEL_PATH`: path to the directory of the trained model (default=`test_model`) - `-a or --architecture ARCHITECTURE`: architecture used for the trained model, either `VGG_720p` or `VGG_1080p` (default=`VGG_720p`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) 3. Default output topics: @@ -502,6 +518,7 @@ whose documentation can be found [here](../../../../docs/reference/image_based_f - `-o or --output_rgb_image_topic OUTPUT_RGB_IMAGE_TOPIC`: topic name for output annotated RGB image, `None` to stop the node from publishing on this topic (default=`/opendr/image_emotion_estimation_annotated`) - `-e or --output_emotions_topic OUTPUT_EMOTIONS_TOPIC`: topic to which we are publishing the facial emotion results, `None` to stop the node from publishing on this topic (default=`"/opendr/facial_emotion_estimation"`) - `-m or --output_emotions_description_topic OUTPUT_EMOTIONS_DESCRIPTION_TOPIC`: topic to which we are publishing the description of the estimated facial emotion, `None` to stop the node from publishing on this topic (default=`/opendr/facial_emotion_estimation_description`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) 3. Default output topics: @@ -540,6 +557,7 @@ whose documentation can be found [here](../../../../docs/reference/landmark-base - `-i or --input_rgb_image_topic INPUT_RGB_IMAGE_TOPIC`: topic name for input RGB image (default=`/usb_cam/image_raw`) - `-o or --output_category_topic OUTPUT_CATEGORY_TOPIC`: topic to which we are publishing the recognized facial expression category info, `None` to stop the node from publishing on this topic (default=`"/opendr/landmark_expression_recognition"`) - `-d or --output_category_description_topic OUTPUT_CATEGORY_DESCRIPTION_TOPIC`: topic to which we are publishing the description of the recognized facial expression, `None` to stop the node from publishing on this topic (default=`/opendr/landmark_expression_recognition_description`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) - `--model`: architecture to use for facial expression recognition, options are `pstbln_ck+`, `pstbln_casia`, `pstbln_afew` (default=`pstbln_afew`) - `-s or --shape_predictor SHAPE_PREDICTOR`: shape predictor (landmark_extractor) to use (default=`./predictor_path`) @@ -590,6 +608,7 @@ Their documentation can be found [here](../../../../docs/reference/skeleton-base - `-i or --input_rgb_image_topic INPUT_RGB_IMAGE_TOPIC`: topic name for input RGB image (default=`/usb_cam/image_raw`) - `-o or --output_rgb_image_topic OUTPUT_RGB_IMAGE_TOPIC`: topic name for output pose-annotated RGB image, `None` to stop the node from publishing on this topic (default=`/opendr/image_pose_annotated`) - `-p or --pose_annotations_topic POSE_ANNOTATIONS_TOPIC`: topic name for pose annotations, `None` to stop the node from publishing on this topic (default=`/opendr/poses`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) 3. Default output topics: @@ -624,6 +643,7 @@ The node makes use of the toolkit's video human activity recognition tools which - `-i or --input_rgb_image_topic INPUT_RGB_IMAGE_TOPIC`: topic name for input RGB image (default=`/usb_cam/image_raw`) - `-o or --output_category_topic OUTPUT_CATEGORY_TOPIC`: topic to which we are publishing the recognized activity, `None` to stop the node from publishing on this topic (default=`"/opendr/human_activity_recognition"`) - `-od or --output_category_description_topic OUTPUT_CATEGORY_DESCRIPTION_TOPIC`: topic to which we are publishing the ID of the recognized action, `None` to stop the node from publishing on this topic (default=`/opendr/human_activity_recognition_description`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--model`: architecture to use for human activity recognition, options are `cox3d-s`, `cox3d-m`, `cox3d-l`, `x3d-xs`, `x3d-s`, `x3d-m`, or `x3d-l` (default=`cox3d-m`) - `--device DEVICE`: device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) @@ -669,6 +689,7 @@ whose documentation can be found [here](../../../../docs/reference/gem.md). - `-oc or --output_rgb_image_topic OUTPUT_RGB_IMAGE_TOPIC`: topic name for output annotated RGB image, `None` to stop the node from publishing on this topic (default=`/opendr/rgb_image_objects_annotated`) - `-oi or --output_infra_image_topic OUTPUT_INFRA_IMAGE_TOPIC`: topic name for output annotated infrared image, `None` to stop the node from publishing on this topic (default=`/opendr/infra_image_objects_annotated`) - `-d or --detections_topic DETECTIONS_TOPIC`: topic name for detection messages, `None` to stop the node from publishing on this topic (default=`/opendr/objects`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) 5. Default output topics: @@ -702,6 +723,7 @@ whose documentation can be found [here](../../../../docs/reference/rgbd-hand-ges - `-ic or --input_rgb_image_topic INPUT_RGB_IMAGE_TOPIC`: topic name for input RGB image (default=`/kinect2/qhd/image_color_rect`) - `-id or --input_depth_image_topic INPUT_DEPTH_IMAGE_TOPIC`: topic name for input depth image (default=`/kinect2/qhd/image_depth_rect`) - `-o or --output_gestures_topic OUTPUT_GESTURES_TOPIC`: topic name for predicted gesture class (default=`/opendr/gestures`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) 3. Default output topics: @@ -732,6 +754,7 @@ whose documentation can be found [here](../../../../docs/reference/audiovisual-e - `-iv or --input_video_topic INPUT_VIDEO_TOPIC`: topic name for input video, expects detected face of size 224x224 (default=`/usb_cam/image_raw`) - `-ia or --input_audio_topic INPUT_AUDIO_TOPIC`: topic name for input audio (default=`/audio/audio`) - `-o or --output_emotions_topic OUTPUT_EMOTIONS_TOPIC`: topic to which we are publishing the predicted emotion (default=`/opendr/audiovisual_emotion`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--buffer_size BUFFER_SIZE`: length of audio and video in seconds, (default=`3.6`) - `--model_path MODEL_PATH`: if given, the pretrained model will be loaded from the specified local path, otherwise it will be downloaded from an OpenDR FTP server @@ -766,6 +789,7 @@ whose documentation can be found here: - `-h or --help`: show a help message and exit - `-i or --input_audio_topic INPUT_AUDIO_TOPIC`: topic name for input audio (default=`/audio/audio`) - `-o or --output_speech_command_topic OUTPUT_SPEECH_COMMAND_TOPIC`: topic name for speech command output (default=`/opendr/speech_recognition`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--buffer_size BUFFER_SIZE`: set the size of the audio buffer (expected command duration) in seconds (default=`1.5`) - `--model MODEL`: the model to use, choices are `matchboxnet`, `edgespeechnets` or `quad_selfonn` (default=`matchboxnet`) - `--model_path MODEL_PATH`: if given, the pretrained model will be loaded from the specified local path, otherwise it will be downloaded from an OpenDR FTP server @@ -803,6 +827,7 @@ whose documentation can be found [here](../../../../docs/reference/voxel-object- - `-h or --help`: show a help message and exit - `-i or --input_point_cloud_topic INPUT_POINT_CLOUD_TOPIC`: point cloud topic provided by either a point_cloud_dataset_node or any other 3D point cloud node (default=`/opendr/dataset_point_cloud`) - `-d or --detections_topic DETECTIONS_TOPIC`: topic name for detection messages (default=`/opendr/objects3d`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) - `-n or --model_name MODEL_NAME`: name of the trained model (default=`tanet_car_xyres_16`) - `-c or --model_config_path MODEL_CONFIG_PATH`: path to a model .proto config (default=`../../src/opendr/perception/object_detection3d/voxel_object_detection_3d/second_detector/configs/tanet/car/xyres_16.proto`) @@ -836,6 +861,7 @@ whose documentation can be found [here](../../../../docs/reference/object-tracki - `-i or --input_point_cloud_topic INPUT_POINT_CLOUD_TOPIC`: point cloud topic provided by either a point_cloud_dataset_node or any other 3D point cloud node (default=`/opendr/dataset_point_cloud`) - `-d or --detections_topic DETECTIONS_TOPIC`: topic name for detection messages, `None` to stop the node from publishing on this topic (default=`/opendr/objects3d`) - `-t or --tracking3d_id_topic TRACKING3D_ID_TOPIC`: topic name for output tracking IDs with the same element count as in detection topic, `None` to stop the node from publishing on this topic (default=`/opendr/objects_tracking_id`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) - `-dn or --detector_model_name DETECTOR_MODEL_NAME`: name of the trained model (default=`tanet_car_xyres_16`) - `-dc or --detector_model_config_path DETECTOR_MODEL_CONFIG_PATH`: path to a model .proto config (default=`../../src/opendr/perception/object_detection3d/voxel_object_detection_3d/second_detector/configs/tanet/car/xyres_16.proto`) @@ -870,6 +896,7 @@ and additional information about EfficientLPS [here](../../../../src/opendr/perc The following optional arguments are available: - `-h, --help`: show a help message and exit - `-i or --input_point_cloud_2_topic INPUT_POINTCLOUD2_TOPIC` : Point Cloud 2 topic provided by either a point_cloud_2_publisher_node or any other 3D Point Cloud 2 Node (default=`/opendr/dataset_point_cloud2`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `-c or --checkpoint CHECKPOINT` : download pretrained models [semantickitti] or load from the provided path (default=`semantickitti`) - `-o or --output_heatmap_pointcloud_topic OUTPUT_HEATMAP_POINTCLOUD_TOPIC`: publish the 3D heatmap pointcloud on `OUTPUT_HEATMAP_POINTCLOUD_TOPIC` (default=`/opendr/panoptic`) @@ -901,6 +928,7 @@ The node makes use of the toolkit's heart anomaly detection tools: [ANBOF tool]( - `-h or --help`: show a help message and exit - `-i or --input_ecg_topic INPUT_ECG_TOPIC`: topic name for input ECG data (default=`/ecg/ecg`) - `-o or --output_heart_anomaly_topic OUTPUT_HEART_ANOMALY_TOPIC`: topic name for heart anomaly detection (default=`/opendr/heart_anomaly`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) - `--model MODEL`: the model to use, choices are `anbof` or `gru` (default=`anbof`) @@ -975,4 +1003,29 @@ The following optional arguments are available: - `-d or --dataset_path DATASET_PATH`: path of the SemanticKITTI dataset to publish the point cloud 2 message (default=`./datasets/semantickitti`) - `-s or --split SPLIT`: split of the dataset to use, only (train, valid, test) are available (default=`valid`) - `-o or --output_point_cloud_2_topic OUTPUT_POINT_CLOUD_2_TOPIC`: topic name to publish the data (default=`/opendr/dataset_point_cloud2`) - - `-t or --test_data`: Add this argument if you want to only test this node with the test data available in our server \ No newline at end of file + - `-t or --test_data`: Add this argument if you want to only test this node with the test data available in our server + +---- +## Utility ROS Nodes + +### Performance ROS Node + +The performance node is used to subscribe to the optional performance topic of a running node and log its performance in terms of the time it +took to process a single input and produce output and in terms of frames per second. It uses a modifiable rolling window to calculate the average FPS. + +You can inspect [the node](./scripts/performance_node.py) and modify it to your needs. + +#### Instructions for basic usage: + +1. Start the node you want to benchmark as usual but also set the optional argument `--performance_topic` to, for example, `/opendr/performance` +2. Start the performance node: + ```shell + rosrun opendr_perception performance_node.py + ``` + The following optional arguments are available: + - `-h or --help`: show a help message and exit + - `-i or --input_performance_topic INPUT_PERFORMANCE_TOPIC`: topic name for input performance data (default=`/opendr/performance`) + - `-w or --window WINDOW`: the window to use in number of frames to calculate the running average FPS (default=`20`) + +Note that the `input_performance_topic` of the performance node must match the `performance_topic` of the running node. +Also note that the running node should properly get input and produce output to publish performance messages for the performance node to use. diff --git a/projects/opendr_ws/src/opendr_perception/scripts/audiovisual_emotion_recognition_node.py b/projects/opendr_ws/src/opendr_perception/scripts/audiovisual_emotion_recognition_node.py index c165c48635..89920b7c8e 100755 --- a/projects/opendr_ws/src/opendr_perception/scripts/audiovisual_emotion_recognition_node.py +++ b/projects/opendr_ws/src/opendr_perception/scripts/audiovisual_emotion_recognition_node.py @@ -20,9 +20,11 @@ import torch import librosa import cv2 +from time import perf_counter import rospy import message_filters +from std_msgs.msg import Float32 from sensor_msgs.msg import Image as ROS_Image from audio_common_msgs.msg import AudioData from vision_msgs.msg import Classification2D @@ -36,7 +38,8 @@ class AudiovisualEmotionNode: def __init__(self, input_video_topic="/usb_cam/image_raw", input_audio_topic="/audio/audio", - output_emotions_topic="/opendr/audiovisual_emotion", buffer_size=3.6, device="cuda"): + output_emotions_topic="/opendr/audiovisual_emotion", performance_topic=None, + buffer_size=3.6, device="cuda"): """ Creates a ROS Node for audiovisual emotion recognition :param input_video_topic: Topic from which we are reading the input video. Expects detected face of size 224x224 @@ -45,6 +48,9 @@ def __init__(self, input_video_topic="/usb_cam/image_raw", input_audio_topic="/a :type input_audio_topic: str :param output_emotions_topic: Topic to which we are publishing the predicted class :type output_emotions_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param buffer_size: length of audio and video in sec :type buffer_size: float :param device: device on which we are running inference ('cpu' or 'cuda') @@ -56,6 +62,11 @@ def __init__(self, input_video_topic="/usb_cam/image_raw", input_audio_topic="/a self.input_video_topic = input_video_topic self.input_audio_topic = input_audio_topic + if performance_topic is not None: + self.performance_publisher = rospy.Publisher(performance_topic, Float32, queue_size=1) + else: + self.performance_publisher = None + self.bridge = ROSBridge() # Initialize the gesture recognition @@ -69,7 +80,7 @@ def __init__(self, input_video_topic="/usb_cam/image_raw", input_audio_topic="/a self.video_buffer = np.zeros((1, 224, 224, 3)) self.video_transform = transforms.Compose([ - transforms.ToTensor(255)]) + transforms.ToTensor(255)]) def listen(self): """ @@ -94,7 +105,9 @@ def callback(self, image_data, audio_data): :param audio_data: input audio message, speech :type audio_data: audio_common_msgs.msg.AudioData """ - audio_data = np.reshape(np.frombuffer(audio_data.data, dtype=np.int16)/32768.0, (1, -1)) + if self.performance_publisher: + start_time = perf_counter() + audio_data = np.reshape(np.frombuffer(audio_data.data, dtype=np.int16) / 32768.0, (1, -1)) self.data_buffer = np.append(self.data_buffer, audio_data) image_data = self.bridge.from_ros_image(image_data, encoding='bgr8').convert(format='channels_last') @@ -102,11 +115,11 @@ def callback(self, image_data, audio_data): self.video_buffer = np.append(self.video_buffer, np.expand_dims(image_data.data, 0), axis=0) - if self.data_buffer.shape[0] > 16000*self.buffer_size: + if self.data_buffer.shape[0] > 16000 * self.buffer_size: audio = librosa.feature.mfcc(self.data_buffer[1:], sr=16000, n_mfcc=10) audio = Timeseries(audio) - to_select = select_distributed(15, len(self.video_buffer)-1) + to_select = select_distributed(15, len(self.video_buffer) - 1) video = self.video_buffer[1:][to_select] video = [self.video_transform(img) for img in video] @@ -114,6 +127,13 @@ def callback(self, image_data, audio_data): class_pred = self.avlearner.infer(audio, video) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + # Publish output ros_class = self.bridge.from_category_to_rosclass(class_pred) self.publisher.publish(ros_class) @@ -122,7 +142,7 @@ def callback(self, image_data, audio_data): self.video_buffer = np.zeros((1, 224, 224, 3)) -def select_distributed(m, n): return [i*n//m + n//(2*m) for i in range(m)] +def select_distributed(m, n): return [i * n // m + n // (2 * m) for i in range(m)] if __name__ == '__main__': @@ -133,6 +153,8 @@ def select_distributed(m, n): return [i*n//m + n//(2*m) for i in range(m)] help="Listen to audio input data on this topic") parser.add_argument("-o", "--output_emotions_topic", type=str, default="/opendr/audiovisual_emotion", help="Topic name for output emotions recognition") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) parser.add_argument("--device", type=str, default="cuda", help="Device to use (cpu, cuda)", choices=["cuda", "cpu"]) parser.add_argument("--buffer_size", type=float, default=3.6, @@ -154,5 +176,6 @@ def select_distributed(m, n): return [i*n//m + n//(2*m) for i in range(m)] avnode = AudiovisualEmotionNode(input_video_topic=args.input_video_topic, input_audio_topic=args.input_audio_topic, output_emotions_topic=args.output_emotions_topic, + performance_topic=args.performance_topic, buffer_size=args.buffer_size, device=device) avnode.listen() diff --git a/projects/opendr_ws/src/opendr_perception/scripts/binary_high_resolution_node.py b/projects/opendr_ws/src/opendr_perception/scripts/binary_high_resolution_node.py index 80b1519d7d..a8d18cbcfc 100644 --- a/projects/opendr_ws/src/opendr_perception/scripts/binary_high_resolution_node.py +++ b/projects/opendr_ws/src/opendr_perception/scripts/binary_high_resolution_node.py @@ -17,8 +17,10 @@ import numpy as np import torch import cv2 +from time import perf_counter import rospy +from std_msgs.msg import Float32 from sensor_msgs.msg import Image as ROS_Image from opendr_bridge import ROSBridge @@ -29,7 +31,7 @@ class BinaryHighResolutionNode: def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", output_heatmap_topic="/opendr/binary_hr_heatmap", - output_rgb_image_topic="/opendr/binary_hr_heatmap_visualization", + output_rgb_image_topic="/opendr/binary_hr_heatmap_visualization", performance_topic=None, model_path=None, architecture="VGG_720p", device="cuda"): """ Create a ROS Node for binary high resolution classification with Binary High Resolution. @@ -40,6 +42,9 @@ def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", output_heatmap_to :param output_rgb_image_topic: Topic to which we are publishing the heatmap image blended with the input image for visualization purposes :type output_rgb_image_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param model_path: The path to the directory of a trained model :type model_path: str :param architecture: Architecture used on trained model (`VGG_720p` or `VGG_1080p`) @@ -59,6 +64,11 @@ def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", output_heatmap_to else: self.visualization_publisher = None + if performance_topic is not None: + self.performance_publisher = rospy.Publisher(performance_topic, Float32, queue_size=1) + else: + self.performance_publisher = None + self.bridge = ROSBridge() # Initialize the binary high resolution model @@ -94,12 +104,21 @@ def callback(self, data): :param data: Input image message :type data: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image image = self.bridge.from_ros_image(data, encoding='bgr8') image = image.convert("channels_last") # Run learner to retrieve the OpenDR heatmap heatmap = self.learner.infer(image) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + # Publish heatmap in the form of an image if self.heatmap_publisher is not None: self.heatmap_publisher.publish(self.bridge.to_ros_image(heatmap)) @@ -126,6 +145,8 @@ def main(): "blended with the input image for visualization purposes", type=lambda value: value if value.lower() != "none" else None, default="/opendr/binary_hr_heatmap_visualization") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) parser.add_argument("-m", "--model_path", help="Path to the directory of the trained model", type=str, default="test_model") parser.add_argument("-a", "--architecture", help="Architecture used for the trained model, either \"VGG_720p\" or " @@ -152,6 +173,7 @@ def main(): input_rgb_image_topic=args.input_rgb_image_topic, output_heatmap_topic=args.output_heatmap_topic, output_rgb_image_topic=args.output_rgb_image_topic, + performance_topic=args.performance_topic, model_path=args.model_path, architecture=args.architecture) binary_hr_node.listen() diff --git a/projects/opendr_ws/src/opendr_perception/scripts/continual_skeleton_based_action_recognition_node.py b/projects/opendr_ws/src/opendr_perception/scripts/continual_skeleton_based_action_recognition_node.py index 7f0864e692..2c16642785 100644 --- a/projects/opendr_ws/src/opendr_perception/scripts/continual_skeleton_based_action_recognition_node.py +++ b/projects/opendr_ws/src/opendr_perception/scripts/continual_skeleton_based_action_recognition_node.py @@ -16,9 +16,10 @@ import argparse import torch import numpy as np +from time import perf_counter import rospy -from std_msgs.msg import String +from std_msgs.msg import String, Float32 from vision_msgs.msg import ObjectHypothesis from sensor_msgs.msg import Image as ROS_Image from opendr_bridge.msg import OpenDRPose2D @@ -37,6 +38,7 @@ def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", pose_annotations_topic="/opendr/poses", output_category_topic="/opendr/continual_skeleton_recognized_action", output_category_description_topic="/opendr/continual_skeleton_recognized_action_description", + performance_topic=None, device="cuda", model='costgcn'): """ Creates a ROS Node for continual skeleton-based action recognition. @@ -54,6 +56,9 @@ def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", :param output_category_description_topic: Topic to which we are publishing the description of the recognized action (if None, we are not publishing the description) :type output_category_description_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str :param model: model to use for continual skeleton-based action recognition. @@ -85,6 +90,11 @@ def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", else: self.string_publisher = None + if performance_topic is not None: + self.performance_publisher = rospy.Publisher(performance_topic, Float32, queue_size=1) + else: + self.performance_publisher = None + # Initialize the pose estimation self.pose_estimator = LightweightOpenPoseLearner(device=device, num_refinement_stages=2, mobilenet_use_stride=False, @@ -117,6 +127,8 @@ def callback(self, data): :param data: input message :type data: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image image = self.bridge.from_ros_image(data, encoding='bgr8') @@ -127,6 +139,22 @@ def callback(self, data): # select two poses with the highest energy poses = _select_2_poses(poses) + num_frames = 1 + poses_list = [poses] + skeleton_seq = _pose2numpy(num_frames, poses_list) + + # Run action recognition + result = self.action_classifier.infer(skeleton_seq) # input_size: BxCxTxVxS + category = result[0] + category.confidence = float(category.confidence.max()) + + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + # Publish detections in ROS message if self.pose_publisher is not None: for pose in poses: @@ -142,15 +170,6 @@ def callback(self, data): # Convert the annotated OpenDR image to ROS image message using bridge and publish it self.image_publisher.publish(self.bridge.to_ros_image(Image(image), encoding='bgr8')) - num_frames = 1 - poses_list = [poses] - skeleton_seq = _pose2numpy(num_frames, poses_list) - - # Run action recognition - result = self.action_classifier.infer(skeleton_seq) # input_size: BxCxTxVxS - category = result[0] - category.confidence = float(category.confidence.max()) - if self.hypothesis_publisher is not None: self.hypothesis_publisher.publish(self.bridge.to_ros_category(category)) @@ -199,6 +218,8 @@ def main(): "recognized action category", type=lambda value: value if value.lower() != "none" else None, default="/opendr/continual_skeleton_recognized_action_description") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) parser.add_argument("--device", help="Device to use, either \"cpu\" or \"cuda\"", type=str, default="cuda", choices=["cuda", "cpu"]) parser.add_argument("--model", help="Model to use, either \"costgcn\"", @@ -225,6 +246,7 @@ def main(): pose_annotations_topic=args.pose_annotations_topic, output_category_topic=args.output_category_topic, output_category_description_topic=args.output_category_description_topic, + performance_topic=args.performance_topic, device=device, model=args.model) continual_skeleton_action_recognition_node.listen() diff --git a/projects/opendr_ws/src/opendr_perception/scripts/face_detection_retinaface_node.py b/projects/opendr_ws/src/opendr_perception/scripts/face_detection_retinaface_node.py index 6549516116..944c216c52 100755 --- a/projects/opendr_ws/src/opendr_perception/scripts/face_detection_retinaface_node.py +++ b/projects/opendr_ws/src/opendr_perception/scripts/face_detection_retinaface_node.py @@ -14,9 +14,11 @@ # limitations under the License. import argparse +from time import perf_counter import mxnet as mx import rospy +from std_msgs.msg import Float32 from vision_msgs.msg import Detection2DArray from sensor_msgs.msg import Image as ROS_Image from opendr_bridge import ROSBridge @@ -30,7 +32,7 @@ class FaceDetectionNode: def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", output_rgb_image_topic="/opendr/image_faces_annotated", detections_topic="/opendr/faces", - device="cuda", backbone="resnet"): + performance_topic=None, device="cuda", backbone="resnet"): """ Creates a ROS Node for face detection with Retinaface. :param input_rgb_image_topic: Topic from which we are reading the input image @@ -41,6 +43,9 @@ def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", :param detections_topic: Topic to which we are publishing the annotations (if None, no face detection message is published) :type detections_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str :param backbone: retinaface backbone, options are either 'mnet' or 'resnet', @@ -59,6 +64,11 @@ def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", else: self.face_publisher = None + if performance_topic is not None: + self.performance_publisher = rospy.Publisher(performance_topic, Float32, queue_size=1) + else: + self.performance_publisher = None + self.bridge = ROSBridge() # Initialize the face detector @@ -82,16 +92,24 @@ def callback(self, data): :param data: Input image message :type data: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image image = self.bridge.from_ros_image(data, encoding='bgr8') # Run face detection boxes = self.face_detector.infer(image) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + # Publish detections in ROS message - ros_boxes = self.bridge.to_ros_boxes(boxes) # Convert to ROS boxes if self.face_publisher is not None: - self.face_publisher.publish(ros_boxes) + self.face_publisher.publish(self.bridge.to_ros_boxes(boxes)) if self.image_publisher is not None: # Get an OpenCV image back @@ -112,6 +130,8 @@ def main(): parser.add_argument("-d", "--detections_topic", help="Topic name for detection messages", type=lambda value: value if value.lower() != "none" else None, default="/opendr/faces") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) 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("--backbone", @@ -136,7 +156,8 @@ def main(): face_detection_node = FaceDetectionNode(device=device, backbone=args.backbone, input_rgb_image_topic=args.input_rgb_image_topic, output_rgb_image_topic=args.output_rgb_image_topic, - detections_topic=args.detections_topic) + detections_topic=args.detections_topic, + performance_topic=args.performance_topic) face_detection_node.listen() diff --git a/projects/opendr_ws/src/opendr_perception/scripts/face_recognition_node.py b/projects/opendr_ws/src/opendr_perception/scripts/face_recognition_node.py index 358541285c..b4012998ed 100755 --- a/projects/opendr_ws/src/opendr_perception/scripts/face_recognition_node.py +++ b/projects/opendr_ws/src/opendr_perception/scripts/face_recognition_node.py @@ -16,9 +16,10 @@ import argparse import cv2 import torch +from time import perf_counter import rospy -from std_msgs.msg import String +from std_msgs.msg import String, Float32 from vision_msgs.msg import ObjectHypothesis from sensor_msgs.msg import Image as ROS_Image from opendr_bridge import ROSBridge @@ -34,7 +35,7 @@ class FaceRecognitionNode: def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", output_rgb_image_topic="/opendr/image_face_reco_annotated", detections_topic="/opendr/face_recognition", detections_id_topic="/opendr/face_recognition_id", - database_path="./database", device="cuda", backbone="mobilefacenet"): + performance_topic=None, database_path="./database", device="cuda", backbone="mobilefacenet"): """ Creates a ROS Node for face recognition. :param input_rgb_image_topic: Topic from which we are reading the input image @@ -48,6 +49,9 @@ def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", :param detections_id_topic: Topic to which we are publishing the ID of the recognized person (if None, no ID message is published) :type detections_id_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: Device on which we are running inference ('cpu' or 'cuda') :type device: str :param backbone: Backbone network @@ -72,6 +76,11 @@ def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", else: self.face_id_publisher = None + if performance_topic is not None: + self.performance_publisher = rospy.Publisher(performance_topic, Float32, queue_size=1) + else: + self.performance_publisher = None + self.bridge = ROSBridge() # Initialize the face recognizer @@ -101,6 +110,8 @@ def callback(self, data): :param data: Input image message :type data: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image image = self.bridge.from_ros_image(data, encoding='bgr8') # Get an OpenCV image back @@ -117,6 +128,13 @@ def callback(self, data): frame = image[startY:endY, startX:endX] result = self.recognizer.infer(frame) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + # Publish face information and ID if self.face_publisher is not None: self.face_publisher.publish(self.bridge.to_ros_face(result)) @@ -152,6 +170,8 @@ def main(): parser.add_argument("-id", "--detections_id_topic", help="Topic name for detection ID messages", type=lambda value: value if value.lower() != "none" else None, default="/opendr/face_recognition_id") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) 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("--backbone", help="Backbone network, defaults to mobilefacenet", @@ -179,7 +199,8 @@ def main(): input_rgb_image_topic=args.input_rgb_image_topic, output_rgb_image_topic=args.output_rgb_image_topic, detections_topic=args.detections_topic, - detections_id_topic=args.detections_id_topic) + detections_id_topic=args.detections_id_topic, + performance_topic=args.performance_topic) face_recognition_node.listen() diff --git a/projects/opendr_ws/src/opendr_perception/scripts/facial_emotion_estimation_node.py b/projects/opendr_ws/src/opendr_perception/scripts/facial_emotion_estimation_node.py index cd2ed65c88..673ec1dda9 100644 --- a/projects/opendr_ws/src/opendr_perception/scripts/facial_emotion_estimation_node.py +++ b/projects/opendr_ws/src/opendr_perception/scripts/facial_emotion_estimation_node.py @@ -19,9 +19,10 @@ import cv2 from torchvision import transforms import PIL +from time import perf_counter import rospy -from std_msgs.msg import String +from std_msgs.msg import String, Float32 from vision_msgs.msg import ObjectHypothesis from sensor_msgs.msg import Image as ROS_Image from opendr_bridge import ROSBridge @@ -44,6 +45,7 @@ def __init__(self, output_rgb_image_topic="/opendr/image_emotion_estimation_annotated", output_emotions_topic="/opendr/facial_emotion_estimation", output_emotions_description_topic="/opendr/facial_emotion_estimation_description", + performance_topic=None, device="cuda"): """ Creates a ROS Node for facial emotion estimation. @@ -58,6 +60,9 @@ def __init__(self, :param output_emotions_description_topic: Topic to which we are publishing the description of the estimated facial emotion (if None, we are not publishing the description) :type output_emotions_description_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str """ @@ -81,6 +86,11 @@ def __init__(self, else: self.string_publisher = None + if performance_topic is not None: + self.performance_publisher = rospy.Publisher(performance_topic, Float32, queue_size=1) + else: + self.performance_publisher = None + self.face_detector = face_detector_learner # Initialize the facial emotion estimator @@ -107,6 +117,8 @@ def callback(self, data): :param data: input message :type data: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() image = self.bridge.from_ros_image(data, encoding='bgr8').opencv() emotion = None @@ -131,6 +143,13 @@ def callback(self, data): affect = affect[0] # a numpy array of valence and arousal values emotion = emotion[0] # the emotion class with confidence tensor + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + cv2.rectangle(image, (startX, startY), (endX, endY), (0, 255, 255), thickness=2) cv2.putText(image, "Valence: %.2f" % affect[0], (startX, endY - 30), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 255), 1, cv2.LINE_AA) @@ -179,6 +198,8 @@ def _pre_process_input_image(image): help='Topic to which we are publishing the description of the estimated facial emotion', type=lambda value: value if value.lower() != "none" else None, default="/opendr/facial_emotion_estimation_description") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) parser.add_argument('-d', '--device', help='Device to use, either cpu or cuda', type=str, default="cuda", choices=["cuda", "cpu"]) args = parser.parse_args() @@ -208,6 +229,7 @@ def _pre_process_input_image(image): output_rgb_image_topic=args.output_rgb_image_topic, output_emotions_topic=args.output_emotions_topic, output_emotions_description_topic=args.output_emotions_description_topic, + performance_topic=args.performance_topic, device=device) facial_emotion_estimation_node.listen() diff --git a/projects/opendr_ws/src/opendr_perception/scripts/fall_detection_node.py b/projects/opendr_ws/src/opendr_perception/scripts/fall_detection_node.py index b21251e624..e52f8db6f4 100755 --- a/projects/opendr_ws/src/opendr_perception/scripts/fall_detection_node.py +++ b/projects/opendr_ws/src/opendr_perception/scripts/fall_detection_node.py @@ -16,8 +16,10 @@ import cv2 import argparse import torch +from time import perf_counter import rospy +from std_msgs.msg import Float32 from vision_msgs.msg import Detection2DArray from sensor_msgs.msg import Image as ROS_Image from opendr_bridge import ROSBridge @@ -33,7 +35,7 @@ class FallDetectionNode: def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", output_rgb_image_topic="/opendr/image_fallen_annotated", detections_topic="/opendr/fallen", - device="cuda", num_refinement_stages=2, use_stride=False, half_precision=False): + performance_topic=None, device="cuda", num_refinement_stages=2, use_stride=False, half_precision=False): """ Creates a ROS Node for rule-based fall detection based on Lightweight OpenPose. :param input_rgb_image_topic: Topic from which we are reading the input image @@ -44,6 +46,9 @@ def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", :param detections_topic: Topic to which we are publishing the annotations (if None, no fall detection message is published) :type detections_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str :param num_refinement_stages: Specifies the number of pose estimation refinement stages are added on the @@ -69,6 +74,11 @@ def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", else: self.fall_publisher = None + if performance_topic is not None: + self.performance_publisher = rospy.Publisher(performance_topic, Float32, queue_size=1) + else: + self.performance_publisher = None + self.bridge = ROSBridge() # Initialize the pose estimation learner @@ -96,15 +106,14 @@ def callback(self, data): :param data: Input image message :type data: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image image = self.bridge.from_ros_image(data, encoding='bgr8') # Run fall detection detections = self.fall_detector.infer(image) - # Get an OpenCV image back - image = image.opencv() - bboxes = BoundingBoxList([]) fallen_pose_id = 0 for detection in detections: @@ -113,7 +122,15 @@ def callback(self, data): if fallen == 1: pose = detection[1] x, y, w, h = get_bbox(pose) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) if self.image_publisher is not None: + # Get an OpenCV image back + image = image.opencv() # Paint person bounding box inferred from pose color = (0, 0, 255) cv2.rectangle(image, (x, y), (x + w, y + h), color, 2) @@ -143,6 +160,8 @@ def main(): parser.add_argument("-d", "--detections_topic", help="Topic name for detection messages", type=lambda value: value if value.lower() != "none" else None, default="/opendr/fallen") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) 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("--accelerate", help="Enables acceleration flags (e.g., stride)", default=False, @@ -175,6 +194,7 @@ def main(): input_rgb_image_topic=args.input_rgb_image_topic, output_rgb_image_topic=args.output_rgb_image_topic, detections_topic=args.detections_topic, + performance_topic=args.performance_topic, num_refinement_stages=stages, use_stride=stride, half_precision=half_prec) fall_detection_node.listen() diff --git a/projects/opendr_ws/src/opendr_perception/scripts/heart_anomaly_detection_node.py b/projects/opendr_ws/src/opendr_perception/scripts/heart_anomaly_detection_node.py index abe142b989..9b6d764719 100755 --- a/projects/opendr_ws/src/opendr_perception/scripts/heart_anomaly_detection_node.py +++ b/projects/opendr_ws/src/opendr_perception/scripts/heart_anomaly_detection_node.py @@ -16,10 +16,11 @@ import argparse import torch +from time import perf_counter import rospy from vision_msgs.msg import Classification2D -from std_msgs.msg import Float32MultiArray +from std_msgs.msg import Float32MultiArray, Float32 from opendr_bridge import ROSBridge from opendr.perception.heart_anomaly_detection import GatedRecurrentUnitLearner, AttentionNeuralBagOfFeatureLearner @@ -28,13 +29,16 @@ class HeartAnomalyNode: def __init__(self, input_ecg_topic="/ecg/ecg", output_heart_anomaly_topic="/opendr/heart_anomaly", - device="cuda", model="anbof"): + performance_topic=None, device="cuda", model="anbof"): """ Creates a ROS Node for heart anomaly (atrial fibrillation) detection from ecg data :param input_ecg_topic: Topic from which we are reading the input array data :type input_ecg_topic: str :param output_heart_anomaly_topic: Topic to which we are publishing the predicted class :type output_heart_anomaly_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str :param model: model to use: anbof or gru @@ -45,6 +49,11 @@ def __init__(self, input_ecg_topic="/ecg/ecg", output_heart_anomaly_topic="/open rospy.Subscriber(input_ecg_topic, Float32MultiArray, self.callback) + if performance_topic is not None: + self.performance_publisher = rospy.Publisher(performance_topic, Float32, queue_size=1) + else: + self.performance_publisher = None + self.bridge = ROSBridge() # AF dataset @@ -75,12 +84,21 @@ def callback(self, msg_data): :param msg_data: input message :type msg_data: std_msgs.msg.Float32MultiArray """ + if self.performance_publisher: + start_time = perf_counter() # Convert Float32MultiArray to OpenDR Timeseries data = self.bridge.from_rosarray_to_timeseries(msg_data, self.channels, self.series_length) # Run ecg classification class_pred = self.learner.infer(data) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + # Publish results ros_class = self.bridge.from_category_to_rosclass(class_pred) self.publisher.publish(ros_class) @@ -92,6 +110,8 @@ def callback(self, msg_data): help="listen to input ECG data on this topic") parser.add_argument("-o", "--output_heart_anomaly_topic", type=str, default="/opendr/heart_anomaly", help="Topic name for heart anomaly detection topic") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) parser.add_argument("--device", type=str, default="cuda", help="Device to use (cpu, cuda)", choices=["cuda", "cpu"]) parser.add_argument("--model", type=str, default="anbof", help="model to be used for prediction: anbof or gru", @@ -114,6 +134,7 @@ def callback(self, msg_data): heart_anomaly_detection_node = HeartAnomalyNode(input_ecg_topic=args.input_ecg_topic, output_heart_anomaly_topic=args.output_heart_anomaly_topic, + performance_topic=args.performance_topic, model=args.model, device=device) heart_anomaly_detection_node.listen() diff --git a/projects/opendr_ws/src/opendr_perception/scripts/hr_pose_estimation_node.py b/projects/opendr_ws/src/opendr_perception/scripts/hr_pose_estimation_node.py index 73028005bf..bcaebcf00b 100755 --- a/projects/opendr_ws/src/opendr_perception/scripts/hr_pose_estimation_node.py +++ b/projects/opendr_ws/src/opendr_perception/scripts/hr_pose_estimation_node.py @@ -15,8 +15,10 @@ import argparse import torch +from time import perf_counter import rospy +from std_msgs.msg import Float32 from sensor_msgs.msg import Image as ROS_Image from opendr_bridge.msg import OpenDRPose2D from opendr_bridge import ROSBridge @@ -26,11 +28,11 @@ from opendr.perception.pose_estimation import HighResolutionPoseEstimationLearner -class PoseEstimationNode: +class HRPoseEstimationNode: def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", output_rgb_image_topic="/opendr/image_pose_annotated", detections_topic="/opendr/poses", device="cuda", - num_refinement_stages=2, use_stride=False, half_precision=False): + performance_topic=None, num_refinement_stages=2, use_stride=False, half_precision=False): """ Creates a ROS Node for high resolution pose estimation with HR Pose Estimation. :param input_rgb_image_topic: Topic from which we are reading the input image @@ -41,6 +43,9 @@ def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", :param detections_topic: Topic to which we are publishing the annotations (if None, no pose detection message is published) :type detections_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str :param num_refinement_stages: Specifies the number of pose estimation refinement stages are added on the @@ -66,6 +71,11 @@ def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", else: self.pose_publisher = None + if performance_topic is not None: + self.performance_publisher = rospy.Publisher(performance_topic, Float32, queue_size=1) + else: + self.performance_publisher = None + self.bridge = ROSBridge() # Initialize the high resolution pose estimation learner @@ -90,12 +100,21 @@ def callback(self, data): :param data: Input image message :type data: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image image = self.bridge.from_ros_image(data, encoding='bgr8') # Run pose estimation poses = self.pose_estimator.infer(image) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + # Publish detections in ROS message if self.pose_publisher is not None: for pose in poses: @@ -124,6 +143,8 @@ def main(): parser.add_argument("-d", "--detections_topic", help="Topic name for detection messages", type=lambda value: value if value.lower() != "none" else None, default="/opendr/poses") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) 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("--accelerate", help="Enables acceleration flags (e.g., stride)", default=False, @@ -152,11 +173,12 @@ def main(): stages = 2 half_prec = False - pose_estimator_node = PoseEstimationNode(device=device, - input_rgb_image_topic=args.input_rgb_image_topic, - output_rgb_image_topic=args.output_rgb_image_topic, - detections_topic=args.detections_topic, - num_refinement_stages=stages, use_stride=stride, half_precision=half_prec) + pose_estimator_node = HRPoseEstimationNode(device=device, + input_rgb_image_topic=args.input_rgb_image_topic, + output_rgb_image_topic=args.output_rgb_image_topic, + detections_topic=args.detections_topic, + performance_topic=args.performance_topic, + num_refinement_stages=stages, use_stride=stride, half_precision=half_prec) pose_estimator_node.listen() diff --git a/projects/opendr_ws/src/opendr_perception/scripts/landmark_based_facial_expression_recognition_node.py b/projects/opendr_ws/src/opendr_perception/scripts/landmark_based_facial_expression_recognition_node.py index 8108485f6f..3ae130fff6 100755 --- a/projects/opendr_ws/src/opendr_perception/scripts/landmark_based_facial_expression_recognition_node.py +++ b/projects/opendr_ws/src/opendr_perception/scripts/landmark_based_facial_expression_recognition_node.py @@ -17,7 +17,8 @@ import rospy import torch import numpy as np -from std_msgs.msg import String +from time import perf_counter +from std_msgs.msg import String, Float32 from vision_msgs.msg import ObjectHypothesis from sensor_msgs.msg import Image as ROS_Image from opendr_bridge import ROSBridge @@ -32,7 +33,7 @@ class LandmarkFacialExpressionRecognitionNode: def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", output_category_topic="/opendr/landmark_expression_recognition", output_category_description_topic="/opendr/landmark_expression_recognition_description", - device="cpu", model='pstbln_afew', shape_predictor='./predictor_path'): + performance_topic=None, device="cpu", model='pstbln_afew', shape_predictor='./predictor_path'): """ Creates a ROS Node for landmark-based facial expression recognition. :param input_rgb_image_topic: Topic from which we are reading the input image @@ -43,6 +44,9 @@ def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", :param output_category_description_topic: Topic to which we are publishing the description of the recognized facial expression (if None, we are not publishing the description) :type output_category_description_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str :param model: model to use for landmark-based facial expression recognition. @@ -66,6 +70,11 @@ def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", else: self.string_publisher = None + if performance_topic is not None: + self.performance_publisher = rospy.Publisher(performance_topic, Float32, queue_size=1) + else: + self.performance_publisher = None + # Initialize the landmark-based facial expression recognition if model == 'pstbln_ck+': num_point = 303 @@ -81,7 +90,7 @@ def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", num_class=num_class, num_point=num_point, num_person=1, in_channels=2, blocksize=5, topology=[15, 10, 15, 5, 5, 10]) - model_saved_path = "./pretrained_models/"+model + model_saved_path = "./pretrained_models/" + model self.expression_classifier.load(model_saved_path, model) self.shape_predictor = shape_predictor @@ -100,7 +109,8 @@ def callback(self, data): :param data: input message :type data: sensor_msgs.msg.Image """ - + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image image = self.bridge.from_ros_image(data, encoding='bgr8') landmarks = landmark_extractor(image, './landmarks.npy', self.shape_predictor) @@ -114,6 +124,13 @@ def callback(self, data): # Run expression recognition category = self.expression_classifier.infer(muscle_data) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + if self.hypothesis_publisher is not None: self.hypothesis_publisher.publish(self.bridge.to_ros_category(category)) @@ -143,6 +160,8 @@ def _landmark2numpy(landmarks): parser.add_argument("-d", "--output_category_description_topic", help="Topic name for category description", type=lambda value: value if value.lower() != "none" else None, default="/opendr/landmark_expression_recognition_description") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) 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("--model", help="Model to use, either 'pstbln_ck+', 'pstbln_casia', 'pstbln_afew'", @@ -169,6 +188,7 @@ def _landmark2numpy(landmarks): input_rgb_image_topic=args.input_rgb_image_topic, output_category_topic=args.output_category_topic, output_category_description_topic=args.output_category_description_topic, + performance_topic=args.performance_topic, device=device, model=args.model, shape_predictor=args.shape_predictor) landmark_expression_estimation_node.listen() diff --git a/projects/opendr_ws/src/opendr_perception/scripts/object_detection_2d_centernet_node.py b/projects/opendr_ws/src/opendr_perception/scripts/object_detection_2d_centernet_node.py index 24d071f156..082d238f27 100755 --- a/projects/opendr_ws/src/opendr_perception/scripts/object_detection_2d_centernet_node.py +++ b/projects/opendr_ws/src/opendr_perception/scripts/object_detection_2d_centernet_node.py @@ -15,8 +15,10 @@ import argparse import mxnet as mx +from time import perf_counter import rospy +from std_msgs.msg import Float32 from vision_msgs.msg import Detection2DArray from sensor_msgs.msg import Image as ROS_Image from opendr_bridge import ROSBridge @@ -30,7 +32,7 @@ class ObjectDetectionCenterNetNode: def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", output_rgb_image_topic="/opendr/image_objects_annotated", detections_topic="/opendr/objects", - device="cuda", backbone="resnet50_v1b"): + performance_topic=None, device="cuda", backbone="resnet50_v1b"): """ Creates a ROS Node for object detection with Centernet. :param input_rgb_image_topic: Topic from which we are reading the input image @@ -41,6 +43,9 @@ def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", :param detections_topic: Topic to which we are publishing the annotations (if None, no object detection message is published) :type detections_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str :param backbone: backbone network @@ -58,6 +63,11 @@ def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", else: self.object_publisher = None + if performance_topic is not None: + self.performance_publisher = rospy.Publisher(performance_topic, Float32, queue_size=1) + else: + self.performance_publisher = None + self.bridge = ROSBridge() # Initialize the object detector @@ -80,16 +90,24 @@ def callback(self, data): :param data: input message :type data: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image image = self.bridge.from_ros_image(data, encoding='bgr8') # Run object detection boxes = self.object_detector.infer(image, threshold=0.45, keep_size=False) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + # Publish detections in ROS message - ros_boxes = self.bridge.to_ros_boxes(boxes) # Convert to ROS boxes if self.object_publisher is not None: - self.object_publisher.publish(ros_boxes) + self.object_publisher.publish(self.bridge.to_ros_boxes(boxes)) if self.image_publisher is not None: # Get an OpenCV image back @@ -110,6 +128,8 @@ def main(): parser.add_argument("-d", "--detections_topic", help="Topic name for detection messages", type=lambda value: value if value.lower() != "none" else None, default="/opendr/objects") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) parser.add_argument("--device", help="Device to use (cpu, cuda)", type=str, default="cuda", choices=["cuda", "cpu"]) parser.add_argument("--backbone", help="Backbone network, defaults to \"resnet50_v1b\"", type=str, default="resnet50_v1b", choices=["resnet50_v1b"]) @@ -131,7 +151,8 @@ def main(): object_detection_centernet_node = ObjectDetectionCenterNetNode(device=device, backbone=args.backbone, input_rgb_image_topic=args.input_rgb_image_topic, output_rgb_image_topic=args.output_rgb_image_topic, - detections_topic=args.detections_topic) + detections_topic=args.detections_topic, + performance_topic=args.performance_topic) object_detection_centernet_node.listen() diff --git a/projects/opendr_ws/src/opendr_perception/scripts/object_detection_2d_detr_node.py b/projects/opendr_ws/src/opendr_perception/scripts/object_detection_2d_detr_node.py index bfa600f009..6a64caef83 100755 --- a/projects/opendr_ws/src/opendr_perception/scripts/object_detection_2d_detr_node.py +++ b/projects/opendr_ws/src/opendr_perception/scripts/object_detection_2d_detr_node.py @@ -16,8 +16,10 @@ import argparse import torch +from time import perf_counter import rospy +from std_msgs.msg import Float32 from vision_msgs.msg import Detection2DArray from sensor_msgs.msg import Image as ROS_Image from opendr_bridge import ROSBridge @@ -29,11 +31,12 @@ class ObjectDetectionDetrNode: def __init__( - self, - input_rgb_image_topic="/usb_cam/image_raw", - output_rgb_image_topic="/opendr/image_objects_annotated", - detections_topic="/opendr/objects", - device="cuda", + self, + input_rgb_image_topic="/usb_cam/image_raw", + output_rgb_image_topic="/opendr/image_objects_annotated", + detections_topic="/opendr/objects", + performance_topic=None, + device="cuda", ): """ Creates a ROS Node for object detection with DETR. @@ -45,6 +48,9 @@ def __init__( :param detections_topic: Topic to which we are publishing the annotations (if None, no object detection message is published) :type detections_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str """ @@ -60,6 +66,11 @@ def __init__( else: self.object_publisher = None + if performance_topic is not None: + self.performance_publisher = rospy.Publisher(performance_topic, Float32, queue_size=1) + else: + self.performance_publisher = None + self.bridge = ROSBridge() self.class_names = [ @@ -175,21 +186,28 @@ def callback(self, data): :param data: input message :type data: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image image = self.bridge.from_ros_image(data, encoding="bgr8") # Run object detection boxes = self.detr_learner.infer(image) - # Get an OpenCV image back - image = image.opencv() + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) # Publish detections in ROS message - ros_boxes = self.bridge.to_ros_bounding_box_list(boxes) # Convert to ROS bounding_box_list if self.object_publisher is not None: - self.object_publisher.publish(ros_boxes) + self.object_publisher.publish(self.bridge.to_ros_bounding_box_list(boxes)) if self.image_publisher is not None: + # Get an OpenCV image back + image = image.opencv() # Annotate image with object detection boxes image = draw_bounding_boxes(image, boxes, class_names=self.class_names) # Convert the annotated OpenDR image to ROS2 image message using bridge and publish it @@ -204,6 +222,8 @@ def main(): type=str, default="/opendr/image_objects_annotated") parser.add_argument("-d", "--detections_topic", help="Topic name for detection messages", type=str, default="/opendr/objects") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) 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() @@ -224,7 +244,8 @@ def main(): object_detection_detr_node = ObjectDetectionDetrNode(device=device, input_rgb_image_topic=args.input_rgb_image_topic, output_rgb_image_topic=args.output_rgb_image_topic, - detections_topic=args.detections_topic) + detections_topic=args.detections_topic, + performance_topic=args.performance_topic) object_detection_detr_node.listen() diff --git a/projects/opendr_ws/src/opendr_perception/scripts/object_detection_2d_gem_node.py b/projects/opendr_ws/src/opendr_perception/scripts/object_detection_2d_gem_node.py index 0cb2fbcae6..67b8563e03 100755 --- a/projects/opendr_ws/src/opendr_perception/scripts/object_detection_2d_gem_node.py +++ b/projects/opendr_ws/src/opendr_perception/scripts/object_detection_2d_gem_node.py @@ -20,6 +20,8 @@ import cv2 import numpy as np import argparse +from time import perf_counter +from std_msgs.msg import Float32 from vision_msgs.msg import Detection2DArray from sensor_msgs.msg import Image as ROS_Image from opendr_bridge import ROSBridge @@ -30,15 +32,16 @@ class ObjectDetectionGemNode: def __init__( - self, - input_rgb_image_topic="/camera/color/image_raw", - input_infra_image_topic="/camera/infra/image_raw", - output_rgb_image_topic="/opendr/rgb_image_objects_annotated", - output_infra_image_topic="/opendr/infra_image_objects_annotated", - detections_topic="/opendr/objects", - device="cuda", - pts_rgb=None, - pts_infra=None, + self, + input_rgb_image_topic="/camera/color/image_raw", + input_infra_image_topic="/camera/infra/image_raw", + output_rgb_image_topic="/opendr/rgb_image_objects_annotated", + output_infra_image_topic="/opendr/infra_image_objects_annotated", + detections_topic="/opendr/objects", + performance_topic=None, + device="cuda", + pts_rgb=None, + pts_infra=None, ): """ Creates a ROS Node for object detection with GEM @@ -55,6 +58,9 @@ def __init__( :param detections_topic: Topic to which we are publishing the annotations (if None, we are not publishing annotations) :type detections_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: Device on which we are running inference ('cpu' or 'cuda') :type device: str :param pts_rgb: Point on the rgb image that define alignment with the infrared image. These are camera @@ -80,6 +86,12 @@ def __init__( self.detection_publisher = rospy.Publisher(detections_topic, Detection2DArray, queue_size=10) else: self.detection_publisher = None + + if performance_topic is not None: + self.performance_publisher = rospy.Publisher(performance_topic, Float32, queue_size=1) + else: + self.performance_publisher = None + if pts_infra is None: pts_infra = np.array( [ @@ -206,6 +218,8 @@ def callback(self, msg_rgb, msg_ir): :param msg_ir: input infrared image message :type msg_ir: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() # Convert images to OpenDR standard image_rgb = self.bridge.from_ros_image(msg_rgb).opencv() image_ir_raw = self.bridge.from_ros_image(msg_ir, "bgr8").opencv() @@ -214,6 +228,13 @@ def callback(self, msg_rgb, msg_ir): # Perform inference on images boxes, w_sensor1, _ = self.gem_learner.infer(image_rgb, image_ir) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + # Annotate image and publish results: if self.detection_publisher is not None: ros_detection = self.bridge.to_ros_bounding_box_list(boxes) @@ -244,6 +265,8 @@ def callback(self, msg_rgb, msg_ir): parser.add_argument("-d", "--detections_topic", help="Topic name for detection messages", type=lambda value: value if value.lower() != "none" else None, default="/opendr/objects") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) 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() @@ -268,5 +291,6 @@ def callback(self, msg_rgb, msg_ir): input_infra_image_topic=args.input_infra_image_topic, output_infra_image_topic=args.output_infra_image_topic, detections_topic=args.detections_topic, + performance_topic=args.performance_topic, ) detection_estimation_node.listen() diff --git a/projects/opendr_ws/src/opendr_perception/scripts/object_detection_2d_nanodet_node.py b/projects/opendr_ws/src/opendr_perception/scripts/object_detection_2d_nanodet_node.py index 0fa9487f53..3b09dd6d32 100755 --- a/projects/opendr_ws/src/opendr_perception/scripts/object_detection_2d_nanodet_node.py +++ b/projects/opendr_ws/src/opendr_perception/scripts/object_detection_2d_nanodet_node.py @@ -15,8 +15,10 @@ import argparse import torch +from time import perf_counter import rospy +from std_msgs.msg import Float32 from vision_msgs.msg import Detection2DArray from sensor_msgs.msg import Image as ROS_Image from opendr_bridge import ROSBridge @@ -30,7 +32,7 @@ class ObjectDetectionNanodetNode: def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", output_rgb_image_topic="/opendr/image_objects_annotated", detections_topic="/opendr/objects", - device="cuda", model="plus_m_1.5x_416"): + performance_topic=None, device="cuda", model="plus_m_1.5x_416"): """ Creates a ROS Node for object detection with Nanodet. :param input_rgb_image_topic: Topic from which we are reading the input image @@ -41,6 +43,9 @@ def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", :param detections_topic: Topic to which we are publishing the annotations (if None, no object detection message is published) :type detections_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str :param model: the name of the model of which we want to load the config file @@ -58,6 +63,11 @@ def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", else: self.object_publisher = None + if performance_topic is not None: + self.performance_publisher = rospy.Publisher(performance_topic, Float32, queue_size=1) + else: + self.performance_publisher = None + self.bridge = ROSBridge() # Initialize the object detector @@ -80,21 +90,28 @@ def callback(self, data): :param data: input message :type data: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image image = self.bridge.from_ros_image(data, encoding='bgr8') # Run object detection - boxes = self.object_detector.infer(image, threshold=0.35) + boxes = self.object_detector.infer(image, conf_threshold=0.35) - # Get an OpenCV image back - image = image.opencv() + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) # Publish detections in ROS message - ros_boxes = self.bridge.to_ros_boxes(boxes) # Convert to ROS boxes if self.object_publisher is not None: - self.object_publisher.publish(ros_boxes) + self.object_publisher.publish(self.bridge.to_ros_boxes(boxes)) if self.image_publisher is not None: + # Get an OpenCV image back + image = image.opencv() # Annotate image with object detection boxes image = draw_bounding_boxes(image, boxes, class_names=self.object_detector.classes) # Convert the annotated OpenDR image to ROS2 image message using bridge and publish it @@ -111,6 +128,8 @@ def main(): parser.add_argument("-d", "--detections_topic", help="Topic name for detection messages", type=lambda value: value if value.lower() != "none" else None, default="/opendr/objects") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) parser.add_argument("--device", help="Device to use (cpu, cuda)", type=str, default="cuda", choices=["cuda", "cpu"]) parser.add_argument("--model", help="Model that config file will be used", type=str, default="plus_m_1.5x_416") args = parser.parse_args() @@ -131,7 +150,8 @@ def main(): object_detection_nanodet_node = ObjectDetectionNanodetNode(device=device, model=args.model, input_rgb_image_topic=args.input_rgb_image_topic, output_rgb_image_topic=args.output_rgb_image_topic, - detections_topic=args.detections_topic) + detections_topic=args.detections_topic, + performance_topic=args.performance_topic) object_detection_nanodet_node.listen() diff --git a/projects/opendr_ws/src/opendr_perception/scripts/object_detection_2d_ssd_node.py b/projects/opendr_ws/src/opendr_perception/scripts/object_detection_2d_ssd_node.py index 8b831a3027..ee06245957 100755 --- a/projects/opendr_ws/src/opendr_perception/scripts/object_detection_2d_ssd_node.py +++ b/projects/opendr_ws/src/opendr_perception/scripts/object_detection_2d_ssd_node.py @@ -15,8 +15,10 @@ import argparse import mxnet as mx +from time import perf_counter import rospy +from std_msgs.msg import Float32 from vision_msgs.msg import Detection2DArray from sensor_msgs.msg import Image as ROS_Image from opendr_bridge import ROSBridge @@ -31,7 +33,7 @@ class ObjectDetectionSSDNode: def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", output_rgb_image_topic="/opendr/image_objects_annotated", detections_topic="/opendr/objects", - device="cuda", backbone="vgg16_atrous", nms_type='default'): + performance_topic=None, device="cuda", backbone="vgg16_atrous", nms_type='default'): """ Creates a ROS Node for object detection with SSD. :param input_rgb_image_topic: Topic from which we are reading the input image @@ -42,6 +44,9 @@ def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", :param detections_topic: Topic to which we are publishing the annotations (if None, no object detection message is published) :type detections_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str :param backbone: backbone network @@ -61,6 +66,11 @@ def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", else: self.object_publisher = None + if performance_topic is not None: + self.performance_publisher = rospy.Publisher(performance_topic, Float32, queue_size=1) + else: + self.performance_publisher = None + self.bridge = ROSBridge() # Initialize the object detector @@ -103,16 +113,24 @@ def callback(self, data): :param data: input message :type data: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image image = self.bridge.from_ros_image(data, encoding='bgr8') # Run object detection boxes = self.object_detector.infer(image, threshold=0.45, keep_size=False, custom_nms=self.custom_nms) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + # Publish detections in ROS message - ros_boxes = self.bridge.to_ros_boxes(boxes) # Convert to ROS boxes if self.object_publisher is not None: - self.object_publisher.publish(ros_boxes) + self.object_publisher.publish(self.bridge.to_ros_boxes(boxes)) if self.image_publisher is not None: # Get an OpenCV image back @@ -133,6 +151,8 @@ def main(): parser.add_argument("-d", "--detections_topic", help="Topic name for detection messages", type=lambda value: value if value.lower() != "none" else None, default="/opendr/objects") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) parser.add_argument("--device", help="Device to use (cpu, cuda)", type=str, default="cuda", choices=["cuda", "cpu"]) parser.add_argument("--backbone", help="Backbone network, defaults to vgg16_atrous", type=str, default="vgg16_atrous", choices=["vgg16_atrous"]) @@ -158,7 +178,8 @@ def main(): object_detection_ssd_node = ObjectDetectionSSDNode(device=device, backbone=args.backbone, nms_type=args.nms_type, input_rgb_image_topic=args.input_rgb_image_topic, output_rgb_image_topic=args.output_rgb_image_topic, - detections_topic=args.detections_topic) + detections_topic=args.detections_topic, + performance_topic=args.performance_topic) object_detection_ssd_node.listen() diff --git a/projects/opendr_ws/src/opendr_perception/scripts/object_detection_2d_yolov3_node.py b/projects/opendr_ws/src/opendr_perception/scripts/object_detection_2d_yolov3_node.py index 64a8230837..1e4f4bd8e9 100755 --- a/projects/opendr_ws/src/opendr_perception/scripts/object_detection_2d_yolov3_node.py +++ b/projects/opendr_ws/src/opendr_perception/scripts/object_detection_2d_yolov3_node.py @@ -15,8 +15,10 @@ import argparse import mxnet as mx +from time import perf_counter import rospy +from std_msgs.msg import Float32 from vision_msgs.msg import Detection2DArray from sensor_msgs.msg import Image as ROS_Image from opendr_bridge import ROSBridge @@ -26,11 +28,11 @@ from opendr.perception.object_detection_2d import draw_bounding_boxes -class ObjectDetectionYOLONode: +class ObjectDetectionYOLOV3Node: def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", output_rgb_image_topic="/opendr/image_objects_annotated", detections_topic="/opendr/objects", - device="cuda", backbone="darknet53"): + performance_topic=None, device="cuda", backbone="darknet53"): """ Creates a ROS Node for object detection with YOLOV3. :param input_rgb_image_topic: Topic from which we are reading the input image @@ -41,6 +43,9 @@ def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", :param detections_topic: Topic to which we are publishing the annotations (if None, no object detection message is published) :type detections_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str :param backbone: backbone network @@ -58,6 +63,11 @@ def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", else: self.object_publisher = None + if performance_topic is not None: + self.performance_publisher = rospy.Publisher(performance_topic, Float32, queue_size=1) + else: + self.performance_publisher = None + self.bridge = ROSBridge() # Initialize the object detector @@ -80,16 +90,24 @@ def callback(self, data): :param data: input message :type data: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image image = self.bridge.from_ros_image(data, encoding='bgr8') # Run object detection boxes = self.object_detector.infer(image, threshold=0.1, keep_size=False) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + # Publish detections in ROS message - ros_boxes = self.bridge.to_ros_bounding_box_list(boxes) # Convert to ROS bounding_box_list if self.object_publisher is not None: - self.object_publisher.publish(ros_boxes) + self.object_publisher.publish(self.bridge.to_ros_bounding_box_list(boxes)) if self.image_publisher is not None: # Get an OpenCV image back @@ -110,6 +128,8 @@ def main(): parser.add_argument("-d", "--detections_topic", help="Topic name for detection messages", type=lambda value: value if value.lower() != "none" else None, default="/opendr/objects") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) 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("--backbone", help="Backbone network, defaults to \"darknet53\"", @@ -129,10 +149,11 @@ def main(): print("Using CPU.") device = "cpu" - object_detection_yolov3_node = ObjectDetectionYOLONode(device=device, backbone=args.backbone, - input_rgb_image_topic=args.input_rgb_image_topic, - output_rgb_image_topic=args.output_rgb_image_topic, - detections_topic=args.detections_topic) + object_detection_yolov3_node = ObjectDetectionYOLOV3Node(device=device, backbone=args.backbone, + input_rgb_image_topic=args.input_rgb_image_topic, + output_rgb_image_topic=args.output_rgb_image_topic, + detections_topic=args.detections_topic, + performance_topic=args.performance_topic) object_detection_yolov3_node.listen() diff --git a/projects/opendr_ws/src/opendr_perception/scripts/object_detection_2d_yolov5_node.py b/projects/opendr_ws/src/opendr_perception/scripts/object_detection_2d_yolov5_node.py index fb0199d635..596511bacf 100644 --- a/projects/opendr_ws/src/opendr_perception/scripts/object_detection_2d_yolov5_node.py +++ b/projects/opendr_ws/src/opendr_perception/scripts/object_detection_2d_yolov5_node.py @@ -15,8 +15,10 @@ import argparse import torch +from time import perf_counter import rospy +from std_msgs.msg import Float32 from vision_msgs.msg import Detection2DArray from sensor_msgs.msg import Image as ROS_Image from opendr_bridge import ROSBridge @@ -26,11 +28,11 @@ from opendr.perception.object_detection_2d import draw_bounding_boxes -class ObjectDetectionYOLONode: +class ObjectDetectionYOLOV5Node: def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", output_rgb_image_topic="/opendr/image_objects_annotated", detections_topic="/opendr/objects", - device="cuda", model_name="yolov5s"): + performance_topic=None, device="cuda", model_name="yolov5s"): """ Creates a ROS Node for object detection with YOLOV5. :param input_rgb_image_topic: Topic from which we are reading the input image @@ -41,6 +43,9 @@ def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", :param detections_topic: Topic to which we are publishing the annotations (if None, no object detection message is published) :type detections_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str :param model_name: network architecture name @@ -58,6 +63,11 @@ def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", else: self.object_publisher = None + if performance_topic is not None: + self.performance_publisher = rospy.Publisher(performance_topic, Float32, queue_size=1) + else: + self.performance_publisher = None + self.bridge = ROSBridge() # Initialize the object detector @@ -78,16 +88,24 @@ def callback(self, data): :param data: input message :type data: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image image = self.bridge.from_ros_image(data, encoding='bgr8') # Run object detection boxes = self.object_detector.infer(image) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + # Publish detections in ROS message - ros_boxes = self.bridge.to_ros_bounding_box_list(boxes) # Convert to ROS bounding_box_list if self.object_publisher is not None: - self.object_publisher.publish(ros_boxes) + self.object_publisher.publish(self.bridge.to_ros_bounding_box_list(boxes)) if self.image_publisher is not None: # Get an OpenCV image back @@ -108,6 +126,8 @@ def main(): parser.add_argument("-d", "--detections_topic", help="Topic name for detection messages", type=lambda value: value if value.lower() != "none" else None, default="/opendr/objects") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) 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("--model_name", help="Network architecture, defaults to \"yolov5s\"", @@ -128,10 +148,11 @@ def main(): print("Using CPU.") device = "cpu" - object_detection_yolov5_node = ObjectDetectionYOLONode(device=device, model_name=args.model_name, - input_rgb_image_topic=args.input_rgb_image_topic, - output_rgb_image_topic=args.output_rgb_image_topic, - detections_topic=args.detections_topic) + object_detection_yolov5_node = ObjectDetectionYOLOV5Node(device=device, model_name=args.model_name, + input_rgb_image_topic=args.input_rgb_image_topic, + output_rgb_image_topic=args.output_rgb_image_topic, + detections_topic=args.detections_topic, + performance_topic=args.performance_topic) object_detection_yolov5_node.listen() diff --git a/projects/opendr_ws/src/opendr_perception/scripts/object_detection_3d_voxel_node.py b/projects/opendr_ws/src/opendr_perception/scripts/object_detection_3d_voxel_node.py index acb1899777..4eba14dfa1 100755 --- a/projects/opendr_ws/src/opendr_perception/scripts/object_detection_3d_voxel_node.py +++ b/projects/opendr_ws/src/opendr_perception/scripts/object_detection_3d_voxel_node.py @@ -16,7 +16,9 @@ import argparse import torch import os +from time import perf_counter import rospy +from std_msgs.msg import Float32 from vision_msgs.msg import Detection3DArray from sensor_msgs.msg import PointCloud as ROS_PointCloud from opendr_bridge import ROSBridge @@ -25,17 +27,18 @@ class ObjectDetection3DVoxelNode: 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", + performance_topic=None, + 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 ROS Node for 3D object detection @@ -43,6 +46,9 @@ def __init__( :type input_point_cloud_topic: str :param detections_topic: Topic to which we are publishing the annotations :type detections_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_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 @@ -66,17 +72,30 @@ def __init__( detections_topic, Detection3DArray, queue_size=1 ) + if performance_topic is not None: + self.performance_publisher = rospy.Publisher(performance_topic, Float32, queue_size=1) + else: + self.performance_publisher = None + 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 """ - + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image point_cloud = self.bridge.from_ros_point_cloud(data) detection_boxes = self.learner.infer(point_cloud) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + # 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) @@ -100,6 +119,8 @@ def main(): parser.add_argument("-d", "--detections_topic", help="Output detections topic", type=str, default="/opendr/objects3d") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) 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", @@ -136,6 +157,7 @@ def main(): input_point_cloud_topic=args.input_point_cloud_topic, temp_dir=args.temp_dir, detections_topic=args.detections_topic, + performance_topic=args.performance_topic ) voxel_node.listen() diff --git a/projects/opendr_ws/src/opendr_perception/scripts/object_tracking_2d_deep_sort_node.py b/projects/opendr_ws/src/opendr_perception/scripts/object_tracking_2d_deep_sort_node.py index 0a83a5bdec..2852923a52 100755 --- a/projects/opendr_ws/src/opendr_perception/scripts/object_tracking_2d_deep_sort_node.py +++ b/projects/opendr_ws/src/opendr_perception/scripts/object_tracking_2d_deep_sort_node.py @@ -17,12 +17,14 @@ import cv2 import torch import os -from opendr.engine.target import TrackingAnnotationList +from time import perf_counter import rospy +from std_msgs.msg import Float32 from vision_msgs.msg import Detection2DArray from std_msgs.msg import Int32MultiArray from sensor_msgs.msg import Image as ROS_Image from opendr_bridge import ROSBridge +from opendr.engine.target import TrackingAnnotationList from opendr.perception.object_tracking_2d import ( ObjectTracking2DDeepSortLearner, ObjectTracking2DFairMotLearner @@ -32,15 +34,16 @@ class ObjectTracking2DDeepSortNode: def __init__( - self, - detector=None, - input_rgb_image_topic="/usb_cam/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="/usb_cam/image_raw", + output_detection_topic="/opendr/objects", + output_tracking_id_topic="/opendr/objects_tracking_id", + output_rgb_image_topic="/opendr/image_objects_annotated", + performance_topic=None, + device="cuda:0", + model_name="deep_sort", + temp_dir="temp", ): """ Creates a ROS Node for 2D object tracking @@ -55,6 +58,9 @@ def __init__( :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 performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_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 @@ -90,19 +96,32 @@ def __init__( output_detection_topic, Detection2DArray, queue_size=10 ) + if performance_topic is not None: + self.performance_publisher = rospy.Publisher(performance_topic, Float32, queue_size=1) + else: + self.performance_publisher = None + 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 """ - + if self.performance_publisher: + start_time = perf_counter() # 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, swap_left_top=True) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + if self.output_image_publisher is not None: frame = image.opencv() draw_predictions(frame, tracking_boxes) @@ -194,6 +213,8 @@ def main(): 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("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) 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", @@ -232,6 +253,7 @@ def main(): output_detection_topic=args.detections_topic, output_tracking_id_topic=args.tracking_id_topic, output_rgb_image_topic=args.output_rgb_image_topic, + performance_topic=args.performance_topic ) deep_sort_node.listen() diff --git a/projects/opendr_ws/src/opendr_perception/scripts/object_tracking_2d_fair_mot_node.py b/projects/opendr_ws/src/opendr_perception/scripts/object_tracking_2d_fair_mot_node.py index 4b337a32ea..068fc37fed 100755 --- a/projects/opendr_ws/src/opendr_perception/scripts/object_tracking_2d_fair_mot_node.py +++ b/projects/opendr_ws/src/opendr_perception/scripts/object_tracking_2d_fair_mot_node.py @@ -17,12 +17,13 @@ import cv2 import torch import os -from opendr.engine.target import TrackingAnnotationList +from time import perf_counter import rospy from vision_msgs.msg import Detection2DArray -from std_msgs.msg import Int32MultiArray +from std_msgs.msg import Int32MultiArray, Float32 from sensor_msgs.msg import Image as ROS_Image from opendr_bridge import ROSBridge +from opendr.engine.target import TrackingAnnotationList from opendr.perception.object_tracking_2d import ( ObjectTracking2DFairMotLearner, ) @@ -31,14 +32,15 @@ class ObjectTracking2DFairMotNode: def __init__( - self, - input_rgb_image_topic="/usb_cam/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="fairmot_dla34", - temp_dir="temp", + self, + input_rgb_image_topic="/usb_cam/image_raw", + output_detection_topic="/opendr/objects", + output_tracking_id_topic="/opendr/objects_tracking_id", + output_rgb_image_topic="/opendr/image_objects_annotated", + performance_topic=None, + device="cuda:0", + model_name="fairmot_dla34", + temp_dir="temp", ): """ Creates a ROS Node for 2D object tracking @@ -51,6 +53,9 @@ def __init__( :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 performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_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 @@ -85,17 +90,30 @@ def __init__( output_rgb_image_topic, ROS_Image, queue_size=10 ) + if performance_topic is not None: + self.performance_publisher = rospy.Publisher(performance_topic, Float32, queue_size=1) + else: + self.performance_publisher = None + 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 """ - + if self.performance_publisher: + start_time = perf_counter() # 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.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + if self.output_image_publisher is not None: frame = image.opencv() draw_predictions(frame, tracking_boxes) @@ -188,6 +206,8 @@ def main(): 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("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) 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", @@ -217,6 +237,7 @@ def main(): output_detection_topic=args.detections_topic, output_tracking_id_topic=args.tracking_id_topic, output_rgb_image_topic=args.output_rgb_image_topic, + performance_topic=args.performance_topic ) fair_mot_node.listen() diff --git a/projects/opendr_ws/src/opendr_perception/scripts/object_tracking_2d_siamrpn_node.py b/projects/opendr_ws/src/opendr_perception/scripts/object_tracking_2d_siamrpn_node.py index 51ced976b0..4f60355756 100644 --- a/projects/opendr_ws/src/opendr_perception/scripts/object_tracking_2d_siamrpn_node.py +++ b/projects/opendr_ws/src/opendr_perception/scripts/object_tracking_2d_siamrpn_node.py @@ -15,11 +15,13 @@ import argparse import mxnet as mx +from time import perf_counter import cv2 from math import dist import rospy +from std_msgs.msg import Float32 from sensor_msgs.msg import Image as ROS_Image from vision_msgs.msg import Detection2D from opendr_bridge import ROSBridge @@ -33,7 +35,7 @@ class ObjectTrackingSiamRPNNode: def __init__(self, object_detector, input_rgb_image_topic="/usb_cam/image_raw", output_rgb_image_topic="/opendr/image_tracking_annotated", - tracker_topic="/opendr/tracked_object", + tracker_topic="/opendr/tracked_object", performance_topic=None, device="cuda"): """ Creates a ROS Node for object tracking with SiamRPN. @@ -46,6 +48,9 @@ def __init__(self, object_detector, input_rgb_image_topic="/usb_cam/image_raw", :type output_rgb_image_topic: str :param tracker_topic: Topic to which we are publishing the annotation :type tracker_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str """ @@ -61,6 +66,11 @@ def __init__(self, object_detector, input_rgb_image_topic="/usb_cam/image_raw", else: self.object_publisher = None + if performance_topic is not None: + self.performance_publisher = rospy.Publisher(performance_topic, Float32, queue_size=1) + else: + self.performance_publisher = None + self.bridge = ROSBridge() self.object_detector = object_detector @@ -84,6 +94,8 @@ def img_callback(self, data): :param data: input message :type data: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image image = self.bridge.from_ros_image(data, encoding='bgr8') self.image = image @@ -109,6 +121,14 @@ def img_callback(self, data): id=0, score=center_box.confidence) self.tracker.infer(self.image, init_box) + + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + self.initialized = True rospy.loginfo("Object Tracking 2D SiamRPN node initialized with the most central bounding box.") @@ -116,6 +136,13 @@ def img_callback(self, data): # Run object tracking box = self.tracker.infer(image) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + if self.object_publisher is not None: # Publish detections in ROS message ros_boxes = self.bridge.to_ros_single_tracking_annotation(box) @@ -141,6 +168,8 @@ def main(): parser.add_argument("-t", "--tracker_topic", help="Topic name for tracker messages", type=lambda value: value if value.lower() != "none" else None, default="/opendr/tracked_object") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) 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() @@ -165,7 +194,8 @@ def main(): object_tracker_2d_siamrpn_node = ObjectTrackingSiamRPNNode(object_detector=object_detector, device=device, input_rgb_image_topic=args.input_rgb_image_topic, output_rgb_image_topic=args.output_rgb_image_topic, - tracker_topic=args.tracker_topic) + tracker_topic=args.tracker_topic, + performance_topic=args.performance_topic) object_tracker_2d_siamrpn_node.listen() diff --git a/projects/opendr_ws/src/opendr_perception/scripts/object_tracking_3d_ab3dmot_node.py b/projects/opendr_ws/src/opendr_perception/scripts/object_tracking_3d_ab3dmot_node.py index 6a05e73fb0..088897b78f 100755 --- a/projects/opendr_ws/src/opendr_perception/scripts/object_tracking_3d_ab3dmot_node.py +++ b/projects/opendr_ws/src/opendr_perception/scripts/object_tracking_3d_ab3dmot_node.py @@ -16,9 +16,10 @@ import argparse import os import torch +from time import perf_counter import rospy from vision_msgs.msg import Detection3DArray -from std_msgs.msg import Int32MultiArray +from std_msgs.msg import Int32MultiArray, Float32 from sensor_msgs.msg import PointCloud as ROS_PointCloud from opendr_bridge import ROSBridge from opendr.perception.object_tracking_3d import ObjectTracking3DAb3dmotLearner @@ -27,12 +28,13 @@ class ObjectTracking3DAb3dmotNode: 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", + performance_topic=None, + device="cuda:0", ): """ Creates a ROS Node for 3D object tracking @@ -44,6 +46,9 @@ def __init__( :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 performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str """ @@ -66,6 +71,11 @@ def __init__( output_tracking3d_id_topic, Int32MultiArray, queue_size=10 ) + if performance_topic is not None: + self.performance_publisher = rospy.Publisher(performance_topic, Float32, queue_size=1) + else: + self.performance_publisher = None + rospy.Subscriber(input_point_cloud_topic, ROS_PointCloud, self.callback) def callback(self, data): @@ -74,11 +84,20 @@ def callback(self, data): :param data: input message :type data: sensor_msgs.msg.Image """ - + if self.performance_publisher: + start_time = perf_counter() # 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) + if self.tracking_id_publisher or self.performance_publisher: + tracking_boxes = self.learner.infer(detection_boxes) + + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) if self.detection_publisher is not None: # Convert detected boxes to ROS type and publish @@ -113,6 +132,8 @@ def main(): 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("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) 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", @@ -165,6 +186,7 @@ def main(): input_point_cloud_topic=input_point_cloud_topic, output_detection3d_topic=output_detection3d_topic, output_tracking3d_id_topic=output_tracking3d_id_topic, + performance_topic=args.performance_topic ) ab3dmot_node.listen() diff --git a/projects/opendr_ws/src/opendr_perception/scripts/panoptic_segmentation_efficient_lps_node.py b/projects/opendr_ws/src/opendr_perception/scripts/panoptic_segmentation_efficient_lps_node.py index e12cd9c020..065df1fe65 100755 --- a/projects/opendr_ws/src/opendr_perception/scripts/panoptic_segmentation_efficient_lps_node.py +++ b/projects/opendr_ws/src/opendr_perception/scripts/panoptic_segmentation_efficient_lps_node.py @@ -17,9 +17,11 @@ from pathlib import Path import argparse from typing import Optional - +from time import perf_counter import matplotlib + import rospy +from std_msgs.msg import Float32 from sensor_msgs.msg import PointCloud2 as ROS_PointCloud2 from opendr_bridge import ROSBridge @@ -35,6 +37,7 @@ def __init__(self, input_pcl_topic: str, checkpoint: str, output_heatmap_pointcloud_topic: Optional[str] = None, + performance_topic=None, ): """ Initialize the EfficientLPS ROS node and create an instance of the respective learner class. @@ -45,19 +48,27 @@ def __init__(self, :type checkpoint: str :param output_heatmap_pointcloud_topic: topic for the output 3D heatmap point cloud :type output_heatmap_pointcloud_topic: Optional[str] + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str """ self.checkpoint = checkpoint self.input_pcl_topic = input_pcl_topic self.output_heatmap_pointcloud_topic = output_heatmap_pointcloud_topic + if performance_topic is not None: + self.performance_publisher = rospy.Publisher(performance_topic, Float32, queue_size=1) + else: + self.performance_publisher = None + # Initialize all ROS related things self._bridge = ROSBridge() self._visualization_pointcloud_publisher = None # Initialize the panoptic segmentation network config_file = Path(sys.modules[ - EfficientLpsLearner.__module__].__file__).parent / 'configs' / 'singlegpu_semantickitti.py' + EfficientLpsLearner.__module__].__file__).parent / 'configs' / 'singlegpu_semantickitti.py' self._learner = EfficientLpsLearner(str(config_file)) # Other @@ -121,6 +132,8 @@ def callback(self, data: ROS_PointCloud2): :param data: PointCloud data message :type data: sensor_msgs.msg.PointCloud """ + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image to OpenDR Image pointcloud = self._bridge.from_ros_point_cloud2(data) @@ -129,6 +142,13 @@ def callback(self, data: ROS_PointCloud2): # or a list of numpy arrays: [instance labels, semantic labels] otherwise. prediction = self._learner.infer(pointcloud) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + # The output topics are only published if there is at least one subscriber if self._visualization_pointcloud_publisher is not None and \ self._visualization_pointcloud_publisher.get_num_connections() > 0: @@ -153,11 +173,14 @@ def callback(self, data: ROS_PointCloud2): help='Download pretrained models [semantickitti] or load from the provided path') parser.add_argument('-o', '--output_heatmap_pointcloud_topic', type=str, default="/opendr/panoptic", help='Publish the rgb visualization on this topic') + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) args = parser.parse_args() efficient_lps_node = EfficientLpsNode(args.input_point_cloud_2_topic, args.checkpoint, - args.output_heatmap_pointcloud_topic) + args.output_heatmap_pointcloud_topic, + performance_topic=args.performance_topic) efficient_lps_node.listen() diff --git a/projects/opendr_ws/src/opendr_perception/scripts/panoptic_segmentation_efficient_ps_node.py b/projects/opendr_ws/src/opendr_perception/scripts/panoptic_segmentation_efficient_ps_node.py index 2dcf935c26..633cf25cde 100755 --- a/projects/opendr_ws/src/opendr_perception/scripts/panoptic_segmentation_efficient_ps_node.py +++ b/projects/opendr_ws/src/opendr_perception/scripts/panoptic_segmentation_efficient_ps_node.py @@ -17,9 +17,11 @@ from pathlib import Path import argparse from typing import Optional - +from time import perf_counter import matplotlib + import rospy +from std_msgs.msg import Float32 from sensor_msgs.msg import Image as ROS_Image from opendr_bridge import ROSBridge @@ -35,7 +37,8 @@ def __init__(self, checkpoint: str, output_heatmap_topic: Optional[str] = None, output_rgb_visualization_topic: Optional[str] = None, - detailed_visualization: bool = False + detailed_visualization: bool = False, + performance_topic=None ): """ Initialize the EfficientPS ROS node and create an instance of the respective learner class. @@ -48,6 +51,9 @@ def __init__(self, :type output_heatmap_topic: str :param output_rgb_visualization_topic: ROS topic for the generated visualization of the panoptic map :type output_rgb_visualization_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param detailed_visualization: if True, generate a combined overview of the input RGB image and the semantic, instance, and panoptic segmentation maps and publish it on output_rgb_visualization_topic :type detailed_visualization: bool @@ -58,6 +64,11 @@ def __init__(self, self.output_rgb_visualization_topic = output_rgb_visualization_topic self.detailed_visualization = detailed_visualization + if performance_topic is not None: + self.performance_publisher = rospy.Publisher(performance_topic, Float32, queue_size=1) + else: + self.performance_publisher = None + # Initialize all ROS related things self._bridge = ROSBridge() self._instance_heatmap_publisher = None @@ -130,6 +141,8 @@ def callback(self, data: ROS_Image): :param data: Input image message :type data: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image to OpenDR Image image = self._bridge.from_ros_image(data) @@ -137,6 +150,13 @@ def callback(self, data: ROS_Image): # Retrieve a list of two OpenDR heatmaps: [instance map, semantic map] prediction = self._learner.infer(image) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + # The output topics are only published if there is at least one subscriber if self._visualization_publisher is not None and self._visualization_publisher.get_num_connections() > 0: panoptic_image = EfficientPsLearner.visualize(image, prediction, show_figure=False, @@ -166,6 +186,8 @@ def callback(self, data: ROS_Image): default='/opendr/panoptic/rgb_visualization', help='publish the panoptic segmentation map as an RGB image on this topic or a more detailed \ overview if using the --detailed_visualization flag') + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) parser.add_argument('--detailed_visualization', action='store_true', help='generate a combined overview of the input RGB image and the semantic, instance, and \ panoptic segmentation maps and publish it on OUTPUT_RGB_IMAGE_TOPIC') @@ -177,5 +199,6 @@ def callback(self, data: ROS_Image): args.checkpoint, args.output_heatmap_topic, args.output_rgb_image_topic, - args.detailed_visualization) + args.detailed_visualization, + performance_topic=args.performance_topic) efficient_ps_node.listen() diff --git a/projects/opendr_ws/src/opendr_perception/scripts/performance_node.py b/projects/opendr_ws/src/opendr_perception/scripts/performance_node.py new file mode 100644 index 0000000000..126445b846 --- /dev/null +++ b/projects/opendr_ws/src/opendr_perception/scripts/performance_node.py @@ -0,0 +1,76 @@ +#!/usr/bin/env python3 +# Copyright 2020-2023 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 +from numpy import mean + +import rospy +from std_msgs.msg import Float32 + + +class PerformanceNode: + + def __init__(self, input_performance_topic="/opendr/performance", window_length=20): + """ + Creates a ROS Node for subscribing to a node's performance topic to measure and log its performance in frames + per second (FPS), rolling window. + :param input_performance_topic: Topic from which we are reading the performance message + :type input_performance_topic: str + :param window_length: The length of the rolling average window + :type window_length: int + """ + self.input_performance_topic = input_performance_topic + self.fps_window = [] + self.window_length = window_length + + def listen(self): + """ + Start the node and begin processing input data. + """ + rospy.init_node('opendr_performance_node', anonymous=True) + rospy.Subscriber(self.input_performance_topic, Float32, self.callback, queue_size=1, buff_size=10000000) + rospy.loginfo("Performance node started.") + rospy.spin() + + def callback(self, data): + """ + Callback that processes the input data and publishes to the corresponding topics. + :param data: Input image message + :type data: sensor_msgs.msg.Image + """ + fps = data.data + rospy.loginfo(f"Time per inference: {str(round(1.0 / fps, 4))} sec") + while len(self.fps_window) < self.window_length: + self.fps_window.append(fps) + self.fps_window = self.fps_window[1:] + self.fps_window.append(fps) + rospy.loginfo(f"Average inferences per second: {round(mean(self.fps_window), 2)}") # NOQA + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("-i", "--input_performance_topic", help="Topic name for performance message", + type=str, default="/opendr/performance") + parser.add_argument("-w", "--window", help="The window to use in frames to calculate running average FPS", + type=int, default=20) + args = parser.parse_args() + + performance_node = PerformanceNode(input_performance_topic=args.input_performance_topic, + window_length=args.window) + performance_node.listen() + + +if __name__ == '__main__': + main() diff --git a/projects/opendr_ws/src/opendr_perception/scripts/pose_estimation_node.py b/projects/opendr_ws/src/opendr_perception/scripts/pose_estimation_node.py index 954acc3deb..255216ff4e 100755 --- a/projects/opendr_ws/src/opendr_perception/scripts/pose_estimation_node.py +++ b/projects/opendr_ws/src/opendr_perception/scripts/pose_estimation_node.py @@ -15,8 +15,10 @@ import argparse import torch +from time import perf_counter import rospy +from std_msgs.msg import Float32 from sensor_msgs.msg import Image as ROS_Image from opendr_bridge.msg import OpenDRPose2D from opendr_bridge import ROSBridge @@ -29,8 +31,8 @@ class PoseEstimationNode: def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", - output_rgb_image_topic="/opendr/image_pose_annotated", detections_topic="/opendr/poses", device="cuda", - num_refinement_stages=2, use_stride=False, half_precision=False): + output_rgb_image_topic="/opendr/image_pose_annotated", detections_topic="/opendr/poses", + performance_topic=None, device="cuda", num_refinement_stages=2, use_stride=False, half_precision=False): """ Creates a ROS Node for pose estimation with Lightweight OpenPose. :param input_rgb_image_topic: Topic from which we are reading the input image @@ -41,6 +43,9 @@ def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", :param detections_topic: Topic to which we are publishing the annotations (if None, no pose detection message is published) :type detections_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str :param num_refinement_stages: Specifies the number of pose estimation refinement stages are added on the @@ -66,6 +71,11 @@ def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", else: self.pose_publisher = None + if performance_topic is not None: + self.performance_publisher = rospy.Publisher(performance_topic, Float32, queue_size=1) + else: + self.performance_publisher = None + self.bridge = ROSBridge() # Initialize the pose estimation learner @@ -90,12 +100,21 @@ def callback(self, data): :param data: Input image message :type data: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image image = self.bridge.from_ros_image(data, encoding='bgr8') # Run pose estimation poses = self.pose_estimator.infer(image) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + # Publish detections in ROS message if self.pose_publisher is not None: for pose in poses: @@ -122,6 +141,8 @@ def main(): parser.add_argument("-d", "--detections_topic", help="Topic name for detection messages", type=lambda value: value if value.lower() != "none" else None, default="/opendr/poses") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) 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("--accelerate", help="Enables acceleration flags (e.g., stride)", default=False, @@ -154,6 +175,7 @@ def main(): input_rgb_image_topic=args.input_rgb_image_topic, output_rgb_image_topic=args.output_rgb_image_topic, detections_topic=args.detections_topic, + performance_topic=args.performance_topic, num_refinement_stages=stages, use_stride=stride, half_precision=half_prec) pose_estimator_node.listen() diff --git a/projects/opendr_ws/src/opendr_perception/scripts/rgbd_hand_gesture_recognition_node.py b/projects/opendr_ws/src/opendr_perception/scripts/rgbd_hand_gesture_recognition_node.py index 103642c597..1d663c319e 100755 --- a/projects/opendr_ws/src/opendr_perception/scripts/rgbd_hand_gesture_recognition_node.py +++ b/projects/opendr_ws/src/opendr_perception/scripts/rgbd_hand_gesture_recognition_node.py @@ -19,9 +19,11 @@ import cv2 import numpy as np import torch +from time import perf_counter import rospy import message_filters +from std_msgs.msg import Float32 from sensor_msgs.msg import Image as ROS_Image from vision_msgs.msg import Classification2D @@ -34,7 +36,8 @@ class RgbdHandGestureNode: def __init__(self, input_rgb_image_topic="/kinect2/qhd/image_color_rect", input_depth_image_topic="/kinect2/qhd/image_depth_rect", - output_gestures_topic="/opendr/gestures", device="cuda", delay=0.1): + output_gestures_topic="/opendr/gestures", + performance_topic=None, device="cuda", delay=0.1): """ Creates a ROS Node for gesture recognition from RGBD. Assuming that the following drivers have been installed: https://github.com/OpenKinect/libfreenect2 and https://github.com/code-iai/iai_kinect2. @@ -44,6 +47,9 @@ def __init__(self, input_rgb_image_topic="/kinect2/qhd/image_color_rect", :type input_depth_image_topic: str :param output_gestures_topic: Topic to which we are publishing the predicted gesture class :type output_gestures_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str :param delay: Define the delay (in seconds) with which rgb message and depth message can be synchronized @@ -56,6 +62,11 @@ def __init__(self, input_rgb_image_topic="/kinect2/qhd/image_color_rect", self.gesture_publisher = rospy.Publisher(output_gestures_topic, Classification2D, queue_size=10) + if performance_topic is not None: + self.performance_publisher = rospy.Publisher(performance_topic, Float32, queue_size=1) + else: + self.performance_publisher = None + self.bridge = ROSBridge() # Initialize the gesture recognition @@ -93,7 +104,8 @@ def callback(self, rgb_data, depth_data): :param depth_data: input depth image message :type depth_data: sensor_msgs.msg.Image """ - + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image and preprocess rgb_image = self.bridge.from_ros_image(rgb_data, encoding='bgr8') depth_data.encoding = 'mono16' @@ -103,6 +115,13 @@ def callback(self, rgb_data, depth_data): # Run gesture recognition gesture_class = self.gesture_learner.infer(img) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + # Publish results ros_gesture = self.bridge.from_category_to_rosclass(gesture_class) self.gesture_publisher.publish(ros_gesture) @@ -115,8 +134,8 @@ def preprocess(self, rgb_image, depth_image): :param depth_image: input depth image :type depth_image: engine.data.Image """ - rgb_image = rgb_image.convert(format='channels_last') / (2**8 - 1) - depth_image = depth_image.convert(format='channels_last') / (2**16 - 1) + rgb_image = rgb_image.convert(format='channels_last') / (2 ** 8 - 1) + depth_image = depth_image.convert(format='channels_last') / (2 ** 16 - 1) # resize the images to 224x224 rgb_image = cv2.resize(rgb_image, (224, 224)) @@ -139,10 +158,12 @@ def preprocess(self, rgb_image, depth_image): type=str, default="/kinect2/qhd/image_depth_rect") parser.add_argument("-o", "--output_gestures_topic", help="Topic name for predicted gesture class", type=str, default="/opendr/gestures") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) parser.add_argument("--device", help="Device to use (cpu, cuda)", type=str, default="cuda", choices=["cuda", "cpu"]) parser.add_argument("--delay", help="The delay (in seconds) with which RGB message and" - "depth message can be synchronized", type=float, default=0.1) + "depth message can be synchronized", type=float, default=0.1) args = parser.parse_args() @@ -161,7 +182,8 @@ def preprocess(self, rgb_image, depth_image): gesture_node = RgbdHandGestureNode(input_rgb_image_topic=args.input_rgb_image_topic, input_depth_image_topic=args.input_depth_image_topic, - output_gestures_topic=args.output_gestures_topic, device=device, - delay=args.delay) + output_gestures_topic=args.output_gestures_topic, + performance_topic=args.performance_topic, + device=device, delay=args.delay) gesture_node.listen() diff --git a/projects/opendr_ws/src/opendr_perception/scripts/semantic_segmentation_bisenet_node.py b/projects/opendr_ws/src/opendr_perception/scripts/semantic_segmentation_bisenet_node.py index 50b145331a..149e023c29 100755 --- a/projects/opendr_ws/src/opendr_perception/scripts/semantic_segmentation_bisenet_node.py +++ b/projects/opendr_ws/src/opendr_perception/scripts/semantic_segmentation_bisenet_node.py @@ -18,8 +18,10 @@ import torch import cv2 import colorsys +from time import perf_counter import rospy +from std_msgs.msg import Float32 from sensor_msgs.msg import Image as ROS_Image from opendr_bridge import ROSBridge @@ -31,7 +33,8 @@ class BisenetNode: def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", output_heatmap_topic="/opendr/heatmap", - output_rgb_image_topic="/opendr/heatmap_visualization", device="cuda"): + output_rgb_image_topic="/opendr/heatmap_visualization", performance_topic=None, + device="cuda"): """ Creates a ROS Node for semantic segmentation with Bisenet. :param input_rgb_image_topic: Topic from which we are reading the input image @@ -42,6 +45,9 @@ class ids :param output_rgb_image_topic: Topic to which we are publishing the heatmap image blended with the input image and a class legend for visualization purposes :type output_rgb_image_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str """ @@ -57,6 +63,11 @@ class ids else: self.visualization_publisher = None + if performance_topic is not None: + self.performance_publisher = rospy.Publisher(performance_topic, Float32, queue_size=1) + else: + self.performance_publisher = None + self.bridge = ROSBridge() # Initialize the semantic segmentation model @@ -83,6 +94,8 @@ def callback(self, data): :param data: Input image message :type data: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image image = self.bridge.from_ros_image(data, encoding='bgr8') @@ -90,6 +103,13 @@ def callback(self, data): # Run semantic segmentation to retrieve the OpenDR heatmap heatmap = self.learner.infer(image) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + # Publish heatmap in the form of an image containing class ids if self.heatmap_publisher is not None: heatmap = Heatmap(heatmap.data.astype(np.uint8)) # Convert to uint8 @@ -165,6 +185,8 @@ def main(): "visualization purposes", type=lambda value: value if value.lower() != "none" else None, default="/opendr/heatmap_visualization") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) 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() @@ -185,7 +207,8 @@ def main(): bisenet_node = BisenetNode(device=device, input_rgb_image_topic=args.input_rgb_image_topic, output_heatmap_topic=args.output_heatmap_topic, - output_rgb_image_topic=args.output_rgb_image_topic) + output_rgb_image_topic=args.output_rgb_image_topic, + performance_topic=args.performance_topic) bisenet_node.listen() diff --git a/projects/opendr_ws/src/opendr_perception/scripts/skeleton_based_action_recognition_node.py b/projects/opendr_ws/src/opendr_perception/scripts/skeleton_based_action_recognition_node.py index d5f39adcf0..c5bab115dd 100755 --- a/projects/opendr_ws/src/opendr_perception/scripts/skeleton_based_action_recognition_node.py +++ b/projects/opendr_ws/src/opendr_perception/scripts/skeleton_based_action_recognition_node.py @@ -17,7 +17,8 @@ import rospy import torch import numpy as np -from std_msgs.msg import String +from time import perf_counter +from std_msgs.msg import String, Float32 from vision_msgs.msg import ObjectHypothesis from opendr_bridge.msg import OpenDRPose2D from sensor_msgs.msg import Image as ROS_Image @@ -36,6 +37,7 @@ def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", pose_annotations_topic="/opendr/poses", output_category_topic="/opendr/skeleton_recognized_action", output_category_description_topic="/opendr/skeleton_recognized_action_description", + performance_topic=None, device="cuda", model='stgcn'): """ Creates a ROS Node for skeleton-based action recognition @@ -53,6 +55,9 @@ def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", :param output_category_description_topic: Topic to which we are publishing the description of the recognized action (if None, we are not publishing the description) :type output_category_description_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str :param model: model to use for skeleton-based action recognition. @@ -84,6 +89,11 @@ def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", else: self.string_publisher = None + if performance_topic is not None: + self.performance_publisher = rospy.Publisher(performance_topic, Float32, queue_size=1) + else: + self.performance_publisher = None + # Initialize the pose estimation self.pose_estimator = LightweightOpenPoseLearner(device=device, num_refinement_stages=2, mobilenet_use_stride=False, @@ -103,10 +113,10 @@ def __init__(self, input_rgb_image_topic="/usb_cam/image_raw", in_channels=2, num_point=18, graph_type='openpose') - model_saved_path = self.action_classifier.download(path="./pretrained_models/"+model, + model_saved_path = self.action_classifier.download(path="./pretrained_models/" + model, method_name=model, mode="pretrained", - file_name=model+'_ntu_cv_lw_openpose') - self.action_classifier.load(model_saved_path, model+'_ntu_cv_lw_openpose') + file_name=model + '_ntu_cv_lw_openpose') + self.action_classifier.load(model_saved_path, model + '_ntu_cv_lw_openpose') def listen(self): """ @@ -123,7 +133,8 @@ def callback(self, data): :param data: input message :type data: sensor_msgs.msg.Image """ - + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image image = self.bridge.from_ros_image(data, encoding='bgr8') @@ -133,21 +144,6 @@ def callback(self, data): # select two poses with highest energy poses = _select_2_poses(poses) - # Get an OpenCV image back - image = image.opencv() - # Annotate image and publish results - for pose in poses: - if self.pose_publisher is not None: - ros_pose = self.bridge.to_ros_pose(pose) - self.pose_publisher.publish(ros_pose) - # We get can the data back using self.bridge.from_ros_pose(ros_pose) - # e.g., opendr_pose = self.bridge.from_ros_pose(ros_pose) - draw(image, pose) - - if self.image_publisher is not None: - message = self.bridge.to_ros_image(Image(image), encoding='bgr8') - self.image_publisher.publish(message) - num_frames = 300 poses_list = [] for _ in range(num_frames): @@ -158,6 +154,27 @@ def callback(self, data): category = self.action_classifier.infer(skeleton_seq) category.confidence = float(category.confidence.max()) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + + if self.image_publisher is not None: + # Get an OpenCV image back + image = image.opencv() + # Annotate image and publish results + for pose in poses: + if self.pose_publisher is not None: + ros_pose = self.bridge.to_ros_pose(pose) + self.pose_publisher.publish(ros_pose) + # We get can the data back using self.bridge.from_ros_pose(ros_pose) + # e.g., opendr_pose = self.bridge.from_ros_pose(ros_pose) + draw(image, pose) + message = self.bridge.to_ros_image(Image(image), encoding='bgr8') + self.image_publisher.publish(message) + if self.hypothesis_publisher is not None: self.hypothesis_publisher.publish(self.bridge.to_ros_category(category)) @@ -208,6 +225,8 @@ def _pose2numpy(num_current_frames, poses_list): help="Topic name for description of the recognized action category", type=lambda value: value if value.lower() != "none" else None, default="/opendr/skeleton_recognized_action_description") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) parser.add_argument("--device", help="Device to use, either \"cpu\" or \"cuda\"", type=str, default="cuda", choices=["cuda", "cpu"]) parser.add_argument("--model", help="Model to use, either \"stgcn\" or \"pstgcn\"", @@ -234,6 +253,7 @@ def _pose2numpy(num_current_frames, poses_list): pose_annotations_topic=args.pose_annotations_topic, output_category_topic=args.output_category_topic, output_category_description_topic=args.output_category_description_topic, + performance_topic=args.performance_topic, device=device, model=args.model) skeleton_action_recognition_node.listen() diff --git a/projects/opendr_ws/src/opendr_perception/scripts/speech_command_recognition_node.py b/projects/opendr_ws/src/opendr_perception/scripts/speech_command_recognition_node.py index 961f75d801..430581cf52 100755 --- a/projects/opendr_ws/src/opendr_perception/scripts/speech_command_recognition_node.py +++ b/projects/opendr_ws/src/opendr_perception/scripts/speech_command_recognition_node.py @@ -17,7 +17,9 @@ import rospy import torch import numpy as np +from time import perf_counter from opendr_bridge import ROSBridge +from std_msgs.msg import Float32 from audio_common_msgs.msg import AudioData from vision_msgs.msg import Classification2D import argparse @@ -29,13 +31,16 @@ class SpeechRecognitionNode: def __init__(self, input_audio_topic="/audio/audio", output_speech_command_topic="/opendr/speech_recognition", - buffer_size=1.5, model="matchboxnet", model_path=None, device="cuda"): + performance_topic=None, buffer_size=1.5, model="matchboxnet", model_path=None, device="cuda"): """ Creates a ROS Node for speech command recognition :param input_audio_topic: Topic from which the audio data is received :type input_audio_topic: str :param output_speech_command_topic: Topic to which the predictions are published :type output_speech_command_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param buffer_size: Length of the audio buffer in seconds :type buffer_size: float :param model: base speech command recognition model: matchboxnet or quad_selfonn @@ -49,6 +54,11 @@ def __init__(self, input_audio_topic="/audio/audio", output_speech_command_topic rospy.Subscriber(input_audio_topic, AudioData, self.callback) + if performance_topic is not None: + self.performance_publisher = rospy.Publisher(performance_topic, Float32, queue_size=1) + else: + self.performance_publisher = None + self.bridge = ROSBridge() # Initialize the internal audio buffer @@ -88,15 +98,24 @@ def callback(self, msg_data): :param msg_data: incoming message :type msg_data: audio_common_msgs.msg.AudioData """ + if self.performance_publisher: + start_time = perf_counter() # Accumulate data until the buffer is full - data = np.reshape(np.frombuffer(msg_data.data, dtype=np.int16)/32768.0, (1, -1)) + data = np.reshape(np.frombuffer(msg_data.data, dtype=np.int16) / 32768.0, (1, -1)) self.data_buffer = np.append(self.data_buffer, data, axis=1) - if self.data_buffer.shape[1] > 16000*self.buffer_size: + if self.data_buffer.shape[1] > 16000 * self.buffer_size: # Convert sample to OpenDR Timeseries and perform classification input_sample = Timeseries(self.data_buffer) class_pred = self.learner.infer(input_sample) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + # Publish output ros_class = self.bridge.from_category_to_rosclass(class_pred) self.publisher.publish(ros_class) @@ -111,6 +130,8 @@ def callback(self, msg_data): help="Listen to input data on this topic") parser.add_argument("-o", "--output_speech_command_topic", type=str, default="/opendr/speech_recognition", help="Topic name for speech command output") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) parser.add_argument("--device", type=str, default="cuda", choices=["cuda", "cpu"], help="Device to use (cpu, cuda)") parser.add_argument("--buffer_size", type=float, default=1.5, help="Size of the audio buffer in seconds") @@ -135,6 +156,7 @@ def callback(self, msg_data): speech_node = SpeechRecognitionNode(input_audio_topic=args.input_audio_topic, output_speech_command_topic=args.output_speech_command_topic, + performance_topic=args.performance_topic, buffer_size=args.buffer_size, model=args.model, model_path=args.model_path, device=device) speech_node.listen() diff --git a/projects/opendr_ws/src/opendr_perception/scripts/video_activity_recognition_node.py b/projects/opendr_ws/src/opendr_perception/scripts/video_activity_recognition_node.py index 5001ed048e..2f80affc39 100755 --- a/projects/opendr_ws/src/opendr_perception/scripts/video_activity_recognition_node.py +++ b/projects/opendr_ws/src/opendr_perception/scripts/video_activity_recognition_node.py @@ -18,8 +18,9 @@ import torch import torchvision import cv2 +from time import perf_counter from pathlib import Path -from std_msgs.msg import String +from std_msgs.msg import String, Float32 from vision_msgs.msg import ObjectHypothesis from sensor_msgs.msg import Image as ROS_Image from opendr_bridge import ROSBridge @@ -31,12 +32,13 @@ class HumanActivityRecognitionNode: def __init__( - self, - input_rgb_image_topic="/usb_cam/image_raw", - output_category_topic="/opendr/human_activity_recognition", - output_category_description_topic="/opendr/human_activity_recognition_description", - device="cuda", - model="cox3d-m", + self, + input_rgb_image_topic="/usb_cam/image_raw", + output_category_topic="/opendr/human_activity_recognition", + output_category_description_topic="/opendr/human_activity_recognition_description", + performance_topic=None, + device="cuda", + model="cox3d-m", ): """ Creates a ROS Node for video-based human activity recognition. @@ -48,6 +50,9 @@ def __init__( :param output_category_description_topic: Topic to which we are publishing the ID of the recognized action (if None, we are not publishing the ID) :type output_category_description_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str :param model: Architecture to use for human activity recognition. @@ -96,6 +101,11 @@ def __init__( else None ) + if performance_topic is not None: + self.performance_publisher = rospy.Publisher(performance_topic, Float32, queue_size=1) + else: + self.performance_publisher = None + self.bridge = ROSBridge() def listen(self): @@ -119,6 +129,8 @@ def callback(self, data): :param data: input message :type data: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() image = self.bridge.from_ros_image(data, encoding="rgb8") if image is None: return @@ -131,6 +143,13 @@ def callback(self, data): category.confidence = float(category.confidence.max()) # Confidence for predicted class category.description = KINETICS400_CLASSES[category.data] # Class name + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + if self.hypothesis_publisher is not None: self.hypothesis_publisher.publish(self.bridge.to_ros_category(category)) @@ -217,7 +236,9 @@ def main(): help="Topic to which we are publishing the ID of the recognized action", type=lambda value: value if value.lower() != "none" else None, default="/opendr/human_activity_recognition_description") - parser.add_argument("--device", help='Device to use, either "cpu" or "cuda", defaults to "cuda"', + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) + 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("--model", help="Architecture to use for human activity recognition.", type=str, default="cox3d-m", @@ -241,6 +262,7 @@ def main(): input_rgb_image_topic=args.input_rgb_image_topic, output_category_topic=args.output_category_topic, output_category_description_topic=args.output_category_description_topic, + performance_topic=args.performance_topic, device=device, model=args.model, ) diff --git a/projects/opendr_ws_2/src/opendr_perception/README.md b/projects/opendr_ws_2/src/opendr_perception/README.md index 6a36be68f0..adadd2bfaf 100755 --- a/projects/opendr_ws_2/src/opendr_perception/README.md +++ b/projects/opendr_ws_2/src/opendr_perception/README.md @@ -53,6 +53,11 @@ Before you can run any of the toolkit's ROS2 nodes, some prerequisites need to b _An example would be to disable the output annotated image topic in a node when visualization is not needed and only use the detection message in another node, thus eliminating the OpenCV operations._ +- ### Logging the node performance in the console + OpenDR provides the utility [performance node](#performance-ros2-node) to log performance messages in the console for the running node. + You can set the `performance_topic` of the node you are using and also run the performance node to get the time it takes for the + node to process a single input and its average speed expressed in frames per second. + - ### An example diagram of OpenDR nodes running ![Face Detection ROS2 node running diagram](../../images/opendr_node_diagram.png) - On the left, the `usb_cam` node can be seen, which is using a system camera to publish images on the `/image_raw` topic. @@ -87,6 +92,7 @@ The node publishes the detected poses in [OpenDR's 2D pose message format](../op - `-i or --input_rgb_image_topic INPUT_RGB_IMAGE_TOPIC`: topic name for input RGB image (default=`/image_raw`) - `-o or --output_rgb_image_topic OUTPUT_RGB_IMAGE_TOPIC`: topic name for output annotated RGB image, `None` to stop the node from publishing on this topic (default=`/opendr/image_pose_annotated`) - `-d or --detections_topic DETECTIONS_TOPIC`: topic name for detection messages, `None` to stop the node from publishing on this topic (default=`/opendr/poses`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: Device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) - `--accelerate`: Acceleration flag that causes pose estimation to run faster but with less accuracy @@ -115,6 +121,7 @@ The node publishes the detected poses in [OpenDR's 2D pose message format](../op - `-i or --input_rgb_image_topic INPUT_RGB_IMAGE_TOPIC`: topic name for input RGB image (default=`/image_raw`) - `-o or --output_rgb_image_topic OUTPUT_RGB_IMAGE_TOPIC`: topic name for output annotated RGB image, `None` to stop the node from publishing on this topic (default=`/opendr/image_pose_annotated`) - `-d or --detections_topic DETECTIONS_TOPIC`: topic name for detection messages, `None` to stop the node from publishing on this topic (default=`/opendr/poses`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: Device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) - `--accelerate`: Acceleration flag that causes pose estimation to run faster but with less accuracy @@ -146,6 +153,7 @@ Fall detection uses the toolkit's pose estimation tool internally. - `-i or --input_rgb_image_topic INPUT_RGB_IMAGE_TOPIC`: topic name for input RGB image (default=`/image_raw`) - `-o or --output_rgb_image_topic OUTPUT_RGB_IMAGE_TOPIC`: topic name for output annotated RGB image, `None` to stop the node from publishing on this topic (default=`/opendr/image_fallen_annotated`) - `-d or --detections_topic DETECTIONS_TOPIC`: topic name for detection messages, `None` to stop the node from publishing on this topic (default=`/opendr/fallen`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) - `--accelerate`: acceleration flag that causes pose estimation that runs internally to run faster but with less accuracy @@ -176,6 +184,7 @@ The node makes use of the toolkit's [face detection tool](../../../../src/opendr - `-i or --input_rgb_image_topic INPUT_RGB_IMAGE_TOPIC`: topic name for input RGB image (default=`/image_raw`) - `-o or --output_rgb_image_topic OUTPUT_RGB_IMAGE_TOPIC`: topic name for output annotated RGB image, `None` to stop the node from publishing on this topic (default=`/opendr/image_faces_annotated`) - `-d or --detections_topic DETECTIONS_TOPIC`: topic name for detection messages, `None` to stop the node from publishing on this topic (default=`/opendr/faces`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) - `--backbone BACKBONE`: retinaface backbone, options are either `mnet` or `resnet`, where `mnet` detects masked faces as well (default=`resnet`) @@ -205,6 +214,7 @@ The node makes use of the toolkit's [face recognition tool](../../../../src/open - `-o or --output_rgb_image_topic OUTPUT_RGB_IMAGE_TOPIC`: topic name for output annotated RGB image, `None` to stop the node from publishing on this topic (default=`/opendr/image_face_reco_annotated`) - `-d or --detections_topic DETECTIONS_TOPIC`: topic name for detection messages, `None` to stop the node from publishing on this topic (default=`/opendr/face_recognition`) - `-id or --detections_id_topic DETECTIONS_ID_TOPIC`: topic name for detection ID messages, `None` to stop the node from publishing on this topic (default=`/opendr/face_recognition_id`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) - `--backbone BACKBONE`: backbone network (default=`mobilefacenet`) - `--dataset_path DATASET_PATH`: path of the directory where the images of the faces to be recognized are stored (default=`./database`) @@ -297,6 +307,7 @@ whose documentation can be found here: - `-i or --input_rgb_image_topic INPUT_RGB_IMAGE_TOPIC`: topic name for input RGB image (default=`/image_raw`) - `-o or --output_rgb_image_topic OUTPUT_RGB_IMAGE_TOPIC`: topic name for output annotated RGB image, `None` to stop the node from publishing on this topic (default=`/opendr/image_objects_annotated`) - `-d or --detections_topic DETECTIONS_TOPIC`: topic name for detection messages, `None` to stop the node from publishing on this topic (default=`/opendr/objects`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: Device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) 3. Default output topics: @@ -325,6 +336,7 @@ The node makes use of the toolkit's [single object tracking 2D SiamRPN tool](../ - `-i or --input_rgb_image_topic INPUT_RGB_IMAGE_TOPIC` : listen to RGB images on this topic (default=`/image_raw`) - `-o or --output_rgb_image_topic OUTPUT_RGB_IMAGE_TOPIC`: topic name for output annotated RGB image, `None` to stop the node from publishing on this topic (default=`/opendr/image_tracking_annotated`) - `-t or --tracker_topic TRACKER_TOPIC`: topic name for tracker messages, `None` to stop the node from publishing on this topic (default=`/opendr/tracked_object`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: Device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) 3. Default output topics: @@ -375,6 +387,7 @@ whose documentation can be found here: [Deep Sort docs](../../../../docs/referen - `-o or --output_rgb_image_topic OUTPUT_RGB_IMAGE_TOPIC`: topic name for output annotated RGB image, `None` to stop the node from publishing on this topic (default=`/opendr/image_objects_annotated`) - `-d or --detections_topic DETECTIONS_TOPIC`: topic name for detection messages, `None` to stop the node from publishing on this topic (default=`/opendr/objects`) - `-t or --tracking_id_topic TRACKING_ID_TOPIC`: topic name for tracking ID messages, `None` to stop the node from publishing on this topic (default=`/opendr/objects_tracking_id`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) - `-td --temp_dir TEMP_DIR`: path to a temporary directory with models (default=`temp`) @@ -415,6 +428,7 @@ and additional information about EfficientPS [here](../../../../src/opendr/perce - `-oh or --output_heatmap_topic OUTPUT_RGB_IMAGE_TOPIC`: publish the semantic and instance maps on this topic as `OUTPUT_HEATMAP_TOPIC/semantic` and `OUTPUT_HEATMAP_TOPIC/instance` (default=`/opendr/panoptic`) - `-ov or --output_rgb_image_topic OUTPUT_RGB_IMAGE_TOPIC`: publish the panoptic segmentation map as an RGB image on `VISUALIZATION_TOPIC` or a more detailed overview if using the `--detailed_visualization` flag (default=`/opendr/panoptic/rgb_visualization`) - `--detailed_visualization`: generate a combined overview of the input RGB image and the semantic, instance, and panoptic segmentation maps and publish it on `OUTPUT_RGB_IMAGE_TOPIC` (default=deactivated) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) 3. Default output topics: - Output images: `/opendr/panoptic/semantic`, `/opendr/panoptic/instance`, `/opendr/panoptic/rgb_visualization` @@ -441,6 +455,7 @@ The node makes use of the toolkit's [semantic segmentation tool](../../../../src - `-i or --input_rgb_image_topic INPUT_RGB_IMAGE_TOPIC`: topic name for input RGB image (default=`/image_raw`) - `-o or --output_heatmap_topic OUTPUT_HEATMAP_TOPIC`: topic to which we are publishing the heatmap in the form of a ROS2 image containing class IDs, `None` to stop the node from publishing on this topic (default=`/opendr/heatmap`) - `-ov or --output_rgb_image_topic OUTPUT_RGB_IMAGE_TOPIC`: topic to which we are publishing the heatmap image blended with the input image and a class legend for visualization purposes, `None` to stop the node from publishing on this topic (default=`/opendr/heatmap_visualization`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) 3. Default output topics: @@ -484,6 +499,7 @@ The node makes use of the toolkit's [binary high resolution tool](../../../../sr - `-ov or --output_rgb_image_topic OUTPUT_RGB_IMAGE_TOPIC`: topic to which we are publishing the heatmap image blended with the input image and a class legend for visualization purposes, `None` to stop the node from publishing on this topic (default=`/opendr/binary_hr_heatmap_visualization`) - `-m or --model_path MODEL_PATH`: path to the directory of the trained model (default=`test_model`) - `-a or --architecture ARCHITECTURE`: architecture used for the trained model, either `VGG_720p` or `VGG_1080p` (default=`VGG_720p`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) 3. Default output topics: @@ -513,6 +529,7 @@ whose documentation can be found [here](../../../../docs/reference/image_based_f - `-o or --output_rgb_image_topic OUTPUT_RGB_IMAGE_TOPIC`: topic name for output annotated RGB image, `None` to stop the node from publishing on this topic (default=`/opendr/image_emotion_estimation_annotated`) - `-e or --output_emotions_topic OUTPUT_EMOTIONS_TOPIC`: topic to which we are publishing the facial emotion results, `None` to stop the node from publishing on this topic (default=`"/opendr/facial_emotion_estimation"`) - `-m or --output_emotions_description_topic OUTPUT_EMOTIONS_DESCRIPTION_TOPIC`: topic to which we are publishing the description of the estimated facial emotion, `None` to stop the node from publishing on this topic (default=`/opendr/facial_emotion_estimation_description`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) 3. Default output topics: @@ -551,6 +568,7 @@ whose documentation can be found [here](../../../../docs/reference/landmark-base - `-i or --input_rgb_image_topic INPUT_RGB_IMAGE_TOPIC`: topic name for input RGB image (default=`/image_raw`) - `-o or --output_category_topic OUTPUT_CATEGORY_TOPIC`: topic to which we are publishing the recognized facial expression category info, `None` to stop the node from publishing on this topic (default=`"/opendr/landmark_expression_recognition"`) - `-d or --output_category_description_topic OUTPUT_CATEGORY_DESCRIPTION_TOPIC`: topic to which we are publishing the description of the recognized facial expression, `None` to stop the node from publishing on this topic (default=`/opendr/landmark_expression_recognition_description`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) - `--model`: architecture to use for facial expression recognition, options are `pstbln_ck+`, `pstbln_casia`, `pstbln_afew` (default=`pstbln_afew`) - `-s or --shape_predictor SHAPE_PREDICTOR`: shape predictor (landmark_extractor) to use (default=`./predictor_path`) @@ -601,6 +619,7 @@ Their documentation can be found [here](../../../../docs/reference/skeleton-base - `-i or --input_rgb_image_topic INPUT_RGB_IMAGE_TOPIC`: topic name for input RGB image (default=`/usb_cam/image_raw`) - `-o or --output_rgb_image_topic OUTPUT_RGB_IMAGE_TOPIC`: topic name for output pose-annotated RGB image, `None` to stop the node from publishing on this topic (default=`/opendr/image_pose_annotated`) - `-p or --pose_annotations_topic POSE_ANNOTATIONS_TOPIC`: topic name for pose annotations, `None` to stop the node from publishing on this topic (default=`/opendr/poses`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) 3. Default output topics: @@ -635,6 +654,7 @@ The node makes use of the toolkit's video human activity recognition tools which - `-i or --input_rgb_image_topic INPUT_RGB_IMAGE_TOPIC`: topic name for input RGB image (default=`/image_raw`) - `-o or --output_category_topic OUTPUT_CATEGORY_TOPIC`: topic to which we are publishing the recognized activity, `None` to stop the node from publishing on this topic (default=`"/opendr/human_activity_recognition"`) - `-od or --output_category_description_topic OUTPUT_CATEGORY_DESCRIPTION_TOPIC`: topic to which we are publishing the ID of the recognized action, `None` to stop the node from publishing on this topic (default=`/opendr/human_activity_recognition_description`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--model`: architecture to use for human activity recognition, options are `cox3d-s`, `cox3d-m`, `cox3d-l`, `x3d-xs`, `x3d-s`, `x3d-m`, or `x3d-l` (default=`cox3d-m`) - `--device DEVICE`: device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) @@ -680,6 +700,7 @@ whose documentation can be found [here](../../../../docs/reference/gem.md). - `-oc or --output_rgb_image_topic OUTPUT_RGB_IMAGE_TOPIC`: topic name for output annotated RGB image, `None` to stop the node from publishing on this topic (default=`/opendr/rgb_image_objects_annotated`) - `-oi or --output_infra_image_topic OUTPUT_INFRA_IMAGE_TOPIC`: topic name for output annotated infrared image, `None` to stop the node from publishing on this topic (default=`/opendr/infra_image_objects_annotated`) - `-d or --detections_topic DETECTIONS_TOPIC`: topic name for detection messages, `None` to stop the node from publishing on this topic (default=`/opendr/objects`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) 5. Default output topics: @@ -713,6 +734,7 @@ whose documentation can be found [here](../../../../docs/reference/rgbd-hand-ges - `-ic or --input_rgb_image_topic INPUT_RGB_IMAGE_TOPIC`: topic name for input RGB image (default=`/kinect2/qhd/image_color_rect`) - `-id or --input_depth_image_topic INPUT_DEPTH_IMAGE_TOPIC`: topic name for input depth image (default=`/kinect2/qhd/image_depth_rect`) - `-o or --output_gestures_topic OUTPUT_GESTURES_TOPIC`: topic name for predicted gesture class (default=`/opendr/gestures`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) 3. Default output topics: @@ -743,6 +765,7 @@ whose documentation can be found [here](../../../../docs/reference/audiovisual-e - `-iv or --input_video_topic INPUT_VIDEO_TOPIC`: topic name for input video, expects detected face of size 224x224 (default=`/image_raw`) - `-ia or --input_audio_topic INPUT_AUDIO_TOPIC`: topic name for input audio (default=`/audio`) - `-o or --output_emotions_topic OUTPUT_EMOTIONS_TOPIC`: topic to which we are publishing the predicted emotion (default=`/opendr/audiovisual_emotion`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--buffer_size BUFFER_SIZE`: length of audio and video in seconds, (default=`3.6`) - `--model_path MODEL_PATH`: if given, the pretrained model will be loaded from the specified local path, otherwise it will be downloaded from an OpenDR FTP server @@ -777,6 +800,7 @@ whose documentation can be found here: - `-h or --help`: show a help message and exit - `-i or --input_audio_topic INPUT_AUDIO_TOPIC`: topic name for input audio (default=`/audio`) - `-o or --output_speech_command_topic OUTPUT_SPEECH_COMMAND_TOPIC`: topic name for speech command output (default=`/opendr/speech_recognition`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--buffer_size BUFFER_SIZE`: set the size of the audio buffer (expected command duration) in seconds (default=`1.5`) - `--model MODEL`: the model to use, choices are `matchboxnet`, `edgespeechnets` or `quad_selfonn` (default=`matchboxnet`) - `--model_path MODEL_PATH`: if given, the pretrained model will be loaded from the specified local path, otherwise it will be downloaded from an OpenDR FTP server @@ -814,6 +838,7 @@ whose documentation can be found [here](../../../../docs/reference/voxel-object- - `-h or --help`: show a help message and exit - `-i or --input_point_cloud_topic INPUT_POINT_CLOUD_TOPIC`: point cloud topic provided by either a point_cloud_dataset_node or any other 3D point cloud node (default=`/opendr/dataset_point_cloud`) - `-d or --detections_topic DETECTIONS_TOPIC`: topic name for detection messages (default=`/opendr/objects3d`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) - `-n or --model_name MODEL_NAME`: name of the trained model (default=`tanet_car_xyres_16`) - `-c or --model_config_path MODEL_CONFIG_PATH`: path to a model .proto config (default=`../../src/opendr/perception/object_detection3d/voxel_object_detection_3d/second_detector/configs/tanet/car/xyres_16.proto`) @@ -847,6 +872,7 @@ whose documentation can be found [here](../../../../docs/reference/object-tracki - `-i or --input_point_cloud_topic INPUT_POINT_CLOUD_TOPIC`: point cloud topic provided by either a point_cloud_dataset_node or any other 3D point cloud node (default=`/opendr/dataset_point_cloud`) - `-d or --detections_topic DETECTIONS_TOPIC`: topic name for detection messages, `None` to stop the node from publishing on this topic (default=`/opendr/objects3d`) - `-t or --tracking3d_id_topic TRACKING3D_ID_TOPIC`: topic name for output tracking IDs with the same element count as in detection topic, `None` to stop the node from publishing on this topic (default=`/opendr/objects_tracking_id`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) - `-dn or --detector_model_name DETECTOR_MODEL_NAME`: name of the trained model (default=`tanet_car_xyres_16`) - `-dc or --detector_model_config_path DETECTOR_MODEL_CONFIG_PATH`: path to a model .proto config (default=`../../src/opendr/perception/object_detection3d/voxel_object_detection_3d/second_detector/configs/tanet/car/xyres_16.proto`) @@ -881,6 +907,7 @@ and additional information about EfficientLPS [here](../../../../src/opendr/perc The following optional arguments are available: - `-h, --help`: show a help message and exit - `-i or --input_point_cloud_2_topic INPUT_POINTCLOUD2_TOPIC` : Point Cloud 2 topic provided by either a point_cloud_2_publisher_node or any other 3D Point Cloud 2 Node (default=`/opendr/dataset_point_cloud2`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `-c or --checkpoint CHECKPOINT` : download pretrained models [semantickitti] or load from the provided path (default=`semantickitti`) - `-o or --output_heatmap_pointcloud_topic OUTPUT_HEATMAP_POINTCLOUD_TOPIC`: publish the 3D heatmap pointcloud on `OUTPUT_HEATMAP_POINTCLOUD_TOPIC` (default=`/opendr/panoptic`) @@ -913,6 +940,7 @@ The node makes use of the toolkit's heart anomaly detection tools: [ANBOF tool]( - `-h or --help`: show a help message and exit - `-i or --input_ecg_topic INPUT_ECG_TOPIC`: topic name for input ECG data (default=`/ecg/ecg`) - `-o or --output_heart_anomaly_topic OUTPUT_HEART_ANOMALY_TOPIC`: topic name for heart anomaly detection (default=`/opendr/heart_anomaly`) + - `--performance_topic PERFORMANCE_TOPIC`: topic name for performance messages (default=`None`, disabled) - `--device DEVICE`: device to use, either `cpu` or `cuda`, falls back to `cpu` if GPU or CUDA is not found (default=`cuda`) - `--model MODEL`: the model to use, choices are `anbof` or `gru` (default=`anbof`) @@ -987,4 +1015,29 @@ The following optional arguments are available: - `-d or --dataset_path DATASET_PATH`: path of the SemanticKITTI dataset to publish the point cloud 2 message (default=`./datasets/semantickitti`) - `-s or --split SPLIT`: split of the dataset to use, only (train, valid, test) are available (default=`valid`) - `-o or --output_point_cloud_2_topic OUTPUT_POINT_CLOUD_2_TOPIC`: topic name to publish the data (default=`/opendr/dataset_point_cloud2`) - - `-t or --test_data`: Add this argument if you want to only test this node with the test data available in our server \ No newline at end of file + - `-t or --test_data`: Add this argument if you want to only test this node with the test data available in our server + +---- +## Utility ROS2 Nodes + +### Performance ROS2 Node + +The performance node is used to subscribe to the optional performance topic of a running node and log its performance in terms of the time it +took to process a single input and produce output and in terms of frames per second. It uses a modifiable rolling window to calculate the average FPS. + +You can inspect [the node](./opendr_perception/performance_node.py) and modify it to your needs. + +#### Instructions for basic usage: + +1. Start the node you want to benchmark as usual but also set the optional argument `--performance_topic` to, for example, `/opendr/performance` +2. Start the performance node: + ```shell + ros2 run opendr_perception performance + ``` + The following optional arguments are available: + - `-h or --help`: show a help message and exit + - `-i or --input_performance_topic INPUT_PERFORMANCE_TOPIC`: topic name for input performance data (default=`/opendr/performance`) + - `-w or --window WINDOW`: the window to use in number of frames to calculate the running average FPS (default=`20`) + +Note that the `input_performance_topic` of the performance node must match the `performance_topic` of the running node. +Also note that the running node should properly get input and produce output to publish performance messages for the performance node to use. diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/audiovisual_emotion_recognition_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/audiovisual_emotion_recognition_node.py index 11a7aa57ca..e72ef19d85 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/audiovisual_emotion_recognition_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/audiovisual_emotion_recognition_node.py @@ -20,10 +20,12 @@ import torch import librosa import cv2 +from time import perf_counter import rclpy from rclpy.node import Node import message_filters +from std_msgs.msg import Float32 from sensor_msgs.msg import Image as ROS_Image from audio_common_msgs.msg import AudioData from vision_msgs.msg import Classification2D @@ -37,8 +39,8 @@ class AudiovisualEmotionNode(Node): def __init__(self, input_video_topic="/image_raw", input_audio_topic="/audio", - output_emotions_topic="/opendr/audiovisual_emotion", buffer_size=3.6, device="cuda", - delay=0.1): + output_emotions_topic="/opendr/audiovisual_emotion", performance_topic=None, + buffer_size=3.6, device="cuda", delay=0.1): """ Creates a ROS2 Node for audiovisual emotion recognition :param input_video_topic: Topic from which we are reading the input video. Expects detected face of size 224x224 @@ -47,6 +49,9 @@ def __init__(self, input_video_topic="/image_raw", input_audio_topic="/audio", :type input_audio_topic: str :param output_emotions_topic: Topic to which we are publishing the predicted class :type output_emotions_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param buffer_size: length of audio and video in sec :type buffer_size: float :param device: device on which we are running inference ('cpu' or 'cuda') @@ -65,6 +70,11 @@ def __init__(self, input_video_topic="/image_raw", input_audio_topic="/audio", allow_headerless=True) ts.registerCallback(self.callback) + if performance_topic is not None: + self.performance_publisher = self.create_publisher(Float32, performance_topic, 1) + else: + self.performance_publisher = None + self.bridge = ROS2Bridge() self.avlearner = AudiovisualEmotionLearner(device=device, fusion='ia', mod_drop='zerodrop') @@ -77,7 +87,7 @@ def __init__(self, input_video_topic="/image_raw", input_audio_topic="/audio", self.video_buffer = np.zeros((1, 224, 224, 3)) self.video_transform = transforms.Compose([ - transforms.ToTensor(255)]) + transforms.ToTensor(255)]) self.get_logger().info("Audiovisual emotion recognition node started!") @@ -89,7 +99,9 @@ def callback(self, image_data, audio_data): :param audio_data: input audio message, speech :type audio_data: audio_common_msgs.msg.AudioData """ - audio_data = np.reshape(np.frombuffer(audio_data.data, dtype=np.int16)/32768.0, (1, -1)) + if self.performance_publisher: + start_time = perf_counter() + audio_data = np.reshape(np.frombuffer(audio_data.data, dtype=np.int16) / 32768.0, (1, -1)) self.data_buffer = np.append(self.data_buffer, audio_data) image_data = self.bridge.from_ros_image(image_data, encoding='bgr8').convert(format='channels_last') @@ -97,11 +109,11 @@ def callback(self, image_data, audio_data): self.video_buffer = np.append(self.video_buffer, np.expand_dims(image_data.data, 0), axis=0) - if self.data_buffer.shape[0] > 16000*self.buffer_size: + if self.data_buffer.shape[0] > 16000 * self.buffer_size: audio = librosa.feature.mfcc(self.data_buffer[1:], sr=16000, n_mfcc=10) audio = Timeseries(audio) - to_select = select_distributed(15, len(self.video_buffer)-1) + to_select = select_distributed(15, len(self.video_buffer) - 1) video = self.video_buffer[1:][to_select] video = [self.video_transform(img) for img in video] @@ -109,6 +121,13 @@ def callback(self, image_data, audio_data): class_pred = self.avlearner.infer(audio, video) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + # Publish output ros_class = self.bridge.from_category_to_rosclass(class_pred, self.get_clock().now().to_msg()) self.publisher.publish(ros_class) @@ -117,7 +136,7 @@ def callback(self, image_data, audio_data): self.video_buffer = np.zeros((1, 224, 224, 3)) -def select_distributed(m, n): return [i*n//m + n//(2*m) for i in range(m)] +def select_distributed(m, n): return [i * n // m + n // (2 * m) for i in range(m)] def main(args=None): @@ -130,12 +149,14 @@ def main(args=None): help="Listen to audio input data on this topic") parser.add_argument("-o", "--output_emotions_topic", type=str, default="/opendr/audiovisual_emotion", help="Topic name for output emotions recognition") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) parser.add_argument("--device", type=str, default="cuda", help="Device to use (cpu, cuda)", choices=["cuda", "cpu"]) parser.add_argument("--buffer_size", type=float, default=3.6, help="Size of the audio buffer in seconds") parser.add_argument("--delay", help="The delay (in seconds) with which RGB message and" - "depth message can be synchronized", type=float, default=0.1) + "depth message can be synchronized", type=float, default=0.1) args = parser.parse_args() @@ -155,6 +176,7 @@ def main(args=None): emotion_node = AudiovisualEmotionNode(input_video_topic=args.input_video_topic, input_audio_topic=args.input_audio_topic, output_emotions_topic=args.output_emotions_topic, + performance_topic=args.performance_topic, buffer_size=args.buffer_size, device=device, delay=args.delay) rclpy.spin(emotion_node) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/binary_high_resolution_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/binary_high_resolution_node.py index 8805fc2b30..660e936819 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/binary_high_resolution_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/binary_high_resolution_node.py @@ -17,10 +17,12 @@ import numpy as np import torch import cv2 +from time import perf_counter import rclpy from rclpy.node import Node +from std_msgs.msg import Float32 from sensor_msgs.msg import Image as ROS_Image from opendr_bridge import ROS2Bridge @@ -31,7 +33,7 @@ class BinaryHighResolutionNode(Node): def __init__(self, input_rgb_image_topic="image_raw", output_heatmap_topic="/opendr/binary_hr_heatmap", - output_rgb_image_topic="/opendr/binary_hr_heatmap_visualization", + output_rgb_image_topic="/opendr/binary_hr_heatmap_visualization", performance_topic=None, model_path=None, architecture="VGG_720p", device="cuda"): """ Create a ROS2 Node for binary high resolution classification with Binary High Resolution. @@ -42,6 +44,9 @@ def __init__(self, input_rgb_image_topic="image_raw", output_heatmap_topic="/ope :param output_rgb_image_topic: Topic to which we are publishing the heatmap image blended with the input image for visualization purposes :type output_rgb_image_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param model_path: The path to the directory of a trained model :type model_path: str :param architecture: Architecture used on trained model (`VGG_720p` or `VGG_1080p`) @@ -63,6 +68,11 @@ def __init__(self, input_rgb_image_topic="image_raw", output_heatmap_topic="/ope else: self.visualization_publisher = None + if performance_topic is not None: + self.performance_publisher = self.create_subscription(Float32, performance_topic, 1) + else: + self.performance_publisher = None + self.bridge = ROS2Bridge() # Initialize the semantic segmentation model @@ -91,12 +101,21 @@ def callback(self, data): :param data: Input image message :type data: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image image = self.bridge.from_ros_image(data, encoding='bgr8') image = image.convert("channels_last") # Run learner to retrieve the OpenDR heatmap heatmap = self.learner.infer(image) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + # Publish heatmap in the form of an image if self.heatmap_publisher is not None: self.heatmap_publisher.publish(self.bridge.to_ros_image(heatmap)) @@ -124,6 +143,8 @@ def main(args=None): "blended with the input image for visualization purposes", type=lambda value: value if value.lower() != "none" else None, default="/opendr/binary_hr_heatmap_visualization") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) parser.add_argument("-m", "--model_path", help="Path to the directory of the trained model", type=str, default="test_model") parser.add_argument("-a", "--architecture", help="Architecture used for the trained model, either \"VGG_720p\" or " @@ -150,6 +171,7 @@ def main(args=None): input_rgb_image_topic=args.input_rgb_image_topic, output_heatmap_topic=args.output_heatmap_topic, output_rgb_image_topic=args.output_rgb_image_topic, + performance_topic=args.performance_topic, model_path=args.model_path, architecture=args.architecture) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/continual_skeleton_based_action_recognition_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/continual_skeleton_based_action_recognition_node.py index a1666a98e0..868eb4625d 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/continual_skeleton_based_action_recognition_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/continual_skeleton_based_action_recognition_node.py @@ -16,10 +16,11 @@ import argparse import torch import numpy as np +from time import perf_counter import rclpy from rclpy.node import Node -from std_msgs.msg import String +from std_msgs.msg import String, Float32 from vision_msgs.msg import ObjectHypothesis from sensor_msgs.msg import Image as ROS_Image from opendr_bridge import ROS2Bridge @@ -38,6 +39,7 @@ def __init__(self, input_rgb_image_topic="image_raw", pose_annotations_topic="/opendr/poses", output_category_topic="/opendr/continual_skeleton_recognized_action", output_category_description_topic="/opendr/continual_skeleton_recognized_action_description", + performance_topic=None, device="cuda", model='costgcn'): """ Creates a ROS2 Node for continual skeleton-based action recognition. @@ -55,6 +57,9 @@ def __init__(self, input_rgb_image_topic="image_raw", :param output_category_description_topic: Topic to which we are publishing the description of the recognized action (if None, we are not publishing the description) :type output_category_description_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str :param model: model to use for skeleton-based action recognition. @@ -86,6 +91,11 @@ def __init__(self, input_rgb_image_topic="image_raw", else: self.string_publisher = None + if performance_topic is not None: + self.performance_publisher = self.create_publisher(Float32, performance_topic, 1) + else: + self.performance_publisher = None + self.bridge = ROS2Bridge() # Initialize the pose estimation @@ -112,6 +122,8 @@ def callback(self, data): :param data: input message :type data: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image image = self.bridge.from_ros_image(data, encoding='bgr8') @@ -122,6 +134,22 @@ def callback(self, data): # select two poses with the highest energy poses = _select_2_poses(poses) + num_frames = 1 + poses_list = [poses] + skeleton_seq = _pose2numpy(num_frames, poses_list) + + # Run action recognition + result = self.action_classifier.infer(skeleton_seq) # input_size: BxCxTxVxS + category = result[0] + category.confidence = float(category.confidence.max()) + + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + # Publish detections in ROS message if self.pose_publisher is not None: for pose in poses: @@ -137,15 +165,6 @@ def callback(self, data): # Convert the annotated OpenDR image to ROS2 image message using bridge and publish it self.image_publisher.publish(self.bridge.to_ros_image(Image(image), encoding='bgr8')) - num_frames = 1 - poses_list = [poses] - skeleton_seq = _pose2numpy(num_frames, poses_list) - - # Run action recognition - result = self.action_classifier.infer(skeleton_seq) # input_size: BxCxTxVxS - category = result[0] - category.confidence = float(category.confidence.max()) - if self.hypothesis_publisher is not None: self.hypothesis_publisher.publish(self.bridge.to_ros_category(category)) @@ -196,6 +215,8 @@ def main(args=None): "recognized action category", type=lambda value: value if value.lower() != "none" else None, default="/opendr/continual_skeleton_recognized_action_description") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) parser.add_argument("--device", help="Device to use, either \"cpu\" or \"cuda\"", type=str, default="cuda", choices=["cuda", "cpu"]) parser.add_argument("--model", help="Model to use, either \"costgcn\"", @@ -222,6 +243,7 @@ def main(args=None): pose_annotations_topic=args.pose_annotations_topic, output_category_topic=args.output_category_topic, output_category_description_topic=args.output_category_description_topic, + performance_topic=args.performance_topic, device=device, model=args.model) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/face_detection_retinaface_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/face_detection_retinaface_node.py index f8d944e575..9cc12b257e 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/face_detection_retinaface_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/face_detection_retinaface_node.py @@ -14,11 +14,13 @@ # limitations under the License. import argparse +from time import perf_counter import mxnet as mx import rclpy from rclpy.node import Node +from std_msgs.msg import Float32 from sensor_msgs.msg import Image as ROS_Image from vision_msgs.msg import Detection2DArray from opendr_bridge import ROS2Bridge @@ -31,7 +33,7 @@ class FaceDetectionNode(Node): def __init__(self, input_rgb_image_topic="image_raw", output_rgb_image_topic="/opendr/image_faces_annotated", - detections_topic="/opendr/faces", device="cuda", backbone="resnet"): + detections_topic="/opendr/faces", performance_topic=None, device="cuda", backbone="resnet"): """ Creates a ROS2 Node for face detection with Retinaface. :param input_rgb_image_topic: Topic from which we are reading the input image @@ -42,6 +44,9 @@ def __init__(self, input_rgb_image_topic="image_raw", output_rgb_image_topic="/o :param detections_topic: Topic to which we are publishing the annotations (if None, no face detection message is published) :type detections_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str :param backbone: retinaface backbone, options are either 'mnet' or 'resnet', @@ -62,6 +67,11 @@ def __init__(self, input_rgb_image_topic="image_raw", output_rgb_image_topic="/o else: self.face_publisher = None + if performance_topic is not None: + self.performance_publisher = self.create_publisher(Float32, performance_topic, 1) + else: + self.performance_publisher = None + self.bridge = ROS2Bridge() self.face_detector = RetinaFaceLearner(backbone=backbone, device=device) @@ -77,12 +87,21 @@ def callback(self, data): :param data: Input image message :type data: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image image = self.bridge.from_ros_image(data, encoding='bgr8') # Run face detection boxes = self.face_detector.infer(image) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + if self.face_publisher is not None: # Publish detections in ROS message ros_boxes = self.bridge.to_ros_boxes(boxes) # Convert to ROS boxes @@ -109,6 +128,8 @@ def main(args=None): parser.add_argument("-d", "--detections_topic", help="Topic name for detection messages", type=lambda value: value if value.lower() != "none" else None, default="/opendr/faces") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) 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("--backbone", @@ -133,7 +154,8 @@ def main(args=None): face_detection_node = FaceDetectionNode(device=device, backbone=args.backbone, input_rgb_image_topic=args.input_rgb_image_topic, output_rgb_image_topic=args.output_rgb_image_topic, - detections_topic=args.detections_topic) + detections_topic=args.detections_topic, + performance_topic=args.performance_topic) rclpy.spin(face_detection_node) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/face_recognition_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/face_recognition_node.py index bb1ed80c24..3e415f04c8 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/face_recognition_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/face_recognition_node.py @@ -16,11 +16,12 @@ import cv2 import argparse import torch +from time import perf_counter import rclpy from rclpy.node import Node -from std_msgs.msg import String +from std_msgs.msg import String, Float32 from sensor_msgs.msg import Image as ROS_Image from vision_msgs.msg import ObjectHypothesisWithPose from opendr_bridge import ROS2Bridge @@ -35,7 +36,7 @@ class FaceRecognitionNode(Node): def __init__(self, input_rgb_image_topic="image_raw", output_rgb_image_topic="/opendr/image_face_reco_annotated", detections_topic="/opendr/face_recognition", detections_id_topic="/opendr/face_recognition_id", - database_path="./database", device="cuda", backbone="mobilefacenet"): + performance_topic=None, database_path="./database", device="cuda", backbone="mobilefacenet"): """ Creates a ROS2 Node for face recognition. :param input_rgb_image_topic: Topic from which we are reading the input image @@ -49,6 +50,9 @@ def __init__(self, input_rgb_image_topic="image_raw", output_rgb_image_topic="/o :param detections_id_topic: Topic to which we are publishing the ID of the recognized person (if None, no ID message is published) :type detections_id_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: Device on which we are running inference ('cpu' or 'cuda') :type device: str :param backbone: Backbone network @@ -76,6 +80,11 @@ def __init__(self, input_rgb_image_topic="image_raw", output_rgb_image_topic="/o else: self.face_id_publisher = None + if performance_topic is not None: + self.performance_publisher = self.create_publisher(Float32, performance_topic, 1) + else: + self.performance_publisher = None + self.bridge = ROS2Bridge() # Initialize the face recognizer @@ -98,6 +107,8 @@ def callback(self, data): :param data: Input image message :type data: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image image = self.bridge.from_ros_image(data, encoding='bgr8') # Get an OpenCV image back @@ -114,6 +125,13 @@ def callback(self, data): frame = image[startY:endY, startX:endX] result = self.recognizer.infer(frame) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + # Publish face information and ID if self.face_publisher is not None: self.face_publisher.publish(self.bridge.to_ros_face(result)) @@ -151,6 +169,8 @@ def main(args=None): parser.add_argument("-id", "--detections_id_topic", help="Topic name for detection ID messages", type=lambda value: value if value.lower() != "none" else None, default="/opendr/face_recognition_id") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) 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("--backbone", help="Backbone network, defaults to mobilefacenet", @@ -178,7 +198,8 @@ def main(args=None): input_rgb_image_topic=args.input_rgb_image_topic, output_rgb_image_topic=args.output_rgb_image_topic, detections_topic=args.detections_topic, - detections_id_topic=args.detections_id_topic) + detections_id_topic=args.detections_id_topic, + performance_topic=args.performance_topic) rclpy.spin(face_recognition_node) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/facial_emotion_estimation_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/facial_emotion_estimation_node.py index e1cc72a2d9..1b684f01c0 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/facial_emotion_estimation_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/facial_emotion_estimation_node.py @@ -19,10 +19,11 @@ import cv2 from torchvision import transforms import PIL +from time import perf_counter import rclpy from rclpy.node import Node -from std_msgs.msg import String +from std_msgs.msg import String, Float32 from vision_msgs.msg import ObjectHypothesis from sensor_msgs.msg import Image as ROS_Image from opendr_bridge import ROS2Bridge @@ -45,6 +46,7 @@ def __init__(self, output_rgb_image_topic="/opendr/image_emotion_estimation_annotated", output_emotions_topic="/opendr/facial_emotion_estimation", output_emotions_description_topic="/opendr/facial_emotion_estimation_description", + performance_topic=None, device="cuda"): """ Creates a ROS Node for facial emotion estimation. @@ -59,6 +61,9 @@ def __init__(self, :param output_emotions_description_topic: Topic to which we are publishing the description of the estimated facial emotion (if None, we are not publishing the description) :type output_emotions_description_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str """ @@ -82,6 +87,11 @@ def __init__(self, else: self.string_publisher = None + if performance_topic is not None: + self.performance_publisher = self.create_publisher(Float32, performance_topic, 1) + else: + self.performance_publisher = None + # Initialize the face detector self.face_detector = face_detector_learner @@ -101,12 +111,13 @@ def callback(self, data): :param data: input message :type data: sensor_msgs.msg.Image """ - + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image image = self.bridge.from_ros_image(data, encoding='bgr8').opencv() + emotion = None # Run face detection and emotion estimation - if image is not None: bounding_boxes = self.face_detector.infer(image) if bounding_boxes: @@ -127,6 +138,13 @@ def callback(self, data): affect = affect[0] # a numpy array of valence and arousal values emotion = emotion[0] # the emotion class with confidence tensor + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + cv2.rectangle(image, (startX, startY), (endX, endY), (0, 255, 255), thickness=2) cv2.putText(image, "Valence: %.2f" % affect[0], (startX, endY - 30), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 255), 1, cv2.LINE_AA) @@ -178,6 +196,8 @@ def main(args=None): help='Topic to which we are publishing the description of the estimated facial emotion', type=lambda value: value if value.lower() != "none" else None, default="/opendr/facial_emotion_estimation_description") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) parser.add_argument('-d', '--device', help='Device to use, either cpu or cuda', type=str, default="cuda", choices=["cuda", "cpu"]) args = parser.parse_args() @@ -207,6 +227,7 @@ def main(args=None): output_rgb_image_topic=args.output_rgb_image_topic, output_emotions_topic=args.output_emotions_topic, output_emotions_description_topic=args.output_emotions_description_topic, + performance_topic=args.performance_topic, device=device) rclpy.spin(facial_emotion_estimation_node) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/fall_detection_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/fall_detection_node.py index 0e9e86c75a..31c1bfd5dd 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/fall_detection_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/fall_detection_node.py @@ -16,10 +16,12 @@ import cv2 import argparse import torch +from time import perf_counter import rclpy from rclpy.node import Node +from std_msgs.msg import Float32 from sensor_msgs.msg import Image as ROS_Image from vision_msgs.msg import Detection2DArray from opendr_bridge import ROS2Bridge @@ -34,7 +36,7 @@ class FallDetectionNode(Node): def __init__(self, input_rgb_image_topic="image_raw", output_rgb_image_topic="/opendr/image_fallen_annotated", - detections_topic="/opendr/fallen", device="cuda", + detections_topic="/opendr/fallen", performance_topic=None, device="cuda", num_refinement_stages=2, use_stride=False, half_precision=False): """ Creates a ROS2 Node for rule-based fall detection based on Lightweight OpenPose. @@ -46,6 +48,9 @@ def __init__(self, input_rgb_image_topic="image_raw", output_rgb_image_topic="/o :param detections_topic: Topic to which we are publishing the annotations (if None, no pose detection message is published) :type detections_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str :param num_refinement_stages: Specifies the number of pose estimation refinement stages are added on the @@ -73,6 +78,11 @@ def __init__(self, input_rgb_image_topic="image_raw", output_rgb_image_topic="/o else: self.fall_publisher = None + if performance_topic is not None: + self.performance_publisher = self.create_publisher(Float32, performance_topic, 1) + else: + self.performance_publisher = None + self.bridge = ROS2Bridge() # Initialize the pose estimation learner @@ -93,24 +103,31 @@ def callback(self, data): :param data: Input image message :type data: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image image = self.bridge.from_ros_image(data, encoding='bgr8') # Run fall detection detections = self.fall_detector.infer(image) - # Get an OpenCV image back - image = image.opencv() - bboxes = BoundingBoxList([]) fallen_pose_id = 0 for detection in detections: fallen = detection[0].data - pose = detection[1] - x, y, w, h = get_bbox(pose) if fallen == 1: + pose = detection[1] + x, y, w, h = get_bbox(pose) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) if self.image_publisher is not None: + # Get an OpenCV image back + image = image.opencv() # Paint person bounding box inferred from pose color = (0, 0, 255) cv2.rectangle(image, (x, y), (x + w, y + h), color, 2) @@ -142,6 +159,8 @@ def main(args=None): parser.add_argument("-d", "--detections_topic", help="Topic name for detection messages", type=lambda value: value if value.lower() != "none" else None, default="/opendr/fallen") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) 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("--accelerate", help="Enables acceleration flags (e.g., stride)", default=False, @@ -174,6 +193,7 @@ def main(args=None): input_rgb_image_topic=args.input_rgb_image_topic, output_rgb_image_topic=args.output_rgb_image_topic, detections_topic=args.detections_topic, + performance_topic=args.performance_topic, num_refinement_stages=stages, use_stride=stride, half_precision=half_prec) rclpy.spin(fall_detection_node) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/heart_anomaly_detection_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/heart_anomaly_detection_node.py index 138eb347d9..403e0e2480 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/heart_anomaly_detection_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/heart_anomaly_detection_node.py @@ -16,11 +16,12 @@ import argparse import torch +from time import perf_counter import rclpy from rclpy.node import Node from vision_msgs.msg import Classification2D -from std_msgs.msg import Float32MultiArray +from std_msgs.msg import Float32MultiArray, Float32 from opendr_bridge import ROS2Bridge from opendr.perception.heart_anomaly_detection import GatedRecurrentUnitLearner, AttentionNeuralBagOfFeatureLearner @@ -29,13 +30,16 @@ class HeartAnomalyNode(Node): def __init__(self, input_ecg_topic="/ecg/ecg", output_heart_anomaly_topic="/opendr/heart_anomaly", - device="cuda", model="anbof"): + performance_topic=None, device="cuda", model="anbof"): """ Creates a ROS2 Node for heart anomaly (atrial fibrillation) detection from ecg data :param input_ecg_topic: Topic from which we are reading the input array data :type input_ecg_topic: str :param output_heart_anomaly_topic: Topic to which we are publishing the predicted class :type output_heart_anomaly_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str :param model: model to use: anbof or gru @@ -47,6 +51,11 @@ def __init__(self, input_ecg_topic="/ecg/ecg", output_heart_anomaly_topic="/open self.subscriber = self.create_subscription(Float32MultiArray, input_ecg_topic, self.callback, 1) + if performance_topic is not None: + self.performance_publisher = self.create_publisher(Float32, performance_topic, 1) + else: + self.performance_publisher = None + self.bridge = ROS2Bridge() # AF dataset @@ -71,12 +80,21 @@ def callback(self, msg_data): :param msg_data: input message :type msg_data: std_msgs.msg.Float32MultiArray """ + if self.performance_publisher: + start_time = perf_counter() # Convert Float32MultiArray to OpenDR Timeseries data = self.bridge.from_rosarray_to_timeseries(msg_data, self.channels, self.series_length) # Run ecg classification class_pred = self.learner.infer(data) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + # Publish results ros_class = self.bridge.from_category_to_rosclass(class_pred, self.get_clock().now().to_msg()) self.publisher.publish(ros_class) @@ -90,6 +108,8 @@ def main(args=None): help="listen to input ECG data on this topic") parser.add_argument("-o", "--output_heart_anomaly_topic", type=str, default="/opendr/heart_anomaly", help="Topic name for heart anomaly detection topic") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) parser.add_argument("--model", type=str, default="anbof", help="model to be used for prediction: anbof or gru", choices=["anbof", "gru"]) parser.add_argument("--device", type=str, default="cuda", help="Device to use (cpu, cuda)", @@ -111,6 +131,7 @@ def main(args=None): heart_anomaly_detection_node = HeartAnomalyNode(input_ecg_topic=args.input_ecg_topic, output_heart_anomaly_topic=args.output_heart_anomaly_topic, + performance_topic=args.performance_topic, model=args.model, device=device) rclpy.spin(heart_anomaly_detection_node) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/hr_pose_estimation_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/hr_pose_estimation_node.py index 7c1240a5d8..5db2057120 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/hr_pose_estimation_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/hr_pose_estimation_node.py @@ -15,10 +15,12 @@ import argparse import torch +from time import perf_counter import rclpy from rclpy.node import Node +from std_msgs.msg import Float32 from sensor_msgs.msg import Image as ROS_Image from opendr_bridge import ROS2Bridge from opendr_interface.msg import OpenDRPose2D @@ -28,13 +30,13 @@ from opendr.perception.pose_estimation import HighResolutionPoseEstimationLearner -class PoseEstimationNode(Node): +class HRPoseEstimationNode(Node): def __init__(self, input_rgb_image_topic="image_raw", output_rgb_image_topic="/opendr/image_pose_annotated", detections_topic="/opendr/poses", device="cuda", - num_refinement_stages=2, use_stride=False, half_precision=False): + performance_topic=None, num_refinement_stages=2, use_stride=False, half_precision=False): """ - Creates a ROS2 Node for pose estimation with Lightweight OpenPose. + Creates a ROS2 Node for high resolution pose estimation with HR Pose Estimation. :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, no annotated @@ -43,6 +45,9 @@ def __init__(self, input_rgb_image_topic="image_raw", output_rgb_image_topic="/o :param detections_topic: Topic to which we are publishing the annotations (if None, no pose detection message is published) :type detections_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str :param num_refinement_stages: Specifies the number of pose estimation refinement stages are added on the @@ -70,6 +75,11 @@ def __init__(self, input_rgb_image_topic="image_raw", output_rgb_image_topic="/o else: self.pose_publisher = None + if performance_topic is not None: + self.performance_publisher = self.create_publisher(Float32, performance_topic, 1) + else: + self.performance_publisher = None + self.bridge = ROS2Bridge() # Initialize the high resolution pose estimation learner @@ -87,12 +97,21 @@ def callback(self, data): :param data: Input image message :type data: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image image = self.bridge.from_ros_image(data, encoding='bgr8') # Run pose estimation poses = self.pose_estimator.infer(image) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + # Publish detections in ROS message if self.pose_publisher is not None: for pose in poses: @@ -126,6 +145,8 @@ def main(args=None): "no detection message is published", type=lambda value: value if value.lower() != "none" else None, default="/opendr/poses") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) 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("--accelerate", help="Enables acceleration flags (e.g., stride)", default=False, @@ -154,11 +175,12 @@ def main(args=None): stages = 2 half_prec = False - pose_estimator_node = PoseEstimationNode(device=device, - input_rgb_image_topic=args.input_rgb_image_topic, - output_rgb_image_topic=args.output_rgb_image_topic, - detections_topic=args.detections_topic, - num_refinement_stages=stages, use_stride=stride, half_precision=half_prec) + pose_estimator_node = HRPoseEstimationNode(device=device, + input_rgb_image_topic=args.input_rgb_image_topic, + output_rgb_image_topic=args.output_rgb_image_topic, + detections_topic=args.detections_topic, + performance_topic=args.performance_topic, + num_refinement_stages=stages, use_stride=stride, half_precision=half_prec) rclpy.spin(pose_estimator_node) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/landmark_based_facial_expression_recognition_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/landmark_based_facial_expression_recognition_node.py index 0c3bf8f3a1..45379a998c 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/landmark_based_facial_expression_recognition_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/landmark_based_facial_expression_recognition_node.py @@ -16,10 +16,11 @@ import argparse import torch import numpy as np +from time import perf_counter import rclpy from rclpy.node import Node -from std_msgs.msg import String +from std_msgs.msg import String, Float32 from vision_msgs.msg import ObjectHypothesis from sensor_msgs.msg import Image as ROS_Image from opendr_bridge import ROS2Bridge @@ -35,7 +36,7 @@ class LandmarkFacialExpressionRecognitionNode(Node): def __init__(self, input_rgb_image_topic="image_raw", output_category_topic="/opendr/landmark_expression_recognition", output_category_description_topic="/opendr/landmark_expression_recognition_description", - device="cpu", model='pstbln_afew', shape_predictor='./predictor_path'): + performance_topic=None, device="cpu", model='pstbln_afew', shape_predictor='./predictor_path'): """ Creates a ROS2 Node for landmark-based facial expression recognition. :param input_rgb_image_topic: Topic from which we are reading the input image @@ -69,6 +70,11 @@ def __init__(self, input_rgb_image_topic="image_raw", else: self.string_publisher = None + if performance_topic is not None: + self.performance_publisher = self.create_publisher(Float32, performance_topic, 1) + else: + self.performance_publisher = None + self.bridge = ROS2Bridge() # Initialize the landmark-based facial expression recognition @@ -98,7 +104,8 @@ def callback(self, data): :param data: input message :type data: sensor_msgs.msg.Image """ - + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image image = self.bridge.from_ros_image(data, encoding='bgr8') landmarks = landmark_extractor(image, './landmarks.npy', self.shape_predictor) @@ -112,6 +119,13 @@ def callback(self, data): # Run expression recognition category = self.expression_classifier.infer(muscle_data) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + if self.hypothesis_publisher is not None: self.hypothesis_publisher.publish(self.bridge.to_ros_category(category)) @@ -142,6 +156,8 @@ def main(args=None): parser.add_argument("-d", "--output_category_description_topic", help="Topic name for category description", type=lambda value: value if value.lower() != "none" else None, default="/opendr/landmark_expression_recognition_description") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) 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("--model", help="Model to use, either 'pstbln_ck+', 'pstbln_casia', 'pstbln_afew'", @@ -168,6 +184,7 @@ def main(args=None): input_rgb_image_topic=args.input_rgb_image_topic, output_category_topic=args.output_category_topic, output_category_description_topic=args.output_category_description_topic, + performance_topic=args.performance_topic, device=device, model=args.model, shape_predictor=args.shape_predictor) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_centernet_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_centernet_node.py index 07b72afdc0..6f5f2d6cc0 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_centernet_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_centernet_node.py @@ -15,10 +15,12 @@ import argparse import mxnet as mx +from time import perf_counter import rclpy from rclpy.node import Node +from std_msgs.msg import Float32 from sensor_msgs.msg import Image as ROS_Image from vision_msgs.msg import Detection2DArray from opendr_bridge import ROS2Bridge @@ -31,7 +33,7 @@ class ObjectDetectionCenterNetNode(Node): def __init__(self, input_rgb_image_topic="image_raw", output_rgb_image_topic="/opendr/image_objects_annotated", - detections_topic="/opendr/objects", device="cuda", backbone="resnet50_v1b"): + detections_topic="/opendr/objects", performance_topic=None, device="cuda", backbone="resnet50_v1b"): """ Creates a ROS2 Node for object detection with Centernet. :param input_rgb_image_topic: Topic from which we are reading the input image @@ -42,6 +44,9 @@ def __init__(self, input_rgb_image_topic="image_raw", output_rgb_image_topic="/o :param detections_topic: Topic to which we are publishing the annotations (if None, no object detection message is published) :type detections_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str :param backbone: backbone network @@ -61,6 +66,11 @@ def __init__(self, input_rgb_image_topic="image_raw", output_rgb_image_topic="/o else: self.object_publisher = None + if performance_topic is not None: + self.performance_publisher = self.create_publisher(Float32, performance_topic, 1) + else: + self.performance_publisher = None + self.bridge = ROS2Bridge() self.object_detector = CenterNetDetectorLearner(backbone=backbone, device=device) @@ -75,12 +85,21 @@ def callback(self, data): :param data: input message :type data: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image image = self.bridge.from_ros_image(data, encoding='bgr8') # Run object detection boxes = self.object_detector.infer(image, threshold=0.45, keep_size=False) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + if self.object_publisher is not None: # Publish detections in ROS message ros_boxes = self.bridge.to_ros_boxes(boxes) # Convert to ROS boxes @@ -107,6 +126,8 @@ def main(args=None): parser.add_argument("-d", "--detections_topic", help="Topic name for detection messages", type=lambda value: value if value.lower() != "none" else None, default="/opendr/objects") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) parser.add_argument("--device", help="Device to use (cpu, cuda)", type=str, default="cuda", choices=["cuda", "cpu"]) parser.add_argument("--backbone", help="Backbone network, defaults to \"resnet50_v1b\"", type=str, default="resnet50_v1b", choices=["resnet50_v1b"]) @@ -128,7 +149,8 @@ def main(args=None): object_detection_centernet_node = ObjectDetectionCenterNetNode(device=device, backbone=args.backbone, input_rgb_image_topic=args.input_rgb_image_topic, output_rgb_image_topic=args.output_rgb_image_topic, - detections_topic=args.detections_topic) + detections_topic=args.detections_topic, + performance_topic=args.performance_topic) rclpy.spin(object_detection_centernet_node) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_detr_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_detr_node.py index 5b0c5fb746..d324b27214 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_detr_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_detr_node.py @@ -16,10 +16,12 @@ import argparse import torch import numpy as np +from time import perf_counter import rclpy from rclpy.node import Node +from std_msgs.msg import Float32 from sensor_msgs.msg import Image as ROS_Image from vision_msgs.msg import Detection2DArray from opendr_bridge import ROS2Bridge @@ -31,11 +33,12 @@ class ObjectDetectionDetrNode(Node): def __init__( - self, - input_rgb_image_topic="image_raw", - output_rgb_image_topic="/opendr/image_objects_annotated", - detections_topic="/opendr/objects", - device="cuda", + self, + input_rgb_image_topic="image_raw", + output_rgb_image_topic="/opendr/image_objects_annotated", + detections_topic="/opendr/objects", + performance_topic=None, + device="cuda", ): """ Creates a ROS2 Node for object detection with DETR. @@ -47,6 +50,9 @@ def __init__( :param detections_topic: Topic to which we are publishing the annotations (if None, no object detection message is published) :type detections_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str """ @@ -62,6 +68,11 @@ def __init__( else: self.detection_publisher = None + if performance_topic is not None: + self.performance_publisher = self.create_publisher(Float32, performance_topic, 1) + else: + self.performance_publisher = None + self.image_subscriber = self.create_subscription(ROS_Image, input_rgb_image_topic, self.callback, 1) self.bridge = ROS2Bridge() @@ -172,12 +183,21 @@ def callback(self, data): :param data: input message :type data: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image image = self.bridge.from_ros_image(data, encoding="bgr8") # Run detection estimation boxes = self.object_detector.infer(image) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + # Annotate image and publish results: if self.detection_publisher is not None: ros_detection = self.bridge.to_ros_bounding_box_list(boxes) @@ -205,6 +225,8 @@ def main(args=None): parser.add_argument("-d", "--detections_topic", help="Topic name for detection messages", type=lambda value: value if value.lower() != "none" else None, default="/opendr/objects") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) 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() @@ -222,12 +244,11 @@ def main(args=None): print("Using CPU.") device = "cpu" - object_detection_detr_node = ObjectDetectionDetrNode( - device=device, - input_rgb_image_topic=args.input_rgb_image_topic, - output_rgb_image_topic=args.output_rgb_image_topic, - detections_topic=args.detections_topic, - ) + object_detection_detr_node = ObjectDetectionDetrNode(device=device, + input_rgb_image_topic=args.input_rgb_image_topic, + output_rgb_image_topic=args.output_rgb_image_topic, + detections_topic=args.detections_topic, + performance_topic=args.performance_topic) rclpy.spin(object_detection_detr_node) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_gem_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_gem_node.py index 3d4f73e1f4..7173dfcd04 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_gem_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_gem_node.py @@ -14,33 +14,35 @@ # limitations under the License. -import argparse -import cv2 -import message_filters -import numpy as np import rclpy -import torch from rclpy.node import Node -from opendr_bridge import ROS2Bridge -from sensor_msgs.msg import Image as ROS_Image +import torch +import message_filters +import cv2 +import numpy as np +import argparse +from time import perf_counter +from std_msgs.msg import Float32 from vision_msgs.msg import Detection2DArray - -from opendr.engine.data import Image +from sensor_msgs.msg import Image as ROS_Image +from opendr_bridge import ROS2Bridge from opendr.perception.object_detection_2d import GemLearner from opendr.perception.object_detection_2d import draw_bounding_boxes +from opendr.engine.data import Image class ObjectDetectionGemNode(Node): def __init__( - self, - input_rgb_image_topic="/camera/color/image_raw", - input_infra_image_topic="/camera/infra/image_raw", - output_rgb_image_topic="/opendr/rgb_image_objects_annotated", - output_infra_image_topic="/opendr/infra_image_objects_annotated", - detections_topic="/opendr/objects", - device="cuda", - pts_rgb=None, - pts_infra=None, + self, + input_rgb_image_topic="/camera/color/image_raw", + input_infra_image_topic="/camera/infra/image_raw", + output_rgb_image_topic="/opendr/rgb_image_objects_annotated", + output_infra_image_topic="/opendr/infra_image_objects_annotated", + detections_topic="/opendr/objects", + performance_topic=None, + device="cuda", + pts_rgb=None, + pts_infra=None, ): """ Creates a ROS2 Node for object detection with GEM @@ -57,6 +59,9 @@ def __init__( :param detections_topic: Topic to which we are publishing the annotations (if None, we are not publishing annotations) :type detections_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: Device on which we are running inference ('cpu' or 'cuda') :type device: str :param pts_rgb: Point on the rgb image that define alignment with the infrared image. These are camera @@ -83,6 +88,12 @@ def __init__( self.detection_publisher = self.create_publisher(msg_type=Detection2DArray, topic=detections_topic, qos_profile=10) else: self.detection_publisher = None + + if performance_topic is not None: + self.performance_publisher = self.create_publisher(Float32, performance_topic, 1) + else: + self.performance_publisher = None + if pts_infra is None: pts_infra = np.array( [ @@ -201,6 +212,8 @@ def callback(self, msg_rgb, msg_ir): :param msg_ir: input infrared image message :type msg_ir: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() # Convert images to OpenDR standard image_rgb = self.bridge.from_ros_image(msg_rgb).opencv() image_ir_raw = self.bridge.from_ros_image(msg_ir, "bgr8").opencv() @@ -209,6 +222,13 @@ def callback(self, msg_rgb, msg_ir): # Perform inference on images boxes, w_sensor1, _ = self.gem_learner.infer(image_rgb, image_ir) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + # Annotate image and publish results: if self.detection_publisher is not None: ros_detection = self.bridge.to_ros_bounding_box_list(boxes) @@ -243,6 +263,8 @@ def main(args=None): parser.add_argument("-d", "--detections_topic", help="Topic name for detection messages", type=lambda value: value if value.lower() != "none" else None, default="/opendr/objects") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) 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() @@ -267,6 +289,7 @@ def main(args=None): input_infra_image_topic=args.input_infra_image_topic, output_infra_image_topic=args.output_infra_image_topic, detections_topic=args.detections_topic, + performance_topic=args.performance_topic, ) rclpy.spin(gem_node) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_nanodet_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_nanodet_node.py index ef5fcbfb2c..64897a99a9 100755 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_nanodet_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_nanodet_node.py @@ -15,10 +15,12 @@ import argparse import torch +from time import perf_counter import rclpy from rclpy.node import Node +from std_msgs.msg import Float32 from sensor_msgs.msg import Image as ROS_Image from vision_msgs.msg import Detection2DArray from opendr_bridge import ROS2Bridge @@ -31,7 +33,7 @@ class ObjectDetectionNanodetNode(Node): def __init__(self, input_rgb_image_topic="image_raw", output_rgb_image_topic="/opendr/image_objects_annotated", - detections_topic="/opendr/objects", device="cuda", model="plus_m_1.5x_416"): + detections_topic="/opendr/objects", performance_topic=None, device="cuda", model="plus_m_1.5x_416"): """ Creates a ROS2 Node for object detection with Nanodet. :param input_rgb_image_topic: Topic from which we are reading the input image @@ -42,6 +44,9 @@ def __init__(self, input_rgb_image_topic="image_raw", output_rgb_image_topic="/o :param detections_topic: Topic to which we are publishing the annotations (if None, no object detection message is published) :type detections_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str :param model: the name of the model of which we want to load the config file @@ -61,6 +66,11 @@ def __init__(self, input_rgb_image_topic="image_raw", output_rgb_image_topic="/o else: self.object_publisher = None + if performance_topic is not None: + self.performance_publisher = self.create_publisher(Float32, performance_topic, 1) + else: + self.performance_publisher = None + self.bridge = ROS2Bridge() # Initialize the object detector @@ -76,21 +86,28 @@ def callback(self, data): :param data: input message :type data: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image image = self.bridge.from_ros_image(data, encoding='bgr8') # Run object detection - boxes = self.object_detector.infer(image, threshold=0.35) + boxes = self.object_detector.infer(image, conf_threshold=0.35) - # Get an OpenCV image back - image = image.opencv() + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) # Publish detections in ROS message - ros_boxes = self.bridge.to_ros_boxes(boxes) # Convert to ROS boxes if self.object_publisher is not None: - self.object_publisher.publish(ros_boxes) + self.object_publisher.publish(self.bridge.to_ros_boxes(boxes)) if self.image_publisher is not None: + # Get an OpenCV image back + image = image.opencv() # Annotate image with object detection boxes image = draw_bounding_boxes(image, boxes, class_names=self.object_detector.classes) # Convert the annotated OpenDR image to ROS2 image message using bridge and publish it @@ -109,6 +126,8 @@ def main(args=None): parser.add_argument("-d", "--detections_topic", help="Topic name for detection messages", type=lambda value: value if value.lower() != "none" else None, default="/opendr/objects") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) parser.add_argument("--device", help="Device to use (cpu, cuda)", type=str, default="cuda", choices=["cuda", "cpu"]) parser.add_argument("--model", help="Model that config file will be used", type=str, default="plus_m_1.5x_416") args = parser.parse_args() @@ -129,7 +148,8 @@ def main(args=None): object_detection_nanodet_node = ObjectDetectionNanodetNode(device=device, model=args.model, input_rgb_image_topic=args.input_rgb_image_topic, output_rgb_image_topic=args.output_rgb_image_topic, - detections_topic=args.detections_topic) + detections_topic=args.detections_topic, + performance_topic=args.performance_topic) rclpy.spin(object_detection_nanodet_node) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_ssd_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_ssd_node.py index 4b92063e5b..80839f677d 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_ssd_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_ssd_node.py @@ -15,10 +15,12 @@ import argparse import mxnet as mx +from time import perf_counter import rclpy from rclpy.node import Node +from std_msgs.msg import Float32 from sensor_msgs.msg import Image as ROS_Image from vision_msgs.msg import Detection2DArray from opendr_bridge import ROS2Bridge @@ -32,7 +34,8 @@ class ObjectDetectionSSDNode(Node): def __init__(self, input_rgb_image_topic="image_raw", output_rgb_image_topic="/opendr/image_objects_annotated", - detections_topic="/opendr/objects", device="cuda", backbone="vgg16_atrous", nms_type='default'): + detections_topic="/opendr/objects", performance_topic=None, + device="cuda", backbone="vgg16_atrous", nms_type='default'): """ Creates a ROS2 Node for object detection with SSD. :param input_rgb_image_topic: Topic from which we are reading the input image @@ -43,6 +46,9 @@ def __init__(self, input_rgb_image_topic="image_raw", output_rgb_image_topic="/o :param detections_topic: Topic to which we are publishing the annotations (if None, no object detection message is published) :type detections_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str :param backbone: backbone network @@ -65,6 +71,11 @@ def __init__(self, input_rgb_image_topic="image_raw", output_rgb_image_topic="/o else: self.object_publisher = None + if performance_topic is not None: + self.performance_publisher = self.create_publisher(Float32, performance_topic, 1) + else: + self.performance_publisher = None + self.bridge = ROS2Bridge() self.object_detector = SingleShotDetectorLearner(backbone=backbone, device=device) @@ -99,16 +110,24 @@ def callback(self, data): :param data: input message :type data: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image image = self.bridge.from_ros_image(data, encoding='bgr8') # Run object detection boxes = self.object_detector.infer(image, threshold=0.45, keep_size=False, custom_nms=self.custom_nms) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + if self.object_publisher is not None: # Publish detections in ROS message - ros_boxes = self.bridge.to_ros_boxes(boxes) # Convert to ROS boxes - self.object_publisher.publish(ros_boxes) + self.object_publisher.publish(self.bridge.to_ros_boxes(boxes)) if self.image_publisher is not None: # Get an OpenCV image back @@ -130,6 +149,8 @@ def main(args=None): default="/opendr/image_objects_annotated") parser.add_argument("-d", "--detections_topic", help="Topic name for detection messages", type=lambda value: value if value.lower() != "none" else None, default="/opendr/objects") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) parser.add_argument("--device", help="Device to use (cpu, cuda)", type=str, default="cuda", choices=["cuda", "cpu"]) parser.add_argument("--backbone", help="Backbone network, defaults to vgg16_atrous", type=str, default="vgg16_atrous", choices=["vgg16_atrous"]) @@ -155,7 +176,8 @@ def main(args=None): object_detection_ssd_node = ObjectDetectionSSDNode(device=device, backbone=args.backbone, nms_type=args.nms_type, input_rgb_image_topic=args.input_rgb_image_topic, output_rgb_image_topic=args.output_rgb_image_topic, - detections_topic=args.detections_topic) + detections_topic=args.detections_topic, + performance_topic=args.performance_topic) rclpy.spin(object_detection_ssd_node) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_yolov3_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_yolov3_node.py index 2ca7e9e860..80c535c922 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_yolov3_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_yolov3_node.py @@ -15,10 +15,12 @@ import argparse import mxnet as mx +from time import perf_counter import rclpy from rclpy.node import Node +from std_msgs.msg import Float32 from sensor_msgs.msg import Image as ROS_Image from vision_msgs.msg import Detection2DArray from opendr_bridge import ROS2Bridge @@ -31,7 +33,7 @@ class ObjectDetectionYOLOV3Node(Node): def __init__(self, input_rgb_image_topic="image_raw", output_rgb_image_topic="/opendr/image_objects_annotated", - detections_topic="/opendr/objects", device="cuda", backbone="darknet53"): + detections_topic="/opendr/objects", performance_topic=None, device="cuda", backbone="darknet53"): """ Creates a ROS2 Node for object detection with YOLOV3 :param input_rgb_image_topic: Topic from which we are reading the input image @@ -42,6 +44,9 @@ def __init__(self, input_rgb_image_topic="image_raw", output_rgb_image_topic="/o :param detections_topic: Topic to which we are publishing the annotations (if None, no object detection message is published) :type detections_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str :param backbone: backbone network @@ -61,6 +66,11 @@ def __init__(self, input_rgb_image_topic="image_raw", output_rgb_image_topic="/o else: self.object_publisher = None + if performance_topic is not None: + self.performance_publisher = self.create_publisher(Float32, performance_topic, 1) + else: + self.performance_publisher = None + self.bridge = ROS2Bridge() self.object_detector = YOLOv3DetectorLearner(backbone=backbone, device=device) @@ -75,16 +85,24 @@ def callback(self, data): :param data: input message :type data: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image image = self.bridge.from_ros_image(data, encoding='bgr8') # Run object detection boxes = self.object_detector.infer(image, threshold=0.1, keep_size=False) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + + # Publish detections in ROS message if self.object_publisher is not None: - # Publish detections in ROS message - ros_boxes = self.bridge.to_ros_bounding_box_list(boxes) # Convert to ROS bounding_box_list - self.object_publisher.publish(ros_boxes) + self.object_publisher.publish(self.bridge.to_ros_bounding_box_list(boxes)) if self.image_publisher is not None: # Get an OpenCV image back @@ -106,6 +124,8 @@ def main(args=None): default="/opendr/image_objects_annotated") parser.add_argument("-d", "--detections_topic", help="Topic name for detection messages", type=lambda value: value if value.lower() != "none" else None, default="/opendr/objects") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) 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("--backbone", help="Backbone network, defaults to \"darknet53\"", @@ -128,7 +148,8 @@ def main(args=None): object_detection_yolov3_node = ObjectDetectionYOLOV3Node(device=device, backbone=args.backbone, input_rgb_image_topic=args.input_rgb_image_topic, output_rgb_image_topic=args.output_rgb_image_topic, - detections_topic=args.detections_topic) + detections_topic=args.detections_topic, + performance_topic=args.performance_topic) rclpy.spin(object_detection_yolov3_node) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_yolov5_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_yolov5_node.py index 259f2c207f..e5069a927d 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_yolov5_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_yolov5_node.py @@ -15,10 +15,12 @@ import argparse import torch +from time import perf_counter import rclpy from rclpy.node import Node +from std_msgs.msg import Float32 from sensor_msgs.msg import Image as ROS_Image from vision_msgs.msg import Detection2DArray from opendr_bridge import ROS2Bridge @@ -31,7 +33,7 @@ class ObjectDetectionYOLOV5Node(Node): def __init__(self, input_rgb_image_topic="image_raw", output_rgb_image_topic="/opendr/image_objects_annotated", - detections_topic="/opendr/objects", device="cuda", model="yolov5s"): + detections_topic="/opendr/objects", performance_topic=None, device="cuda", model="yolov5s"): """ Creates a ROS2 Node for object detection with YOLOV5. :param input_rgb_image_topic: Topic from which we are reading the input image @@ -42,6 +44,9 @@ def __init__(self, input_rgb_image_topic="image_raw", output_rgb_image_topic="/o :param detections_topic: Topic to which we are publishing the annotations (if None, no object detection message is published) :type detections_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str :param model: model to use @@ -61,6 +66,11 @@ def __init__(self, input_rgb_image_topic="image_raw", output_rgb_image_topic="/o else: self.object_publisher = None + if performance_topic is not None: + self.performance_publisher = self.create_publisher(Float32, performance_topic, 1) + else: + self.performance_publisher = None + self.bridge = ROS2Bridge() self.object_detector = YOLOv5DetectorLearner(model_name=model, device=device) @@ -73,16 +83,24 @@ def callback(self, data): :param data: input message :type data: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image image = self.bridge.from_ros_image(data, encoding='bgr8') # Run object detection boxes = self.object_detector.infer(image) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + + # Publish detections in ROS message if self.object_publisher is not None: - # Publish detections in ROS message - ros_boxes = self.bridge.to_ros_bounding_box_list(boxes) # Convert to ROS bounding_box_list - self.object_publisher.publish(ros_boxes) + self.object_publisher.publish(self.bridge.to_ros_bounding_box_list(boxes)) if self.image_publisher is not None: # Get an OpenCV image back @@ -104,6 +122,8 @@ def main(args=None): default="/opendr/image_objects_annotated") parser.add_argument("-d", "--detections_topic", help="Topic name for detection messages", type=lambda value: value if value.lower() != "none" else None, default="/opendr/objects") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) 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("--model", help="Model to use, defaults to \"yolov5s\"", type=str, default="yolov5s", @@ -127,7 +147,8 @@ def main(args=None): object_detection_yolov5_node = ObjectDetectionYOLOV5Node(device=device, model=args.model, input_rgb_image_topic=args.input_rgb_image_topic, output_rgb_image_topic=args.output_rgb_image_topic, - detections_topic=args.detections_topic) + detections_topic=args.detections_topic, + performance_topic=args.performance_topic) rclpy.spin(object_detection_yolov5_node) 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 859fb6099a..5567e526c6 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 @@ -16,8 +16,10 @@ import torch import argparse import os +from time import perf_counter import rclpy from rclpy.node import Node +from std_msgs.msg import Float32 from vision_msgs.msg import Detection3DArray from sensor_msgs.msg import PointCloud as ROS_PointCloud from opendr_bridge import ROS2Bridge @@ -28,7 +30,7 @@ class ObjectDetection3DVoxelNode(Node): def __init__( self, input_point_cloud_topic="/opendr/dataset_point_cloud", - detections_topic="/opendr/objects3d", + detections_topic="/opendr/objects3d", performance_topic=None, device="cuda:0", model_name="tanet_car_xyres_16", model_config_path=os.path.join( @@ -44,6 +46,9 @@ def __init__( :type input_point_cloud_topic: str :param detections_topic: Topic to which we are publishing the annotations :type detections_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_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 @@ -71,6 +76,11 @@ def __init__( Detection3DArray, detections_topic, 1 ) + if performance_topic is not None: + self.performance_publisher = self.create_subscription(Float32, performance_topic, 1) + else: + self.performance_publisher = None + self.create_subscription(ROS_PointCloud, input_point_cloud_topic, self.callback, 1) self.get_logger().info("Object Detection 3D Voxel Node initialized.") @@ -81,11 +91,19 @@ def callback(self, data): :param data: input message :type data: sensor_msgs.msg.Image """ - + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image point_cloud = self.bridge.from_ros_point_cloud(data) detection_boxes = self.learner.infer(point_cloud) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + # Convert detected boxes to ROS type and publish ros_boxes = self.bridge.to_ros_boxes_3d(detection_boxes) self.detection_publisher.publish(ros_boxes) @@ -101,6 +119,8 @@ def main(args=None): parser.add_argument("-d", "--detections_topic", help="Output detections topic", type=str, default="/opendr/objects3d") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) 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", @@ -137,6 +157,7 @@ def main(args=None): input_point_cloud_topic=args.input_point_cloud_topic, temp_dir=args.temp_dir, detections_topic=args.detections_topic, + performance_topic=args.performance_topic ) rclpy.spin(voxel_node) 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 f139492246..e3db562b8c 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,13 +17,15 @@ import argparse import cv2 import os -from opendr.engine.target import TrackingAnnotationList +from time import perf_counter import rclpy from rclpy.node import Node +from std_msgs.msg import Float32 from vision_msgs.msg import Detection2DArray from std_msgs.msg import Int32MultiArray from sensor_msgs.msg import Image as ROS_Image from opendr_bridge import ROS2Bridge +from opendr.engine.target import TrackingAnnotationList from opendr.perception.object_tracking_2d import ( ObjectTracking2DDeepSortLearner, ObjectTracking2DFairMotLearner @@ -39,6 +41,7 @@ def __init__( output_detection_topic="/opendr/objects", output_tracking_id_topic="/opendr/objects_tracking_id", output_rgb_image_topic="/opendr/image_objects_annotated", + performance_topic=None, device="cuda:0", model_name="deep_sort", temp_dir="temp", @@ -56,6 +59,9 @@ def __init__( :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 performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_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 @@ -94,6 +100,11 @@ def __init__( Detection2DArray, output_detection_topic, 1 ) + if performance_topic is not None: + self.performance_publisher = self.create_subscription(Float32, performance_topic, 1) + else: + self.performance_publisher = None + self.create_subscription(ROS_Image, input_rgb_image_topic, self.callback, 1) def callback(self, data): @@ -102,13 +113,21 @@ def callback(self, data): :param data: input message :type data: sensor_msgs.msg.Image """ - + if self.performance_publisher: + start_time = perf_counter() # 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, swap_left_top=False) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + if self.output_image_publisher is not None: frame = image.opencv() draw_predictions(frame, tracking_boxes) @@ -196,6 +215,8 @@ def main(args=None): 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("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) 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", @@ -234,6 +255,7 @@ def main(args=None): output_detection_topic=args.detections_topic, output_tracking_id_topic=args.tracking_id_topic, output_rgb_image_topic=args.output_rgb_image_topic, + performance_topic=args.performance_topic ) 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 65097bb120..b6c733d845 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,13 +17,14 @@ import argparse import cv2 import os -from opendr.engine.target import TrackingAnnotationList +from time import perf_counter import rclpy from rclpy.node import Node from vision_msgs.msg import Detection2DArray -from std_msgs.msg import Int32MultiArray +from std_msgs.msg import Int32MultiArray, Float32 from sensor_msgs.msg import Image as ROS_Image from opendr_bridge import ROS2Bridge +from opendr.engine.target import TrackingAnnotationList from opendr.perception.object_tracking_2d import ( ObjectTracking2DFairMotLearner, ) @@ -37,6 +38,7 @@ def __init__( output_rgb_image_topic="/opendr/image_objects_annotated", output_detection_topic="/opendr/objects", output_tracking_id_topic="/opendr/objects_tracking_id", + performance_topic=None, device="cuda:0", model_name="fairmot_dla34", temp_dir="temp", @@ -52,6 +54,9 @@ def __init__( :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 performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_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 @@ -88,6 +93,11 @@ def __init__( ROS_Image, output_rgb_image_topic, 1 ) + if performance_topic is not None: + self.performance_publisher = self.create_subscription(Float32, performance_topic, 1) + else: + self.performance_publisher = None + self.create_subscription(ROS_Image, input_rgb_image_topic, self.callback, 1) def callback(self, data): @@ -96,11 +106,19 @@ def callback(self, data): :param data: input message :type data: sensor_msgs.msg.Image """ - + if self.performance_publisher: + start_time = perf_counter() # 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.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + if self.output_image_publisher is not None: frame = image.opencv() draw_predictions(frame, tracking_boxes) @@ -188,6 +206,8 @@ def main(args=None): 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("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) 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", @@ -217,6 +237,7 @@ def main(args=None): output_detection_topic=args.detections_topic, output_tracking_id_topic=args.tracking_id_topic, output_rgb_image_topic=args.output_rgb_image_topic, + performance_topic=args.performance_topic ) rclpy.spin(fair_mot_node) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_siamrpn_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_siamrpn_node.py index 9b3e602e49..95e3a6888f 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_siamrpn_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_siamrpn_node.py @@ -15,12 +15,14 @@ import argparse import mxnet as mx +from time import perf_counter import cv2 from math import dist import rclpy from rclpy.node import Node +from std_msgs.msg import Float32 from sensor_msgs.msg import Image as ROS_Image from vision_msgs.msg import Detection2D from opendr_bridge import ROS2Bridge @@ -35,7 +37,7 @@ class ObjectTrackingSiamRPNNode(Node): def __init__(self, object_detector, input_rgb_image_topic="/image_raw", output_rgb_image_topic="/opendr/image_tracking_annotated", - tracker_topic="/opendr/tracked_object", + tracker_topic="/opendr/tracked_object", performance_topic=None, device="cuda"): """ Creates a ROS2 Node for object tracking with SiamRPN. @@ -48,6 +50,9 @@ def __init__(self, object_detector, input_rgb_image_topic="/image_raw", :type output_rgb_image_topic: str :param tracker_topic: Topic to which we are publishing the annotation :type tracker_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str """ @@ -65,6 +70,11 @@ def __init__(self, object_detector, input_rgb_image_topic="/image_raw", else: self.object_publisher = None + if performance_topic is not None: + self.performance_publisher = self.create_publisher(Float32, performance_topic, 1) + else: + self.performance_publisher = None + self.bridge = ROS2Bridge() self.object_detector = object_detector @@ -81,6 +91,8 @@ def callback(self, data): :param data: input message :type data: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image image = self.bridge.from_ros_image(data, encoding='bgr8') self.image = image @@ -106,6 +118,14 @@ def callback(self, data): id=0, score=center_box.confidence) self.tracker.infer(self.image, init_box) + + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + self.initialized = True self.get_logger().info("Object Tracking 2D SiamRPN node initialized with the most central bounding box.") @@ -113,6 +133,13 @@ def callback(self, data): # Run object tracking box = self.tracker.infer(image) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + if self.object_publisher is not None: # Publish detections in ROS message ros_boxes = self.bridge.to_ros_single_tracking_annotation(box) @@ -140,6 +167,8 @@ def main(args=None): parser.add_argument("-t", "--tracker_topic", help="Topic name for tracker messages", type=lambda value: value if value.lower() != "none" else None, default="/opendr/tracked_object") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) 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() @@ -164,7 +193,8 @@ def main(args=None): object_tracker_2d_siamrpn_node = ObjectTrackingSiamRPNNode(object_detector=object_detector, device=device, input_rgb_image_topic=args.input_rgb_image_topic, output_rgb_image_topic=args.output_rgb_image_topic, - tracker_topic=args.tracker_topic) + tracker_topic=args.tracker_topic, + performance_topic=args.performance_topic) rclpy.spin(object_tracker_2d_siamrpn_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 d0f32545d3..1e3dd1fe22 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,10 +16,11 @@ import torch import argparse import os +from time import perf_counter import rclpy from rclpy.node import Node from vision_msgs.msg import Detection3DArray -from std_msgs.msg import Int32MultiArray +from std_msgs.msg import Int32MultiArray, Float32 from sensor_msgs.msg import PointCloud as ROS_PointCloud from opendr_bridge import ROS2Bridge from opendr.perception.object_tracking_3d import ObjectTracking3DAb3dmotLearner @@ -33,6 +34,7 @@ def __init__( input_point_cloud_topic="/opendr/dataset_point_cloud", output_detection3d_topic="/opendr/detection3d", output_tracking3d_id_topic="/opendr/tracking3d_id", + performance_topic=None, device="cuda:0", ): """ @@ -45,6 +47,9 @@ def __init__( :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 performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str """ @@ -68,6 +73,11 @@ def __init__( Int32MultiArray, output_tracking3d_id_topic, 1 ) + if performance_topic is not None: + self.performance_publisher = self.create_subscription(Float32, performance_topic, 1) + else: + self.performance_publisher = None + self.create_subscription(ROS_PointCloud, input_point_cloud_topic, self.callback, 1) self.get_logger().info("Object Tracking 3D Ab3dmot Node initialized.") @@ -78,19 +88,27 @@ def callback(self, data): :param data: input message :type data: sensor_msgs.msg.Image """ - + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image point_cloud = self.bridge.from_ros_point_cloud(data) detection_boxes = self.detector.infer(point_cloud) + if self.tracking_id_publisher or self.performance_publisher: + tracking_boxes = self.learner.infer(detection_boxes) + + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) - # Convert detected boxes to ROS type and publish if self.detection_publisher is not None: - ros_boxes = self.bridge.to_ros_boxes_3d(detection_boxes) - self.detection_publisher.publish(ros_boxes) + # Convert detected boxes to ROS type and publish + self.detection_publisher.publish(self.bridge.to_ros_boxes_3d(detection_boxes)) 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 @@ -111,6 +129,8 @@ def main(args=None): 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("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) 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", @@ -163,6 +183,7 @@ def main(args=None): input_point_cloud_topic=input_point_cloud_topic, output_detection3d_topic=output_detection3d_topic, output_tracking3d_id_topic=output_tracking3d_id_topic, + performance_topic=args.performance_topic ) rclpy.spin(ab3dmot_node) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/panoptic_segmentation_efficient_lps_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/panoptic_segmentation_efficient_lps_node.py index 06e9300f91..4492ca551e 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/panoptic_segmentation_efficient_lps_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/panoptic_segmentation_efficient_lps_node.py @@ -17,10 +17,12 @@ from pathlib import Path import argparse from typing import Optional +from time import perf_counter +import matplotlib import rclpy from rclpy.node import Node -import matplotlib +from std_msgs.msg import Float32 from sensor_msgs.msg import PointCloud2 as ROS_PointCloud2 from opendr_bridge import ROS2Bridge @@ -35,7 +37,8 @@ class EfficientLpsNode(Node): def __init__(self, input_pcl_topic: str, checkpoint: str, - output_heatmap_pointcloud_topic: Optional[str] = None + output_heatmap_pointcloud_topic: Optional[str] = None, + performance_topic=None, ): """ Initialize the EfficientLPS ROS node and create an instance of the respective learner class. @@ -45,6 +48,9 @@ def __init__(self, :type checkpoint: str :param output_heatmap_pointcloud_topic: topic for the output 3D heatmap point cloud :type output_heatmap_pointcloud_topic: Optional[str] + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str """ super().__init__('opendr_efficient_lps_node') @@ -52,13 +58,18 @@ def __init__(self, self.checkpoint = checkpoint self.output_heatmap_pointcloud_topic = output_heatmap_pointcloud_topic + if performance_topic is not None: + self.performance_publisher = self.create_publisher(Float32, performance_topic, 1) + else: + self.performance_publisher = None + # Initialize all ROS2 related things self._bridge = ROS2Bridge() self._visualization_publisher = None # Initialize the panoptic segmentation network config_file = Path(sys.modules[ - EfficientLpsLearner.__module__].__file__).parent / 'configs' / 'singlegpu_semantickitti.py' + EfficientLpsLearner.__module__].__file__).parent / 'configs' / 'singlegpu_semantickitti.py' self._learner = EfficientLpsLearner(str(config_file)) # Other @@ -125,12 +136,20 @@ def callback(self, data: ROS_PointCloud2): :param data: PointCloud2 data message :type data: sensor_msgs.msg.PointCloud2 """ - + if self.performance_publisher: + start_time = perf_counter() pointcloud = self._bridge.from_ros_point_cloud2(data) try: prediction = self._learner.infer(pointcloud) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + except Exception as e: self.get_logger().error('Failed to perform inference: {}'.format(e)) return @@ -161,11 +180,14 @@ def main(args=None): help='Download pretrained models [semantickitti] or load from the provided path') parser.add_argument('-o', '--output_heatmap_pointcloud_topic', type=str, default="/opendr/panoptic", help='Publish the output 3D heatmap point cloud on this topic') + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) args = parser.parse_args() efficient_lps_node = EfficientLpsNode(args.input_point_cloud_2_topic, args.checkpoint, - args.output_heatmap_pointcloud_topic) + args.output_heatmap_pointcloud_topic, + performance_topic=args.performance_topic) efficient_lps_node.listen() diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/panoptic_segmentation_efficient_ps_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/panoptic_segmentation_efficient_ps_node.py index a60017c432..962b0baa26 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/panoptic_segmentation_efficient_ps_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/panoptic_segmentation_efficient_ps_node.py @@ -17,10 +17,12 @@ from pathlib import Path import argparse from typing import Optional +from time import perf_counter +import matplotlib import rclpy from rclpy.node import Node -import matplotlib +from std_msgs.msg import Float32 from sensor_msgs.msg import Image as ROS_Image from opendr_bridge import ROS2Bridge @@ -36,7 +38,8 @@ def __init__(self, checkpoint: str, output_heatmap_topic: Optional[str] = None, output_rgb_visualization_topic: Optional[str] = None, - detailed_visualization: bool = False + detailed_visualization: bool = False, + performance_topic=None ): """ Initialize the EfficientPS ROS2 node and create an instance of the respective learner class. @@ -49,6 +52,9 @@ def __init__(self, :type output_heatmap_topic: str :param output_rgb_visualization_topic: ROS topic for the generated visualization of the panoptic map :type output_rgb_visualization_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param detailed_visualization: if True, generate a combined overview of the input RGB image and the semantic, instance, and panoptic segmentation maps and publish it on output_rgb_visualization_topic :type detailed_visualization: bool @@ -61,6 +67,11 @@ def __init__(self, self.output_rgb_visualization_topic = output_rgb_visualization_topic self.detailed_visualization = detailed_visualization + if performance_topic is not None: + self.performance_publisher = self.create_subscription(Float32, performance_topic, 1) + else: + self.performance_publisher = None + # Initialize all ROS2 related things self._bridge = ROS2Bridge() self._instance_heatmap_publisher = None @@ -142,6 +153,8 @@ def callback(self, data: ROS_Image): :param data: Input image message :type data: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image to OpenDR Image image = self._bridge.from_ros_image(data) @@ -149,6 +162,13 @@ def callback(self, data: ROS_Image): # Retrieve a list of two OpenDR heatmaps: [instance map, semantic map] prediction = self._learner.infer(image) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + # The output topics are only published if there is at least one subscriber if self._visualization_publisher is not None and self._visualization_publisher.get_subscription_count() > 0: panoptic_image = EfficientPsLearner.visualize(image, prediction, show_figure=False, @@ -179,6 +199,8 @@ def main(args=None): default='/opendr/panoptic/rgb_visualization', help='publish the panoptic segmentation map as an RGB image on this topic or a more detailed \ overview if using the --detailed_visualization flag') + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) parser.add_argument('--detailed_visualization', action='store_true', help='generate a combined overview of the input RGB image and the semantic, instance, and \ panoptic segmentation maps and publish it on OUTPUT_RGB_IMAGE_TOPIC') @@ -190,7 +212,8 @@ def main(args=None): args.checkpoint, args.output_heatmap_topic, args.output_rgb_image_topic, - args.detailed_visualization) + args.detailed_visualization, + performance_topic=args.performance_topic) efficient_ps_node.listen() diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/performance_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/performance_node.py new file mode 100644 index 0000000000..1be7ba1fbf --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/performance_node.py @@ -0,0 +1,80 @@ +#!/usr/bin/env python3 +# Copyright 2020-2023 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 +from numpy import mean + +import rclpy +from rclpy.node import Node +from std_msgs.msg import Float32 + + +class PerformanceNode(Node): + + def __init__(self, input_performance_topic="/opendr/performance", window_length=20): + """ + Creates a ROS2 Node for subscribing to a node's performance topic to measure and log its performance in frames + per second (FPS), rolling window. + :param input_performance_topic: Topic from which we are reading the performance message + :type input_performance_topic: str + :param window_length: The length of the rolling average window + :type window_length: int + """ + super().__init__('opendr_performance_node') + self.performance_subscriber = self.create_subscription(Float32, input_performance_topic, self.callback, 1) + + self.fps_window = [] + self.window_length = window_length + self.get_logger().info("Performance node initialized.") + + def callback(self, data): + """ + Callback that processes the input data and publishes to the corresponding topics. + :param data: Input image message + :type data: sensor_msgs.msg.Image + """ + fps = data.data + self.get_logger().info(f"Time per inference: {str(round(1.0 / fps, 4))} sec") + while len(self.fps_window) < self.window_length: + self.fps_window.append(fps) + self.fps_window = self.fps_window[1:] + self.fps_window.append(fps) + self.get_logger().info(f"Average inferences per second: {round(mean(self.fps_window), 2)}") # NOQA + + +def main(args=None): + rclpy.init(args=args) + + parser = argparse.ArgumentParser() + parser.add_argument("-i", "--input_performance_topic", help="Topic name for performance message", + type=str, default="/opendr/performance") + parser.add_argument("-w", "--window", help="The window to use in frames to calculate running average FPS", + type=int, default=20) + args = parser.parse_args() + + performance_node = PerformanceNode(input_performance_topic=args.input_performance_topic, + window_length=args.window) + + rclpy.spin(performance_node) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + performance_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/pose_estimation_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/pose_estimation_node.py index df18f1f1fa..90c9a70d6f 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/pose_estimation_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/pose_estimation_node.py @@ -15,10 +15,12 @@ import argparse import torch +from time import perf_counter import rclpy from rclpy.node import Node +from std_msgs.msg import Float32 from sensor_msgs.msg import Image as ROS_Image from opendr_bridge import ROS2Bridge from opendr_interface.msg import OpenDRPose2D @@ -31,7 +33,7 @@ class PoseEstimationNode(Node): def __init__(self, input_rgb_image_topic="image_raw", output_rgb_image_topic="/opendr/image_pose_annotated", - detections_topic="/opendr/poses", device="cuda", + detections_topic="/opendr/poses", performance_topic=None, device="cuda", num_refinement_stages=2, use_stride=False, half_precision=False): """ Creates a ROS2 Node for pose estimation with Lightweight OpenPose. @@ -43,6 +45,9 @@ def __init__(self, input_rgb_image_topic="image_raw", output_rgb_image_topic="/o :param detections_topic: Topic to which we are publishing the annotations (if None, no pose detection message is published) :type detections_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str :param num_refinement_stages: Specifies the number of pose estimation refinement stages are added on the @@ -70,6 +75,11 @@ def __init__(self, input_rgb_image_topic="image_raw", output_rgb_image_topic="/o else: self.pose_publisher = None + if performance_topic is not None: + self.performance_publisher = self.create_publisher(Float32, performance_topic, 1) + else: + self.performance_publisher = None + self.bridge = ROS2Bridge() self.pose_estimator = LightweightOpenPoseLearner(device=device, num_refinement_stages=num_refinement_stages, @@ -86,12 +96,21 @@ def callback(self, data): :param data: Input image message :type data: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image image = self.bridge.from_ros_image(data, encoding='bgr8') # Run pose estimation poses = self.pose_estimator.infer(image) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + # Publish detections in ROS message for pose in poses: if self.pose_publisher is not None: @@ -122,6 +141,8 @@ def main(args=None): "no detection message is published", type=lambda value: value if value.lower() != "none" else None, default="/opendr/poses") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) 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("--accelerate", help="Enables acceleration flags (e.g., stride)", default=False, @@ -154,6 +175,7 @@ def main(args=None): input_rgb_image_topic=args.input_rgb_image_topic, output_rgb_image_topic=args.output_rgb_image_topic, detections_topic=args.detections_topic, + performance_topic=args.performance_topic, num_refinement_stages=stages, use_stride=stride, half_precision=half_prec) rclpy.spin(pose_estimator_node) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/rgbd_hand_gesture_recognition_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/rgbd_hand_gesture_recognition_node.py index 48669b59ef..b069558044 100755 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/rgbd_hand_gesture_recognition_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/rgbd_hand_gesture_recognition_node.py @@ -19,10 +19,12 @@ import cv2 import numpy as np import torch +from time import perf_counter import rclpy from rclpy.node import Node import message_filters +from std_msgs.msg import Float32 from sensor_msgs.msg import Image as ROS_Image from vision_msgs.msg import Classification2D @@ -35,7 +37,8 @@ class RgbdHandGestureNode(Node): def __init__(self, input_rgb_image_topic="/kinect2/qhd/image_color_rect", input_depth_image_topic="/kinect2/qhd/image_depth_rect", - output_gestures_topic="/opendr/gestures", device="cuda", delay=0.1): + output_gestures_topic="/opendr/gestures", + performance_topic=None, device="cuda", delay=0.1): """ Creates a ROS2 Node for gesture recognition from RGBD. Assuming that the following drivers have been installed: https://github.com/OpenKinect/libfreenect2 and https://github.com/code-iai/iai_kinect2. @@ -45,6 +48,9 @@ def __init__(self, input_rgb_image_topic="/kinect2/qhd/image_color_rect", :type input_depth_image_topic: str :param output_gestures_topic: Topic to which we are publishing the predicted gesture class :type output_gestures_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: Device on which we are running inference ('cpu' or 'cuda') :type device: str :param delay: Define the delay (in seconds) with which rgb message and depth message can be synchronized @@ -54,6 +60,11 @@ def __init__(self, input_rgb_image_topic="/kinect2/qhd/image_color_rect", self.gesture_publisher = self.create_publisher(Classification2D, output_gestures_topic, 1) + if performance_topic is not None: + self.performance_publisher = self.create_publisher(Float32, performance_topic, 1) + else: + self.performance_publisher = None + image_sub = message_filters.Subscriber(self, ROS_Image, input_rgb_image_topic, qos_profile=1) depth_sub = message_filters.Subscriber(self, ROS_Image, input_depth_image_topic, qos_profile=1) # synchronize image and depth data topics @@ -83,7 +94,8 @@ def callback(self, rgb_data, depth_data): :param depth_data: input depth image message :type depth_data: sensor_msgs.msg.Image """ - + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image and preprocess rgb_image = self.bridge.from_ros_image(rgb_data, encoding='bgr8') depth_data.encoding = 'mono16' @@ -93,6 +105,13 @@ def callback(self, rgb_data, depth_data): # Run gesture recognition gesture_class = self.gesture_learner.infer(img) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + # Publish results ros_gesture = self.bridge.from_category_to_rosclass(gesture_class, self.get_clock().now().to_msg()) self.gesture_publisher.publish(ros_gesture) @@ -105,8 +124,8 @@ def preprocess(self, rgb_image, depth_image): :param depth_image: input depth image :type depth_image: engine.data.Image """ - rgb_image = rgb_image.convert(format='channels_last') / (2**8 - 1) - depth_image = depth_image.convert(format='channels_last') / (2**16 - 1) + rgb_image = rgb_image.convert(format='channels_last') / (2 ** 8 - 1) + depth_image = depth_image.convert(format='channels_last') / (2 ** 16 - 1) # resize the images to 224x224 rgb_image = cv2.resize(rgb_image, (224, 224)) @@ -131,10 +150,12 @@ def main(args=None): type=str, default="/kinect2/qhd/image_depth_rect") parser.add_argument("-o", "--output_gestures_topic", help="Topic name for predicted gesture class", type=str, default="/opendr/gestures") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) parser.add_argument("--device", help="Device to use (cpu, cuda)", type=str, default="cuda", choices=["cuda", "cpu"]) parser.add_argument("--delay", help="The delay (in seconds) with which RGB message and" - "depth message can be synchronized", type=float, default=0.1) + "depth message can be synchronized", type=float, default=0.1) args = parser.parse_args() @@ -154,6 +175,7 @@ def main(args=None): gesture_node = RgbdHandGestureNode(input_rgb_image_topic=args.input_rgb_image_topic, input_depth_image_topic=args.input_depth_image_topic, output_gestures_topic=args.output_gestures_topic, device=device, + performance_topic=args.performance_topic, delay=args.delay) rclpy.spin(gesture_node) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/semantic_segmentation_bisenet_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/semantic_segmentation_bisenet_node.py index 8aac4c17f8..508b055634 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/semantic_segmentation_bisenet_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/semantic_segmentation_bisenet_node.py @@ -18,10 +18,12 @@ import torch import cv2 import colorsys +from time import perf_counter import rclpy from rclpy.node import Node +from std_msgs.msg import Float32 from sensor_msgs.msg import Image as ROS_Image from opendr_bridge import ROS2Bridge @@ -33,7 +35,8 @@ class BisenetNode(Node): def __init__(self, input_rgb_image_topic="/image_raw", output_heatmap_topic="/opendr/heatmap", - output_rgb_image_topic="/opendr/heatmap_visualization", device="cuda"): + output_rgb_image_topic="/opendr/heatmap_visualization", performance_topic=None, + device="cuda"): """ Creates a ROS2 Node for semantic segmentation with Bisenet. :param input_rgb_image_topic: Topic from which we are reading the input image @@ -44,6 +47,9 @@ class ids :param output_rgb_image_topic: Topic to which we are publishing the heatmap image blended with the input image and a class legend for visualization purposes :type output_rgb_image_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str """ @@ -61,6 +67,11 @@ class ids else: self.visualization_publisher = None + if performance_topic is not None: + self.performance_publisher = self.create_publisher(Float32, performance_topic, 1) + else: + self.performance_publisher = None + self.bridge = ROS2Bridge() # Initialize the semantic segmentation model @@ -80,6 +91,8 @@ def callback(self, data): :param data: Input image message :type data: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image image = self.bridge.from_ros_image(data, encoding='bgr8') @@ -87,6 +100,13 @@ def callback(self, data): # Run semantic segmentation to retrieve the OpenDR heatmap heatmap = self.learner.infer(image) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + # Publish heatmap in the form of an image containing class ids if self.heatmap_publisher is not None: heatmap = Heatmap(heatmap.data.astype(np.uint8)) # Convert to uint8 @@ -162,6 +182,8 @@ def main(args=None): "visualization purposes", type=lambda value: value if value.lower() != "none" else None, default="/opendr/heatmap_visualization") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) 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() @@ -182,7 +204,8 @@ def main(args=None): bisenet_node = BisenetNode(device=device, input_rgb_image_topic=args.input_rgb_image_topic, output_heatmap_topic=args.output_heatmap_topic, - output_rgb_image_topic=args.output_rgb_image_topic) + output_rgb_image_topic=args.output_rgb_image_topic, + performance_topic=args.performance_topic) rclpy.spin(bisenet_node) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/skeleton_based_action_recognition_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/skeleton_based_action_recognition_node.py index 125f08d751..679dbdf4e8 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/skeleton_based_action_recognition_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/skeleton_based_action_recognition_node.py @@ -16,10 +16,11 @@ import argparse import torch import numpy as np +from time import perf_counter import rclpy from rclpy.node import Node -from std_msgs.msg import String +from std_msgs.msg import String, Float32 from vision_msgs.msg import ObjectHypothesis from sensor_msgs.msg import Image as ROS_Image from opendr_bridge import ROS2Bridge @@ -39,6 +40,7 @@ def __init__(self, input_rgb_image_topic="image_raw", pose_annotations_topic="/opendr/poses", output_category_topic="/opendr/skeleton_recognized_action", output_category_description_topic="/opendr/skeleton_recognized_action_description", + performance_topic=None, device="cuda", model='stgcn'): """ Creates a ROS2 Node for skeleton-based action recognition @@ -56,6 +58,9 @@ def __init__(self, input_rgb_image_topic="image_raw", :param output_category_description_topic: Topic to which we are publishing the description of the recognized action (if None, we are not publishing the description) :type output_category_description_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str :param model: model to use for skeleton-based action recognition. @@ -87,6 +92,11 @@ def __init__(self, input_rgb_image_topic="image_raw", else: self.string_publisher = None + if performance_topic is not None: + self.performance_publisher = self.create_publisher(Float32, performance_topic, 1) + else: + self.performance_publisher = None + self.bridge = ROS2Bridge() # Initialize the pose estimation @@ -108,10 +118,10 @@ def __init__(self, input_rgb_image_topic="image_raw", in_channels=2, num_point=18, graph_type='openpose') - model_saved_path = self.action_classifier.download(path="./pretrained_models/"+model, + model_saved_path = self.action_classifier.download(path="./pretrained_models/" + model, method_name=model, mode="pretrained", - file_name=model+'_ntu_cv_lw_openpose') - self.action_classifier.load(model_saved_path, model+'_ntu_cv_lw_openpose') + file_name=model + '_ntu_cv_lw_openpose') + self.action_classifier.load(model_saved_path, model + '_ntu_cv_lw_openpose') self.get_logger().info("Skeleton-based action recognition node initialized.") @@ -121,7 +131,8 @@ def callback(self, data): :param data: input message :type data: sensor_msgs.msg.Image """ - + if self.performance_publisher: + start_time = perf_counter() # Convert sensor_msgs.msg.Image into OpenDR Image image = self.bridge.from_ros_image(data, encoding='bgr8') @@ -131,21 +142,6 @@ def callback(self, data): # select two poses with highest energy poses = _select_2_poses(poses) - # Get an OpenCV image back - image = image.opencv() - # Annotate image and publish results - for pose in poses: - if self.pose_publisher is not None: - ros_pose = self.bridge.to_ros_pose(pose) - self.pose_publisher.publish(ros_pose) - # We get can the data back using self.bridge.from_ros_pose(ros_pose) - # e.g., opendr_pose = self.bridge.from_ros_pose(ros_pose) - draw(image, pose) - - if self.image_publisher is not None: - message = self.bridge.to_ros_image(Image(image), encoding='bgr8') - self.image_publisher.publish(message) - num_frames = 300 poses_list = [] for _ in range(num_frames): @@ -156,6 +152,27 @@ def callback(self, data): category = self.action_classifier.infer(skeleton_seq) category.confidence = float(category.confidence.max()) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + + if self.image_publisher is not None: + # Get an OpenCV image back + image = image.opencv() + # Annotate image and publish results + for pose in poses: + if self.pose_publisher is not None: + ros_pose = self.bridge.to_ros_pose(pose) + self.pose_publisher.publish(ros_pose) + # We get can the data back using self.bridge.from_ros_pose(ros_pose) + # e.g., opendr_pose = self.bridge.from_ros_pose(ros_pose) + draw(image, pose) + message = self.bridge.to_ros_image(Image(image), encoding='bgr8') + self.image_publisher.publish(message) + if self.hypothesis_publisher is not None: self.hypothesis_publisher.publish(self.bridge.to_ros_category(category)) @@ -207,6 +224,8 @@ def main(args=None): "recognized action category", type=lambda value: value if value.lower() != "none" else None, default="/opendr/skeleton_recognized_action_description") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) parser.add_argument("--device", help="Device to use, either \"cpu\" or \"cuda\"", type=str, default="cuda", choices=["cuda", "cpu"]) parser.add_argument("--model", help="Model to use, either \"stgcn\" or \"pstgcn\"", @@ -233,6 +252,7 @@ def main(args=None): pose_annotations_topic=args.pose_annotations_topic, output_category_topic=args.output_category_topic, output_category_description_topic=args.output_category_description_topic, + performance_topic=args.performance_topic, device=device, model=args.model) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/speech_command_recognition_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/speech_command_recognition_node.py index 60fc89879f..8d892cc96f 100755 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/speech_command_recognition_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/speech_command_recognition_node.py @@ -17,9 +17,11 @@ import argparse import torch import numpy as np +from time import perf_counter import rclpy from rclpy.node import Node +from std_msgs.msg import Float32 from audio_common_msgs.msg import AudioData from vision_msgs.msg import Classification2D @@ -31,13 +33,16 @@ class SpeechRecognitionNode(Node): def __init__(self, input_audio_topic="/audio", output_speech_command_topic="/opendr/speech_recognition", - buffer_size=1.5, model="matchboxnet", model_path=None, device="cuda"): + performance_topic=None, buffer_size=1.5, model="matchboxnet", model_path=None, device="cuda"): """ Creates a ROS2 Node for speech command recognition :param input_audio_topic: Topic from which the audio data is received :type input_audio_topic: str :param output_speech_command_topic: Topic to which the predictions are published :type output_speech_command_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param buffer_size: Length of the audio buffer in seconds :type buffer_size: float :param model: base speech command recognition model: matchboxnet or quad_selfonn @@ -52,6 +57,11 @@ def __init__(self, input_audio_topic="/audio", output_speech_command_topic="/ope self.create_subscription(AudioData, input_audio_topic, self.callback, 1) + if performance_topic is not None: + self.performance_publisher = self.create_subscription(Float32, performance_topic, 1) + else: + self.performance_publisher = None + self.bridge = ROS2Bridge() # Initialize the internal audio buffer @@ -84,16 +94,24 @@ def callback(self, msg_data): :param msg_data: incoming message :type msg_data: audio_common_msgs.msg.AudioData """ + if self.performance_publisher: + start_time = perf_counter() # Accumulate data until the buffer is full - data = np.reshape(np.frombuffer(msg_data.data, dtype=np.int16)/32768.0, (1, -1)) + data = np.reshape(np.frombuffer(msg_data.data, dtype=np.int16) / 32768.0, (1, -1)) self.data_buffer = np.append(self.data_buffer, data, axis=1) - - if self.data_buffer.shape[1] > 16000*self.buffer_size: + if self.data_buffer.shape[1] > 16000 * self.buffer_size: # Convert sample to OpenDR Timeseries and perform classification input_sample = Timeseries(self.data_buffer) class_pred = self.learner.infer(input_sample) + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + # Publish output ros_class = self.bridge.from_category_to_rosclass(class_pred, self.get_clock().now().to_msg()) self.publisher.publish(ros_class) @@ -110,6 +128,8 @@ def main(args=None): help="Listen to input data on this topic") parser.add_argument("-o", "--output_speech_command_topic", type=str, default="/opendr/speech_recognition", help="Topic name for speech command output") + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) parser.add_argument("--device", type=str, default="cuda", choices=["cuda", "cpu"], help="Device to use (cpu, cuda)") parser.add_argument("--buffer_size", type=float, default=1.5, help="Size of the audio buffer in seconds") @@ -134,6 +154,7 @@ def main(args=None): speech_node = SpeechRecognitionNode(input_audio_topic=args.input_audio_topic, output_speech_command_topic=args.output_speech_command_topic, + performance_topic=args.performance_topic, buffer_size=args.buffer_size, model=args.model, model_path=args.model_path, device=device) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/video_activity_recognition_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/video_activity_recognition_node.py index 67c43636ba..c0fc352c70 100644 --- a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/video_activity_recognition_node.py +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/video_activity_recognition_node.py @@ -17,11 +17,12 @@ import torch import torchvision import cv2 +from time import perf_counter import rclpy from rclpy.node import Node from pathlib import Path -from std_msgs.msg import String +from std_msgs.msg import String, Float32 from vision_msgs.msg import ObjectHypothesis from sensor_msgs.msg import Image as ROS_Image from opendr_bridge import ROS2Bridge @@ -34,12 +35,13 @@ class HumanActivityRecognitionNode(Node): def __init__( - self, - input_rgb_image_topic="image_raw", - output_category_topic="/opendr/human_activity_recognition", - output_category_description_topic="/opendr/human_activity_recognition_description", - device="cuda", - model="cox3d-m", + self, + input_rgb_image_topic="image_raw", + output_category_topic="/opendr/human_activity_recognition", + output_category_description_topic="/opendr/human_activity_recognition_description", + performance_topic=None, + device="cuda", + model="cox3d-m", ): """ Creates a ROS2 Node for video-based human activity recognition. @@ -51,6 +53,9 @@ def __init__( :param output_category_description_topic: Topic to which we are publishing the ID of the recognized action (if None, we are not publishing the ID) :type output_category_description_topic: str + :param performance_topic: Topic to which we are publishing performance information (if None, no performance + message is published) + :type performance_topic: str :param device: device on which we are running inference ('cpu' or 'cuda') :type device: str :param model: Architecture to use for human activity recognition. @@ -100,6 +105,12 @@ def __init__( if output_category_description_topic else None ) + + if performance_topic is not None: + self.performance_publisher = self.create_publisher(Float32, performance_topic, 1) + else: + self.performance_publisher = None + self.bridge = ROS2Bridge() self.get_logger().info("Video Human Activity Recognition node initialized.") @@ -109,6 +120,8 @@ def callback(self, data): :param data: input message :type data: sensor_msgs.msg.Image """ + if self.performance_publisher: + start_time = perf_counter() image = self.bridge.from_ros_image(data, encoding="rgb8") if image is None: return @@ -122,6 +135,13 @@ def callback(self, data): category.confidence = float(category.confidence.max()) category.description = KINETICS400_CLASSES[category.data] # Class name + if self.performance_publisher: + end_time = perf_counter() + fps = 1.0 / (end_time - start_time) # NOQA + fps_msg = Float32() + fps_msg.data = fps + self.performance_publisher.publish(fps_msg) + if self.hypothesis_publisher is not None: self.hypothesis_publisher.publish(self.bridge.to_ros_category(category)) @@ -210,7 +230,9 @@ def main(args=None): help="Topic to which we are publishing the ID of the recognized action", type=lambda value: value if value.lower() != "none" else None, default="/opendr/human_activity_recognition_description") - parser.add_argument("--device", help='Device to use, either "cpu" or "cuda", defaults to "cuda"', + parser.add_argument("--performance_topic", help="Topic name for performance messages, disabled (None) by default", + type=str, default=None) + 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("--model", help="Architecture to use for human activity recognition.", type=str, default="cox3d-m", @@ -234,6 +256,7 @@ def main(args=None): input_rgb_image_topic=args.input_rgb_image_topic, output_category_topic=args.output_category_topic, output_category_description_topic=args.output_category_description_topic, + performance_topic=args.performance_topic, device=device, model=args.model, ) diff --git a/projects/opendr_ws_2/src/opendr_perception/setup.py b/projects/opendr_ws_2/src/opendr_perception/setup.py index a2a21291ee..a2b634f500 100644 --- a/projects/opendr_ws_2/src/opendr_perception/setup.py +++ b/projects/opendr_ws_2/src/opendr_perception/setup.py @@ -55,6 +55,7 @@ 'binary_high_resolution = opendr_perception.binary_high_resolution_node:main', 'continual_skeleton_based_action_recognition = \ opendr_perception.continual_skeleton_based_action_recognition_node:main', + 'performance = opendr_perception.performance_node:main', ], }, )