From d9fa3264527f947721019ad762fdec1d3a37fec1 Mon Sep 17 00:00:00 2001 From: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> Date: Fri, 23 Dec 2022 21:53:12 +0200 Subject: [PATCH] ROS2 workspace and example nodes (#256) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * ROS2 workspace with example pose estimation node and initial ros2_bridge package * Add ROS2 object detection 2D, face detection and semantic segmentation nodes (#273) * Added to and from ros boxes bridge methods * Added object detection 2d ssd node according to ros1 node * Added object detection 2d centernet node according to ros1 node * Added to and from bounding box list bridge methods * Added object detection 2d detr node according to ros1 node * Fixed some issues with type conversions in bridge * Added object detection 2d yolov3 node according to ros1 node * Added face detection retinaface node according to ros1 node * Added retinaface ros2 node in setup.py * Added semantic segmenation bisenet ROS2 node according to ROS1 node * Added additional checks in learner download methods to stop redownloading * Improved ROS2 packages names and bridge import * Tester moved * Changed bridge import and fixed some nms stuff causing errors * Changed bridge import and made all queues 1 to avoid delays * Minor pep8 fix * Another minor pep8 fix * Added licence check skip for setup.pys * Added licence check skip for test.pys * Added appropriate docstring on existing bridge methods * Removed unused commented line * Finalized ROS2 pose estimation node with argparse * Minor formatting * Finalized ROS2 bisenet semantic segmentation node with argparse * Improved docstring * Minor comment addition * Finalized ROS2 face detection retinaface node with argparse * Minor improvements * Finalized ROS2 object detection 2d yolov3 node with argparse * Finalized ROS2 object detection 2d centernet node with argparse * Finalized ROS2 object detection 2d detr node with argparse * Finalized ROS2 object detection 2d ssd node with argparse * Fixed typo in package.xml description Co-authored-by: Stefania Pedrazzi Co-authored-by: Stefania Pedrazzi * Removed tester node * Add ROS2 face recognition and fall detection nodes (#279) * Fixed wrong argparse default values * Some reordering for internal consistency and to/from face/face_id * Initial version of ROS2 face recognition node * Added annotated image publishing to ROS2 face recognition node * Fixed face recognition node class name * Fixed face recognition node class name in main * Added ROS2 fall detection node * Detr node now properly uses torch * Fixed missing condition in publishing face reco image * Updated ROS2 semseg bisenet node according to new ROS1 node * Added lambda expression in argparse to handle passing of none value * Minor optimization * Added new ros2 cmake package for OpenDR custom messages * Pose estimation ROS2 node now uses custom pose messages * Bridge to/from pose methods now use new opendr pose message * Moved opendr messages package to correct subdirectory (src) * Face det: Added lambda expression in argparse to handle passing of none value and minor optimization * Face reco: Added lambda expression in argparse to handle passing of none value * Fall det: Added lambda expression in argparse to handle passing of none value and minor fixes in callback * Obj det 2d: Added lambda expression in argparse to handle passing of none value and minor optimization * Sem segm bisenet: Added lambda expression in argparse to handle passing of none value and reintroduced try/except * Fall det: message now gets published only when there's a fallen pose present and each pose has its own id. * Ros2 detr (#296) * use same drawing function as other 2d object detectors * create object_detection_2d_detr ros2 node * Update projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_detr_node.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_detr_node.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_detr_node.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_detr_node.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_detr_node.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_detr_node.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * format code Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Ros2 har (#323) * Add ros2 video_activity_recognition_node * Fix resizing inference issues * Added missing bridge methods * Added missing node entry point declaration in setup.py * Fixed docstring and wrong default input topic Co-authored-by: tsampazk * Initial version of main ROS2 README.md * Updated contents based on newer ROS1 version * Added initial ROS2 main READMEs * Changed usb cam run command * Work in progress adding setup instructions * Complete setup and build instructions plus fixed links * Minor fixes in introduction and line separators * Commented out cv bridge installation * ROS2 RGBD hand gestures recognition (#341) * Add RGBD hand recogniton node * Update RGB hand gesture recognition node * Remove redundancy parameters * Update projects/opendr_ws_2/src/opendr_perception/opendr_perception/rgbd_hand_gesture_recognition_node.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * ROS2 implementation for human_model_generation module (#291) * changes for ros2 in opendr_simulation code * Update mesh_util.py * Update human_model_generation_service.py * Update human_model_generation_client.py * Update bridge.py * Update human_model_generation_service.py * Update human_model_generation_client.py * Update bridge.py * Update human_model_generation_client.py * Update human_model_generation_service.py * Update bridge.py * Update setup.py * Update human_model_generation_client.py * Update human_model_generation_service.py * Update test_flake8.py * Update test_flake8.py * Update test_license.py * Update setup.py * Update test_license.py * Update projects/opendr_ws_2/src/opendr_ros2_bridge/opendr_ros2_bridge/bridge.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/opendr_interfaces/package.xml Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/opendr_simulation/opendr_simulation/human_model_generation_service.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/opendr_simulation/opendr_simulation/human_model_generation_client.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/opendr_ros2_bridge/opendr_ros2_bridge/bridge.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/opendr_simulation/opendr_simulation/human_model_generation_client.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/opendr_simulation/opendr_simulation/human_model_generation_service.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/opendr_interfaces/package.xml Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/opendr_interfaces/srv/Mesh.srv Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/opendr_simulation/opendr_simulation/human_model_generation_client.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/opendr_simulation/opendr_simulation/human_model_generation_client.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/opendr_ros2_bridge/opendr_ros2_bridge/bridge.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/opendr_simulation/opendr_simulation/human_model_generation_service.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/opendr_simulation/opendr_simulation/human_model_generation_service.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * changes to ros2 * changes to ros2 * changes to ROS2 nodes * changes to ROS2 nodes * ROS2 changes * ROS2 changes * ROS2 changes * ROS2 changes * ROS2 changes * ROS2 changes * ROS2 changes * ROS2 changes * ROS2 changes * ROS2 changes * ROS2 changes * Update projects/opendr_ws_2/src/opendr_simulation/opendr_simulation/human_model_generation_service.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/opendr_simulation/opendr_simulation/human_model_generation_client.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update bridge.py * Update bridge.py * Update projects/opendr_ws_2/src/opendr_simulation/opendr_simulation/human_model_generation_client.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update dependencies.ini * Update dependencies.ini * Update mesh_util.py * Update dependencies.ini Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Add ROS2 node for EfficientPS (#270) * Update submodule * Add ROS2 node * Do not remove downloaded checkpoint file * Conclude merge * Fix PEP8 issue * ROS2 adaptation wrt. ROS imp. on dev. branch * fix: EfficientPS Learner updated, ROS2 subscriber queue fixed, ros2 bridge import fixed EfficientPS learner file is updated to the current changes in the develop branch. Queue parameter of the subscriber in the ROS2 script has adapted correctly, also implementation of the ROS2 bridge fixed. * refactor: Resolved conflict in the setup.py of ROS2 implementation * style: PEP8 fix * fix: arguments, ROS2Bridge import and rclpy.init fixed * style: PEP8 blank line fix * fix: Fixed encoder and default image topic location * fix: logging order and pep8 fixed * style: pep8 fix Co-authored-by: aselimc Co-authored-by: Ahmet Selim Çanakçı <73101853+aselimc@users.noreply.github.com> * ROS2 for heart anomaly detection (#337) * Implement ROS2 for heart anomaly detection * Remove redundant blank line * Organize import libraries * Change from ROS to ROS2 in docstring * Update heart_anomaly_detection_node.py Fix wrong initialized node name * Update bridge.py Update from single quotes to double quotes in docstring * Change node name for consistency naming across ROS2 nodes Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Fixed wrong name in dependency * ROS2 speech command recognition (#340) * Implement ROS2 for speech command recognition * Change default audio topic from /audio/audio to /audio * Update bridge.py Update from single quotes to double quotes in docstring * Fix style * Change node name for consistency across ROS2 nodes * Update speech_command_recognition_node.py Update blank lines in different positions * PEP8 removed whitespace Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> Co-authored-by: tsampazk * ROS2 for audiovisual emotion recognition node (#342) * Implement audiovisual emotion recognition node * Update projects/opendr_ws_2/src/opendr_perception/opendr_perception/audiovisual_emotion_recognition_node.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/opendr_perception/opendr_perception/audiovisual_emotion_recognition_node.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Skeleton_based_action_recognition ROS2 node (#344) * skeleton_har ros2 node added * ros2 node fixed * Add skeleton based action recognition in setup.py * Update projects/opendr_ws_2/src/opendr_perception/opendr_perception/skeleton_based_action_recognition_node.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/opendr_perception/opendr_perception/skeleton_based_action_recognition_node.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/opendr_perception/opendr_perception/skeleton_based_action_recognition_node.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/opendr_perception/opendr_perception/skeleton_based_action_recognition_node.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Fixed extra newline at end of line Co-authored-by: tsampazk Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * landmark_based_facial_expression_recognition ROS2 node (#345) * landmark_fer ros2 node added * ros2 node fixed * Add landmark based facial expression recognition in setup.py * Update projects/opendr_ws_2/src/opendr_perception/opendr_perception/landmark_based_facial_expression_recognition_node.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/opendr_perception/opendr_perception/landmark_based_facial_expression_recognition_node.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/opendr_perception/opendr_perception/landmark_based_facial_expression_recognition_node.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> Co-authored-by: tsampazk Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Reformatted file * Ros2 synthetic facial generation (#288) * opendr_ws_ros2 synthetic facial image generation * Delete projects/opendr_ws_2/src/ros2_bridge directory delete directory * delete unneccesary directory * Update README.md change to be correct the execution of the module * Update package.xml change to be correct * Update setup.py * Update package.xml * Update package.xml * prepared with test * Update synthetic_facial_generation.py * Update synthetic_facial_generation.py * Update synthetic_facial_generation.py * Update setup.py * Update test_pep257.py * Update test_flake8.py * Update test_copyright.py * Update mesh_util.py * Update Dockerfile-cuda Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update .github/workflows/tests_suite_develop.yml Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update .github/workflows/tests_suite_develop.yml Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/data_generation/package.xml Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update test_copyright.py * Update test_flake8.py * Update test_pep257.py * Update test_license.py * Update projects/opendr_ws_2/src/data_generation/setup.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/data_generation/setup.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/data_generation/data_generation/synthetic_facial_generation.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/data_generation/data_generation/synthetic_facial_generation.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/data_generation/data_generation/synthetic_facial_generation.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/data_generation/data_generation/synthetic_facial_generation.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/data_generation/data_generation/synthetic_facial_generation.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/data_generation/data_generation/synthetic_facial_generation.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/data_generation/data_generation/synthetic_facial_generation.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update package.xml * Update synthetic_facial_generation.py * Update .github/workflows/tests_suite_develop.yml Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update synthetic_facial_generation.py * Update synthetic_facial_generation.py * ros2 * ros2 * ros2 * Update README.md * Update synthetic_facial_generation.py * Update synthetic_facial_generation.py * Update synthetic_facial_generation.py * Update synthetic_facial_generation.py * Update test_copyright.py * Update test_flake8.py * Update test_pep257.py * Update test_license.py * Update test_license.py * Update Dockerfile-cuda Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/data_generation/data_generation/synthetic_facial_generation.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/data_generation/data_generation/synthetic_facial_generation.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/data_generation/data_generation/synthetic_facial_generation.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/data_generation/data_generation/synthetic_facial_generation.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/data_generation/data_generation/synthetic_facial_generation.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/data_generation/test/test_copyright.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/data_generation/test/test_flake8.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/data_generation/test/test_pep257.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update tests/test_license.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/data_generation/data_generation/synthetic_facial_generation.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/data_generation/data_generation/synthetic_facial_generation.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/data_generation/data_generation/synthetic_facial_generation.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/data_generation/data_generation/synthetic_facial_generation.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/data_generation/data_generation/synthetic_facial_generation.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/data_generation/data_generation/synthetic_facial_generation.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/data_generation/data_generation/synthetic_facial_generation.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/data_generation/data_generation/synthetic_facial_generation.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/data_generation/data_generation/synthetic_facial_generation.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update synthetic_facial_generation.py * Update synthetic_facial_generation.py * Update synthetic_facial_generation.py * new updates * updates * Update bridge.py * Update README.md * Update data.py * Update projects/opendr_ws_2/src/opendr_ros2_bridge/opendr_ros2_bridge/bridge.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/opendr_ros2_bridge/opendr_ros2_bridge/bridge.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/data_generation/data_generation/synthetic_facial_generation.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Ros2 gem (#295) * use same drawing function as other object_detection_2d nodes * Add gem ros2 node * Apply suggestions from code review Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Rename node class to ObjectDetectionGemNode * add argparse consistent with node args * make arguments consistent * tested gem ros and ros2 node * fix pep8 errors * apply changes requested by reviewer Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Renamed data generation package * Renamed opendr_ros2_messages to _interfaces * Fixed imports for opendr_ros2_interface * Minor fixes across all existing nodes - brought up to speed with latest ROS1 nodes * Added correct test_license skipped directories * Renamed bridge package to opendr_bridge * Fixed bridge dependency * Fixed bridge dependency and imports in perception and simulation packages * Fixed bridge dependency for simulation package * Renamed opendr_ros2_interface to opendr_interface * Fixed colcon build deprecation warnings * Fixed bridge import in init * Nanodet and yolov5 new ros2 nodes * Fix class names for yolo nodes * Convert to proper nanodet ros2 node * Minor comment fix on yolo nodes * Fixed node name in log info of nanodet node * Upgrade pip prior to install in docker * Update Dockerfile * Revert * Revert dependency change * Some future ros1 fixes for ros2 yolov5 node * Added new siamrpn node in setup.py * Added initialization service for siamrpn node * Initial SiamRPN ROS2 node * New bridge methods for tracking siamrpn * SiamRPN ROS2 node with built-in detector * Removed unused imports * Added missing description on siamrpn docstring * Fixed siamrpn node name * Minor fix in ros1 main readme * Updated ROS2 main readme, consistent with ROS1 * Minor fix in ros1 readme * Added ROS2 node diagram * Updated ROS2 node readme introductory sections and pose estimation * Updated whole ROS2 node readme for ROS2 specific stuff * Updated default usb cam ROS2 topic * Ros2 nodes for 3D Detection and 2D/3D tracking (#319) * Add ros2 point cloud dataset node * Fix point cloude dataset node args * Update default dataset path * Add voxel 3d detection node * Add output topic to args * Add tracking 3d node * Add tracking 2d fairmot node * Add deep sort ros2 node * Fix style errors * Fix C++ style error * Add device parsing * Move ros2 gitignore to global * Fix image dataset conditions * Fix docstrings * Fix pc dataset comments and conditions * Fix voxel 3d arguments and conditions * Fix ab3dmot conditions * Fix unused device var * Fix deep sort conditions * Fix fairmot conditions * Fix ab3dmot docstrings * Fix style errors * Apply suggestions from code review Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Fix parameter names * Fix deep sort inference * Fix deep sort no detections fail * Removed depend on rclcpp for data generation package * Fix bridge imports * Fix interface import * Minor fixes on voxel node based on ros1 voxel node * Future fix for embedded devices * Fixes from ros1 * Minor formatting * Matched arguments with ros1 * Fixed node name * Fixed docstring * Some autoformatting * Some fixes * Various fixes to match ros1 node * Fixes from ros1 * Autoformatting * Some fixes from ros1 * Some fixes from ros1 Co-authored-by: Illia Oleksiienko Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * ROS2 node for e2e planner and environment upgrade to Webots R2022b (#358) * ros1 planning package * end to end planner node * end to end planner ros2 node initiated * updated gym environment * planner updated with tests and environment * ROS1 node for e2e planner * Update readme and doc * Delete end_to_end_planner.py * Apply suggestions from code review Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Apply suggestions from code review * change the world to Webots 2022b initiated * apply suggestions from code review, update doc * Update projects/opendr_ws/src/planning/scripts/end_to_end_planner.py Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> * Update e2e_planning_learner.py * Update e2e_planning_learner.py * Update UAV_depth_planning_env.py * Update test_end_to_end_planning.py * Update euler_quaternion_transformations.py * Update obstacle_randomizer.py * Update sys_utils.py * Update dependencies.ini * changes for test learner independent from Webots * Webots world updated to R2022b * ROS1 node fix for 2022b version * ROS2 node for e2e planner * ROS2 node for e2e planner * cleanup * end-to-end planning ros2 node webots world launcher * disable ros-numpy package dependency * license test * cleanup * Apply suggestions from code review Co-authored-by: ad-daniel <44834743+ad-daniel@users.noreply.github.com> * Apply suggestions from code review * Apply suggestions from code review - fix for webots-ros2 apt installation. * fix driver name. * fix docs Webots version * Update src/opendr/planning/end_to_end_planning/__init__.py Co-authored-by: ad-daniel <44834743+ad-daniel@users.noreply.github.com> Co-authored-by: Kostas Tsampazis <27914645+tsampazk@users.noreply.github.com> Co-authored-by: ad-daniel Co-authored-by: ad-daniel <44834743+ad-daniel@users.noreply.github.com> * Added hr pose estimation entry in setup * Fixed warning * HR Pose Estimation ROS2 node * Added hr pose estimation node entry * Added hr pose estimation node entry in index * Added missing dot * Image-based facial emotion estimation ROS2 node (#346) * ros2 node added * pep8 style fixed * Added new facial emotion estimation in setup.py * Applied fixes from ros1 node * ROS2 update * Fixes for ROS2 facial emotion node * Minor fix for float confidence casting in category * Fixed setup.py entry for facial emotion estimation ROS2 node Co-authored-by: tsampazk <27914645+tsampazk@users.noreply.github.com> * test different imports for imports that break tests * Revert old imports on learner * Remove tools that get imported by learner with full path * Added init files in facial emotion estimation algorithm directories * Removed importing of stuff that are not used from outside the src of this tool * Added another missing init * Reintroduced wrongly removed import of datasets in init * Fix panoptic segmentation * Apply suggestions from code review Co-authored-by: ad-daniel <44834743+ad-daniel@users.noreply.github.com> * Fixed node names in run commands * Remove comment of additional installations * Removed runtime section * Removed additional skipped directory * Some fixes in the audio node docs * Fix audio_common_msgs for ROS2 * Added engine dependency in end_to_end_planning * Added missing packages in packages.txt and rearranged them * Revert "Fix audio_common_msgs for ROS2" This reverts commit dae3cde12e1414a4c506fa37611f05b38086c9b9. * Undo audio fix * Fixed planning bridge dependency name * Fixed default audio argument for ros 2 * Added audio prerequisites information for default audio ros package * Possible fix for single object tracking * Minor docs typo * Added note on landmark based facial reco about lack of pretrained model * Remove logging of image shape Co-authored-by: ad-daniel <44834743+ad-daniel@users.noreply.github.com> Co-authored-by: Stefania Pedrazzi Co-authored-by: Jelle <43064291+jelledouwe@users.noreply.github.com> Co-authored-by: Nikolaos Passalis Co-authored-by: Lukas Hedegaard Co-authored-by: Quoc Nguyen <53263073+minhquoc0712@users.noreply.github.com> Co-authored-by: charsyme <63857415+charsyme@users.noreply.github.com> Co-authored-by: Niclas <49001036+vniclas@users.noreply.github.com> Co-authored-by: aselimc Co-authored-by: Ahmet Selim Çanakçı <73101853+aselimc@users.noreply.github.com> Co-authored-by: Negar Heidari <36771997+negarhdr@users.noreply.github.com> Co-authored-by: ekakalet <63847549+ekakalet@users.noreply.github.com> Co-authored-by: ad-daniel Co-authored-by: Illia Oleksiienko Co-authored-by: Illia Oleksiienko Co-authored-by: halil93ibrahim --- .gitignore | 6 + bin/install.sh | 9 +- docs/reference/end-to-end-planning.md | 2 +- docs/reference/ros2bridge.md | 397 +++++ packages.txt | 6 +- projects/opendr_ws/README.md | 4 +- .../opendr_ws/src/opendr_perception/README.md | 14 +- .../scripts/end_to_end_planner_node.py | 37 +- projects/opendr_ws_2/README.md | 88 + .../images/opendr_node_diagram.png | Bin 0 -> 51820 bytes .../opendr_bridge/opendr_bridge/__init__.py | 3 + .../src/opendr_bridge/opendr_bridge/bridge.py | 629 +++++++ .../opendr_ws_2/src/opendr_bridge/package.xml | 21 + .../src/opendr_bridge/resource/opendr_bridge | 0 .../opendr_ws_2/src/opendr_bridge/setup.cfg | 4 + .../opendr_ws_2/src/opendr_bridge/setup.py | 26 + .../src/opendr_bridge/test/test_copyright.py | 23 + .../src/opendr_bridge/test/test_flake8.py | 25 + .../src/opendr_bridge/test/test_pep257.py | 23 + .../opendr_data_generation/__init__.py | 0 .../synthetic_facial_generation_node.py | 173 ++ .../src/opendr_data_generation/package.xml | 25 + .../resource/opendr_data_generation | 0 .../src/opendr_data_generation/setup.cfg | 4 + .../src/opendr_data_generation/setup.py | 40 + .../test/test_copyright.py | 23 + .../test/test_flake8.py | 25 + .../test/test_pep257.py | 23 + .../src/opendr_interface/CMakeLists.txt | 50 + .../include/opendr_interface/.keep | 0 .../src/opendr_interface/msg/OpenDRPose2D.msg | 26 + .../msg/OpenDRPose2DKeypoint.msg | 22 + .../src/opendr_interface/msg/OpenDRPose3D.msg | 26 + .../msg/OpenDRPose3DKeypoint.msg | 22 + .../src/opendr_interface/package.xml | 24 + .../src/opendr_interface/src/.keep | 0 .../src/opendr_interface/srv/ImgToMesh.srv | 21 + .../srv/OpenDRSingleObjectTracking.srv | 17 + .../src/opendr_perception/README.md | 885 ++++++++++ .../opendr_perception/__init__.py | 0 .../audiovisual_emotion_recognition_node.py | 167 ++ .../face_detection_retinaface_node.py | 148 ++ .../face_recognition_node.py | 193 +++ .../facial_emotion_estimation_node.py | 218 +++ .../opendr_perception/fall_detection_node.py | 189 +++ .../heart_anomaly_detection_node.py | 123 ++ .../hr_pose_estimation_node.py | 173 ++ .../opendr_perception/image_dataset_node.py | 113 ++ ...ased_facial_expression_recognition_node.py | 184 ++ .../object_detection_2d_centernet_node.py | 143 ++ .../object_detection_2d_detr_node.py | 242 +++ .../object_detection_2d_gem_node.py | 282 ++++ .../object_detection_2d_nanodet_node.py | 144 ++ .../object_detection_2d_ssd_node.py | 170 ++ .../object_detection_2d_yolov3_node.py | 143 ++ .../object_detection_2d_yolov5_node.py | 142 ++ .../object_detection_3d_voxel_node.py | 151 ++ .../object_tracking_2d_deep_sort_node.py | 247 +++ .../object_tracking_2d_fair_mot_node.py | 231 +++ .../object_tracking_2d_siamrpn_node.py | 179 ++ .../object_tracking_3d_ab3dmot_node.py | 177 ++ ...panoptic_segmentation_efficient_ps_node.py | 198 +++ .../point_cloud_dataset_node.py | 110 ++ .../opendr_perception/pose_estimation_node.py | 169 ++ .../rgbd_hand_gesture_recognition_node.py | 166 ++ .../semantic_segmentation_bisenet_node.py | 197 +++ .../skeleton_based_action_recognition_node.py | 249 +++ .../speech_command_recognition_node.py | 147 ++ .../video_activity_recognition_node.py | 250 +++ .../src/opendr_perception/package.xml | 26 + .../resource/opendr_perception | 0 .../src/opendr_perception/setup.cfg | 6 + .../src/opendr_perception/setup.py | 55 + .../opendr_perception/test/test_copyright.py | 23 + .../src/opendr_perception/test/test_flake8.py | 25 + .../src/opendr_perception/test/test_pep257.py | 23 + .../end_to_end_planning_robot_launch.py | 56 + .../opendr_planning/__init__.py | 0 .../end_to_end_planner_node.py | 98 ++ .../end_to_end_planning_robot_driver.py | 25 + .../src/opendr_planning/package.xml | 28 + .../src/opendr_planning/protos/box.proto | 88 + .../opendr_planning/resource/opendr_planning | 0 .../opendr_planning/resource/uav_robot.urdf | 34 + .../opendr_ws_2/src/opendr_planning/setup.cfg | 6 + .../opendr_ws_2/src/opendr_planning/setup.py | 31 + .../opendr_planning/test/test_copyright.py | 23 + .../src/opendr_planning/test/test_flake8.py | 25 + .../src/opendr_planning/test/test_pep257.py | 23 + .../train-no-dynamic-random-obstacles.wbt | 503 ++++++ .../opendr_simulation/__init__.py | 0 .../human_model_generation_client.py | 108 ++ .../human_model_generation_service.py | 109 ++ .../src/opendr_simulation/package.xml | 36 + .../resource/opendr_simulation | 0 .../src/opendr_simulation/setup.cfg | 6 + .../src/opendr_simulation/setup.py | 27 + .../opendr_simulation/test/test_copyright.py | 23 + .../src/opendr_simulation/test/test_flake8.py | 25 + .../src/opendr_simulation/test/test_pep257.py | 23 + .../SyntheticDataGeneration.py | 6 +- .../models/networks/__init__.py | 16 +- .../Rotate_and_Render/options/base_options.py | 4 +- .../Rotate_and_Render/test_multipose.py | 2 +- .../facial_expression_recognition/__init__.py | 20 +- .../algorithm/__init__.py | 0 .../algorithm/model/__init__.py | 0 .../algorithm/utils/__init__.py | 0 .../centernet/centernet_learner.py | 26 +- .../object_detection_2d/ssd/ssd_learner.py | 1489 +++++++++-------- .../second_detector/core/cc/nms/nms_cpu.h | 2 + .../deep_sort/algorithm/deep_sort_tracker.py | 4 +- .../planning/end_to_end_planning/README.md | 2 +- .../planning/end_to_end_planning/__init__.py | 4 +- .../end_to_end_planning/dependencies.ini | 8 +- .../envs/UAV_depth_planning_env.py | 65 +- .../envs/webots/protos/box.proto | 15 +- .../train-no-dynamic-random-obstacles.wbt | 134 +- .../utils/obstacle_randomizer.py | 44 +- .../human_model_generation/dependencies.ini | 2 +- .../utilities/PIFu/lib/mesh_util.py | 2 +- tests/test_license.py | 17 +- tests/test_pep8.py | 1 + 123 files changed, 10339 insertions(+), 947 deletions(-) create mode 100755 docs/reference/ros2bridge.md create mode 100755 projects/opendr_ws_2/README.md create mode 100644 projects/opendr_ws_2/images/opendr_node_diagram.png create mode 100644 projects/opendr_ws_2/src/opendr_bridge/opendr_bridge/__init__.py create mode 100644 projects/opendr_ws_2/src/opendr_bridge/opendr_bridge/bridge.py create mode 100644 projects/opendr_ws_2/src/opendr_bridge/package.xml create mode 100644 projects/opendr_ws_2/src/opendr_bridge/resource/opendr_bridge create mode 100644 projects/opendr_ws_2/src/opendr_bridge/setup.cfg create mode 100644 projects/opendr_ws_2/src/opendr_bridge/setup.py create mode 100644 projects/opendr_ws_2/src/opendr_bridge/test/test_copyright.py create mode 100644 projects/opendr_ws_2/src/opendr_bridge/test/test_flake8.py create mode 100644 projects/opendr_ws_2/src/opendr_bridge/test/test_pep257.py create mode 100644 projects/opendr_ws_2/src/opendr_data_generation/opendr_data_generation/__init__.py create mode 100644 projects/opendr_ws_2/src/opendr_data_generation/opendr_data_generation/synthetic_facial_generation_node.py create mode 100644 projects/opendr_ws_2/src/opendr_data_generation/package.xml create mode 100644 projects/opendr_ws_2/src/opendr_data_generation/resource/opendr_data_generation create mode 100644 projects/opendr_ws_2/src/opendr_data_generation/setup.cfg create mode 100644 projects/opendr_ws_2/src/opendr_data_generation/setup.py create mode 100644 projects/opendr_ws_2/src/opendr_data_generation/test/test_copyright.py create mode 100644 projects/opendr_ws_2/src/opendr_data_generation/test/test_flake8.py create mode 100644 projects/opendr_ws_2/src/opendr_data_generation/test/test_pep257.py create mode 100644 projects/opendr_ws_2/src/opendr_interface/CMakeLists.txt create mode 100644 projects/opendr_ws_2/src/opendr_interface/include/opendr_interface/.keep create mode 100644 projects/opendr_ws_2/src/opendr_interface/msg/OpenDRPose2D.msg create mode 100644 projects/opendr_ws_2/src/opendr_interface/msg/OpenDRPose2DKeypoint.msg create mode 100644 projects/opendr_ws_2/src/opendr_interface/msg/OpenDRPose3D.msg create mode 100644 projects/opendr_ws_2/src/opendr_interface/msg/OpenDRPose3DKeypoint.msg create mode 100644 projects/opendr_ws_2/src/opendr_interface/package.xml create mode 100644 projects/opendr_ws_2/src/opendr_interface/src/.keep create mode 100644 projects/opendr_ws_2/src/opendr_interface/srv/ImgToMesh.srv create mode 100644 projects/opendr_ws_2/src/opendr_interface/srv/OpenDRSingleObjectTracking.srv create mode 100755 projects/opendr_ws_2/src/opendr_perception/README.md create mode 100644 projects/opendr_ws_2/src/opendr_perception/opendr_perception/__init__.py create mode 100644 projects/opendr_ws_2/src/opendr_perception/opendr_perception/audiovisual_emotion_recognition_node.py create mode 100644 projects/opendr_ws_2/src/opendr_perception/opendr_perception/face_detection_retinaface_node.py create mode 100644 projects/opendr_ws_2/src/opendr_perception/opendr_perception/face_recognition_node.py create mode 100644 projects/opendr_ws_2/src/opendr_perception/opendr_perception/facial_emotion_estimation_node.py create mode 100644 projects/opendr_ws_2/src/opendr_perception/opendr_perception/fall_detection_node.py create mode 100644 projects/opendr_ws_2/src/opendr_perception/opendr_perception/heart_anomaly_detection_node.py create mode 100644 projects/opendr_ws_2/src/opendr_perception/opendr_perception/hr_pose_estimation_node.py create mode 100644 projects/opendr_ws_2/src/opendr_perception/opendr_perception/image_dataset_node.py create mode 100644 projects/opendr_ws_2/src/opendr_perception/opendr_perception/landmark_based_facial_expression_recognition_node.py create mode 100644 projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_centernet_node.py create mode 100644 projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_detr_node.py create mode 100644 projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_gem_node.py create mode 100755 projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_nanodet_node.py create mode 100644 projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_ssd_node.py create mode 100644 projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_yolov3_node.py create mode 100644 projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_yolov5_node.py create mode 100644 projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py create mode 100644 projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py create mode 100755 projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py create mode 100644 projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_siamrpn_node.py create mode 100644 projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py create mode 100644 projects/opendr_ws_2/src/opendr_perception/opendr_perception/panoptic_segmentation_efficient_ps_node.py create mode 100644 projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py create mode 100644 projects/opendr_ws_2/src/opendr_perception/opendr_perception/pose_estimation_node.py create mode 100755 projects/opendr_ws_2/src/opendr_perception/opendr_perception/rgbd_hand_gesture_recognition_node.py create mode 100644 projects/opendr_ws_2/src/opendr_perception/opendr_perception/semantic_segmentation_bisenet_node.py create mode 100644 projects/opendr_ws_2/src/opendr_perception/opendr_perception/skeleton_based_action_recognition_node.py create mode 100755 projects/opendr_ws_2/src/opendr_perception/opendr_perception/speech_command_recognition_node.py create mode 100644 projects/opendr_ws_2/src/opendr_perception/opendr_perception/video_activity_recognition_node.py create mode 100644 projects/opendr_ws_2/src/opendr_perception/package.xml create mode 100644 projects/opendr_ws_2/src/opendr_perception/resource/opendr_perception create mode 100644 projects/opendr_ws_2/src/opendr_perception/setup.cfg create mode 100644 projects/opendr_ws_2/src/opendr_perception/setup.py create mode 100644 projects/opendr_ws_2/src/opendr_perception/test/test_copyright.py create mode 100644 projects/opendr_ws_2/src/opendr_perception/test/test_flake8.py create mode 100644 projects/opendr_ws_2/src/opendr_perception/test/test_pep257.py create mode 100644 projects/opendr_ws_2/src/opendr_planning/launch/end_to_end_planning_robot_launch.py create mode 100644 projects/opendr_ws_2/src/opendr_planning/opendr_planning/__init__.py create mode 100755 projects/opendr_ws_2/src/opendr_planning/opendr_planning/end_to_end_planner_node.py create mode 100644 projects/opendr_ws_2/src/opendr_planning/opendr_planning/end_to_end_planning_robot_driver.py create mode 100644 projects/opendr_ws_2/src/opendr_planning/package.xml create mode 100644 projects/opendr_ws_2/src/opendr_planning/protos/box.proto create mode 100644 projects/opendr_ws_2/src/opendr_planning/resource/opendr_planning create mode 100644 projects/opendr_ws_2/src/opendr_planning/resource/uav_robot.urdf create mode 100644 projects/opendr_ws_2/src/opendr_planning/setup.cfg create mode 100644 projects/opendr_ws_2/src/opendr_planning/setup.py create mode 100644 projects/opendr_ws_2/src/opendr_planning/test/test_copyright.py create mode 100644 projects/opendr_ws_2/src/opendr_planning/test/test_flake8.py create mode 100644 projects/opendr_ws_2/src/opendr_planning/test/test_pep257.py create mode 100644 projects/opendr_ws_2/src/opendr_planning/worlds/train-no-dynamic-random-obstacles.wbt create mode 100644 projects/opendr_ws_2/src/opendr_simulation/opendr_simulation/__init__.py create mode 100644 projects/opendr_ws_2/src/opendr_simulation/opendr_simulation/human_model_generation_client.py create mode 100644 projects/opendr_ws_2/src/opendr_simulation/opendr_simulation/human_model_generation_service.py create mode 100644 projects/opendr_ws_2/src/opendr_simulation/package.xml create mode 100644 projects/opendr_ws_2/src/opendr_simulation/resource/opendr_simulation create mode 100644 projects/opendr_ws_2/src/opendr_simulation/setup.cfg create mode 100644 projects/opendr_ws_2/src/opendr_simulation/setup.py create mode 100644 projects/opendr_ws_2/src/opendr_simulation/test/test_copyright.py create mode 100644 projects/opendr_ws_2/src/opendr_simulation/test/test_flake8.py create mode 100644 projects/opendr_ws_2/src/opendr_simulation/test/test_pep257.py create mode 100644 src/opendr/perception/facial_expression_recognition/image_based_facial_emotion_estimation/algorithm/__init__.py create mode 100644 src/opendr/perception/facial_expression_recognition/image_based_facial_emotion_estimation/algorithm/model/__init__.py create mode 100644 src/opendr/perception/facial_expression_recognition/image_based_facial_emotion_estimation/algorithm/utils/__init__.py mode change 100755 => 100644 tests/test_license.py diff --git a/.gitignore b/.gitignore index 58ae399224..e20f95bb75 100644 --- a/.gitignore +++ b/.gitignore @@ -71,3 +71,9 @@ temp/ projects/opendr_ws/.catkin_workspace projects/opendr_ws/devel/ projects/python/control/eagerx/eagerx_ws/ + +projects/opendr_ws_2/build +projects/opendr_ws_2/install +projects/opendr_ws_2/log +projects/opendr_ws_2/MOT +projects/opendr_ws_2/KITTI diff --git a/bin/install.sh b/bin/install.sh index 77c624c93f..f3818ced2d 100755 --- a/bin/install.sh +++ b/bin/install.sh @@ -42,13 +42,18 @@ make install_runtime_dependencies # Install additional ROS packages if [[ ${ROS_DISTRO} == "noetic" || ${ROS_DISTRO} == "melodic" ]]; then echo "Installing ROS dependencies" - sudo apt-get -y install ros-$ROS_DISTRO-vision-msgs ros-$ROS_DISTRO-geometry-msgs ros-$ROS_DISTRO-sensor-msgs ros-$ROS_DISTRO-audio-common-msgs ros-$ROS_DISTRO-usb-cam + sudo apt-get -y install ros-$ROS_DISTRO-vision-msgs ros-$ROS_DISTRO-geometry-msgs ros-$ROS_DISTRO-sensor-msgs ros-$ROS_DISTRO-audio-common-msgs ros-$ROS_DISTRO-usb-cam ros-$ROS_DISTRO-webots-ros fi # Install additional ROS2 packages if [[ ${ROS_DISTRO} == "foxy" || ${ROS_DISTRO} == "humble" ]]; then echo "Installing ROS2 dependencies" - sudo apt-get -y install ros-$ROS_DISTRO-usb-cam + sudo apt-get -y install ros-$ROS_DISTRO-usb-cam ros-$ROS_DISTRO-webots-ros2 python3-colcon-common-extensions ros-$ROS_DISTRO-vision-msgs + LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/ros/$ROS_DISTRO/lib/controller + cd $OPENDR_HOME/projects/opendr_ws_2/ + git clone --depth 1 --branch ros2 https://github.com/ros-drivers/audio_common src/audio_common + rosdep install -i --from-path src/audio_common --rosdistro $ROS_DISTRO -y + cd $OPENDR_HOME fi # If working on GPU install GPU dependencies as needed diff --git a/docs/reference/end-to-end-planning.md b/docs/reference/end-to-end-planning.md index 89d007d7c6..748ecdde91 100644 --- a/docs/reference/end-to-end-planning.md +++ b/docs/reference/end-to-end-planning.md @@ -102,7 +102,7 @@ Parameters: ### Simulation environment setup The environment is provided with a [world](../../src/opendr/planning/end_to_end_planning/envs/webots/worlds/train-no-dynamic-random-obstacles.wbt) -that needs to be opened with Webots version 2021a in order to demonstrate the end-to-end planner. +that needs to be opened with Webots version 2022b in order to demonstrate the end-to-end planner. The environment includes an optional Ardupilot controlled quadrotor for simulating dynamics. For the installation of Ardupilot instructions are available [here](https://github.com/ArduPilot/ardupilot). diff --git a/docs/reference/ros2bridge.md b/docs/reference/ros2bridge.md new file mode 100755 index 0000000000..d0c155e4d7 --- /dev/null +++ b/docs/reference/ros2bridge.md @@ -0,0 +1,397 @@ +## ROSBridge Package + + +This *ROSBridge* package provides an interface to convert OpenDR data types and targets into ROS-compatible ones similar to CvBridge. +The *ROSBridge* class provides two methods for each data type X: +1. *from_ros_X()* : converts the ROS equivalent of X into OpenDR data type +2. *to_ros_X()* : converts the OpenDR data type into the ROS equivalent of X + +### Class ROSBridge + +The *ROSBridge* class provides an interface to convert OpenDR data types and targets into ROS-compatible ones. + +The ROSBridge class has the following public methods: + +#### `ROSBridge` constructor +The constructor only initializes the state of the class and does not require any input arguments. +```python +ROSBridge(self) +``` + +#### `ROSBridge.from_ros_image` + +```python +ROSBridge.from_ros_image(self, + message, + encoding) +``` + +This method converts a ROS Image into an OpenDR image. + +Parameters: + +- **message**: *sensor_msgs.msg.Img*\ + ROS image to be converted into an OpenDR image. +- **encoding**: *str, default='bgr8'*\ + Encoding to be used for the conversion (inherited from CvBridge). + +#### `ROSBridge.to_ros_image` + +```python +ROSBridge.to_ros_image(self, + image, + encoding) +``` + +This method converts an OpenDR image into a ROS image. + +Parameters: + +- **message**: *engine.data.Image*\ + OpenDR image to be converted into a ROS message. +- **encoding**: *str, default='bgr8'*\ + Encoding to be used for the conversion (inherited from CvBridge). + +#### `ROSBridge.from_ros_pose` + +```python +ROSBridge.from_ros_pose(self, + ros_pose) +``` + +Converts an OpenDRPose2D message into an OpenDR Pose. + +Parameters: + +- **ros_pose**: *ros_bridge.msg.OpenDRPose2D*\ + ROS pose to be converted into an OpenDR Pose. + +#### `ROSBridge.to_ros_pose` + +```python +ROSBridge.to_ros_pose(self, + pose) +``` +Converts an OpenDR Pose into a OpenDRPose2D msg that can carry the same information, i.e. a list of keypoints, +the pose detection confidence and the pose id. +Each keypoint is represented as an OpenDRPose2DKeypoint with x, y pixel position on input image with (0, 0) +being the top-left corner. + +Parameters: + +- **pose**: *engine.target.Pose*\ + OpenDR Pose to be converted to ROS OpenDRPose2D. + + +#### `ROSBridge.to_ros_category` + +```python +ROSBridge.to_ros_category(self, + category) +``` +Converts an OpenDR Category used for category recognition into a ROS ObjectHypothesis. + +Parameters: + +- **message**: *engine.target.Category*\ + OpenDR Category used for category recognition to be converted to ROS ObjectHypothesis. + +#### `ROSBridge.to_ros_category_description` + +```python +ROSBridge.to_ros_category_description(self, + category) +``` +Converts an OpenDR Category into a ROS String. + +Parameters: + +- **message**: *engine.target.Category*\ + OpenDR Category to be converted to ROS String. + + +#### `ROSBridge.from_ros_category` + +```python +ROSBridge.from_ros_category(self, + ros_hypothesis) +``` + +Converts a ROS ObjectHypothesis message into an OpenDR Category. + +Parameters: + +- **message**: *ros_bridge.msg.ObjectHypothesis*\ + ROS ObjectHypothesis to be converted into an OpenDR Category. + + +#### `ROSBridge.from_ros_face` + +```python +ROSBridge.from_ros_face(self, + ros_hypothesis) +``` + +Converts a ROS ObjectHypothesis message into an OpenDR Category. + +Parameters: + +- **message**: *ros_bridge.msg.ObjectHypothesis*\ + ROS ObjectHypothesis to be converted into an OpenDR Category. + +#### `ROSBridge.to_ros_face` + +```python +ROSBridge.to_ros_face(self, + category) +``` +Converts an OpenDR Category used for face recognition into a ROS ObjectHypothesis. + +Parameters: + +- **message**: *engine.target.Category*\ + OpenDR Category used for face recognition to be converted to ROS ObjectHypothesis. + +#### `ROSBridge.to_ros_face_id` + +```python +ROSBridge.to_ros_face_id(self, + category) +``` +Converts an OpenDR Category into a ROS String. + +Parameters: + +- **message**: *engine.target.Category*\ + OpenDR Category to be converted to ROS String. + +#### `ROSBridge.to_ros_boxes` + +```python +ROSBridge.to_ros_boxes(self, + box_list) +``` +Converts an OpenDR BoundingBoxList into a Detection2DArray msg that can carry the same information. Each bounding box is +represented by its center coordinates as well as its width/height dimensions. + +#### `ROSBridge.from_ros_boxes` + +```python +ROSBridge.from_ros_boxes(self, + ros_detections) +``` +Converts a ROS Detection2DArray message with bounding boxes into an OpenDR BoundingBoxList + +#### `ROSBridge.from_ros_3Dpose` + +```python +ROSBridge.from_ros_3Dpose(self, + ros_pose) +``` + +Converts a ROS pose into an OpenDR pose (used for a 3D pose). + +Parameters: + +- **ros_pose**: *geometry_msgs.msg.Pose*\ + ROS pose to be converted into an OpenDR pose. + +#### `ROSBridge.to_ros_3Dpose` + +```python +ROSBridge.to_ros_3Dpose(self, + opendr_pose) +``` +Converts an OpenDR pose into a ROS ```geometry_msgs.msg.Pose``` message. + +Parameters: + +- **opendr_pose**: *engine.target.Pose*\ + OpenDR pose to be converted to ```geometry_msgs.msg.Pose``` message. + +#### `ROSBridge.to_ros_mesh` + +```python +ROSBridge.to_ros_mesh(self, + vertices, faces) +``` +Converts a triangle mesh consisting of vertices, faces into a ROS ```shape_msgs.msg.Mesh``` message. + +Parameters: + +- **vertices**: *numpy.ndarray*\ + Vertices (Nx3) of a triangle mesh. +- **faces**: *numpy.ndarray*\ + Faces (Nx3) of a triangle mesh. + + #### `ROSBridge.to_ros_colors` + +```python +ROSBridge.to_ros_colors(self, + colors) +``` +Converts a list of colors into a list of ROS ```std_msgs.msg.colorRGBA``` messages. + +Parameters: + +- **colors**: *list of list of size 3*\ + List of colors to be converted to a list of ROS colors. + + #### `ROSBridge.from_ros_mesh` + +```python +ROSBridge.from_ros_mesh(self, + ros_mesh) +``` +Converts a ROS mesh into arrays of vertices and faces of a triangle mesh. + +Parameters: + +- **ros_mesh**: *shape_msgs.msg.Mesh*\ + + #### `ROSBridge.from_ros_colors` + +```python +ROSBridge.from_ros_colors(self, + ros_colors) +``` +Converts a list of ROS colors into an array (Nx3). + +Parameters: + +- **ros_colors**: list of *std_msgs.msg.colorRGBA* + + +#### `ROSBridge.from_ros_image_to_depth` + +```python +ROSBridge.from_ros_image_to_depth(self, + message, + encoding) +``` + +This method converts a ROS image message into an OpenDR grayscale depth image. + +Parameters: + +- **message**: *sensor_msgs.msg.Img*\ + ROS image to be converted into an OpenDR image. +- **encoding**: *str, default='mono16'*\ + Encoding to be used for the conversion. + +#### `ROSBridge.from_category_to_rosclass` + +```python +ROSBridge.from_category_to_rosclass(self, + prediction, + source_data) +``` +This method converts an OpenDR Category object into Classification2D message with class label, confidence, timestamp and optionally corresponding input. + +Parameters: + +- **prediction**: *engine.target.Category*\ + OpenDR Category object +- **source_data**: *default=None*\ + Corresponding input, default=None + +#### `ROSBridge.from_rosarray_to_timeseries` + +```python +ROSBridge.from_rosarray_to_timeseries(self, + ros_array, + dim1, + dim2) +``` +This method converts a ROS array into OpenDR Timeseries object. + +Parameters: + +- **ros_array**: *std_msgs.msg.Float32MultiArray*\ + ROS array of data +- **dim1**: *int*\ + First dimension +- **dim2**: *int*\ + Second dimension + +#### `ROSBridge.from_ros_point_cloud` + +```python +ROSBridge.from_ros_point_cloud(self, point_cloud) +``` + +Converts a ROS PointCloud message into an OpenDR PointCloud. + +Parameters: + +- **point_cloud**: *sensor_msgs.msg.PointCloud*\ + ROS PointCloud to be converted. + +#### `ROSBridge.to_ros_point_cloud` + +```python +ROSBridge.to_ros_point_cloud(self, point_cloud) +``` +Converts an OpenDR PointCloud message into a ROS PointCloud. + +Parameters: + +- **point_cloud**: *engine.data.PointCloud*\ + OpenDR PointCloud to be converted. + +#### `ROSBridge.from_ros_boxes_3d` + +```python +ROSBridge.from_ros_boxes_3d(self, ros_boxes_3d, classes) +``` + +Converts a ROS Detection3DArray message into an OpenDR BoundingBox3D object. + +Parameters: + +- **ros_boxes_3d**: *vision_msgs.msg.Detection3DArray*\ + The ROS boxes to be converted. + +- **classes**: *[str]*\ + The array of classes to transform an index into a string name. + +#### `ROSBridge.to_ros_boxes_3d` + +```python +ROSBridge.to_ros_boxes_3d(self, boxes_3d, classes) +``` +Converts an OpenDR BoundingBox3DList object into a ROS Detection3DArray message. + +Parameters: + +- **boxes_3d**: *engine.target.BoundingBox3DList*\ + The ROS boxes to be converted. + +- **classes**: *[str]* + The array of classes to transform from string name into an index. + +#### `ROSBridge.from_ros_tracking_annotation` + +```python +ROSBridge.from_ros_tracking_annotation(self, ros_detections, ros_tracking_ids, frame) +``` + +Converts a pair of ROS messages with bounding boxes and tracking ids into an OpenDR TrackingAnnotationList. + +Parameters: + +- **ros_detections**: *sensor_msgs.msg.Detection2DArray*\ + The boxes to be converted. +- **ros_tracking_ids**: *std_msgs.msg.Int32MultiArray*\ + The tracking ids corresponding to the boxes. +- **frame**: *int, default=-1*\ + The frame index to assign to the tracking boxes. + +## ROS message equivalence with OpenDR +1. `sensor_msgs.msg.Img` is used as an equivelant to `engine.data.Image` +2. `ros_bridge.msg.Pose` is used as an equivelant to `engine.target.Pose` +3. `vision_msgs.msg.Detection2DArray` is used as an equivalent to `engine.target.BoundingBoxList` +4. `vision_msgs.msg.Detection2D` is used as an equivalent to `engine.target.BoundingBox` +5. `geometry_msgs.msg.Pose` is used as an equivelant to `engine.target.Pose` for 3D poses conversion only. +6. `vision_msgs.msg.Detection3DArray` is used as an equivelant to `engine.target.BoundingBox3DList`. +7. `sensor_msgs.msg.PointCloud` is used as an equivelant to `engine.data.PointCloud`. \ No newline at end of file diff --git a/packages.txt b/packages.txt index f3111fa7fe..558a26bfb4 100644 --- a/packages.txt +++ b/packages.txt @@ -6,7 +6,6 @@ perception/pose_estimation perception/fall_detection perception/compressive_learning perception/heart_anomaly_detection -simulation/human_model_generation perception/multimodal_human_centric perception/facial_expression_recognition perception/activity_recognition @@ -16,7 +15,10 @@ perception/object_tracking_2d perception/object_detection_3d perception/object_tracking_3d perception/panoptic_segmentation +simulation/human_model_generation +control/single_demo_grasp +control/mobile_manipulation +planning/end_to_end_planning utils/hyperparameter_tuner utils/ambiguity_measure -control/single_demo_grasp opendr diff --git a/projects/opendr_ws/README.md b/projects/opendr_ws/README.md index df653de55c..31a6aba763 100644 --- a/projects/opendr_ws/README.md +++ b/projects/opendr_ws/README.md @@ -49,8 +49,8 @@ For the initial setup you can follow the instructions below: You can now run an OpenDR ROS node. More information below. #### After first time setup -For running OpenDR nodes after you have completed the initial setup, you can skip steps 2 and 5 from the list above. -You can also skip building the workspace (step 6) granted it's been already built and no changes were made to the code inside the workspace, e.g. you modified the source code of a node. +For running OpenDR nodes after you have completed the initial setup, you can skip step 0 from the list above. +You can also skip building the workspace (step 4) granted it's been already built and no changes were made to the code inside the workspace, e.g. you modified the source code of a node. #### More information After completing the setup you can read more information on the [opendr perception package README](src/opendr_perception/README.md), where you can find a concise list of prerequisites and helpful notes to view the output of the nodes or optimize their performance. diff --git a/projects/opendr_ws/src/opendr_perception/README.md b/projects/opendr_ws/src/opendr_perception/README.md index c2239dae81..c868a9648d 100644 --- a/projects/opendr_ws/src/opendr_perception/README.md +++ b/projects/opendr_ws/src/opendr_perception/README.md @@ -11,7 +11,7 @@ Before you can run any of the package's ROS nodes, some prerequisites need to be 2. Start roscore by running `roscore &`, if you haven't already done so. 3. _(Optional for nodes with [RGB input](#rgb-input-nodes))_ - For basic usage and testing, all the toolkit's ROS nodes that use RGB images are set up to expect input from a basic webcam using the default package `usb_cam`. + For basic usage and testing, all the toolkit's ROS nodes that use RGB images are set up to expect input from a basic webcam using the default package `usb_cam`, which is installed with the toolkit. You can run the webcam node in the terminal with the workspace sourced using: ```shell rosrun usb_cam usb_cam_node & @@ -51,7 +51,7 @@ Before you can run any of the package's ROS nodes, some prerequisites need to be - 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. - In the middle, OpenDR's pose estimation node is running taking as input the published image. By default, the node has its input topic set to `/usb_cam/image_raw`. - To the right the two output topics of the pose estimation node can be seen. - The bottom topic `opendr/image_pose_annotated` is the annotated image which can be easily viewed with `rqt_image_view` as explained earlier. + The bottom topic `/opendr/image_pose_annotated` is the annotated image which can be easily viewed with `rqt_image_view` as explained earlier. The other topic `/opendr/poses` is the detection message which contains the detected poses' detailed information. This message can be easily viewed by running `rostopic echo /opendr/poses` in a terminal with the OpenDR ROS workspace sourced. @@ -456,7 +456,9 @@ Feel free to modify the node to detect faces in a different way that matches you ### Landmark-based Facial Expression Recognition ROS Node -A ROS node for performing landmark-based facial expression recognition using the pretrained model PST-BLN on AFEW, CK+ or Oulu-CASIA datasets. +A ROS node for performing landmark-based facial expression recognition using a trained model on AFEW, CK+ or Oulu-CASIA datasets. +OpenDR does not include a pretrained model, so one should be provided by the user. +An alternative would be to use the [image-based facial expression estimation node](#image-based-facial-emotion-estimation-ros-node) provided by the toolkit. You can find the landmark-based facial expression recognition ROS node python script [here](./scripts/landmark_based_facial_expression_recognition_node.py) to inspect the code and modify it as you wish to fit your needs. The node makes use of the toolkit's landmark-based facial expression recognition tool which can be found [here](../../../../src/opendr/perception/facial_expression_recognition/landmark_based_facial_expression_recognition/progressive_spatio_temporal_bln_learner.py) @@ -640,10 +642,10 @@ whose documentation can be found [here](../../../../docs/reference/audiovisual-e 1. Start the node responsible for publishing images. If you have a USB camera, then you can use the `usb_cam_node` as explained in the [prerequisites above](#prerequisites). 2. Start the node responsible for publishing audio. Remember to modify the input topics using the arguments in step 2 if needed. -3. You are then ready to start the face detection node +3. You are then ready to start the audiovisual emotion recognition node ```shell - rosrun opendr_perception speech_command_recognition_node.py + rosrun opendr_perception audiovisual_emotion_recognition_node.py ``` The following optional arguments are available: - `-h or --help`: show a help message and exit @@ -675,7 +677,7 @@ whose documentation can be found here: 1. Start the node responsible for publishing audio. Remember to modify the input topics using the arguments in step 2, if needed. -2. You are then ready to start the face detection node +2. You are then ready to start the speech command recognition node ```shell rosrun opendr_perception speech_command_recognition_node.py diff --git a/projects/opendr_ws/src/opendr_planning/scripts/end_to_end_planner_node.py b/projects/opendr_ws/src/opendr_planning/scripts/end_to_end_planner_node.py index 2264d6e68f..757280aa16 100755 --- a/projects/opendr_ws/src/opendr_planning/scripts/end_to_end_planner_node.py +++ b/projects/opendr_ws/src/opendr_planning/scripts/end_to_end_planner_node.py @@ -17,7 +17,7 @@ import rospy import numpy as np import webots_ros.srv -import ros_numpy +from cv_bridge import CvBridge from std_msgs.msg import String from sensor_msgs.msg import Imu, Image from geometry_msgs.msg import PoseStamped, PointStamped @@ -33,6 +33,7 @@ def __init__(self): Creates a ROS Node for end-to-end planner """ self.node_name = "opendr_end_to_end_planner" + self.bridge = CvBridge() self.model_name = "" self.current_pose = PoseStamped() self.target_pose = PoseStamped() @@ -41,22 +42,28 @@ def __init__(self): rospy.init_node(self.node_name, anonymous=True) self.r = rospy.Rate(25) rospy.Subscriber("/model_name", String, self.model_name_callback) + counter = 0 while self.model_name == "": self.r.sleep() - print("Waiting for webots model to start!") - self.input_depth_image_topic = "/" + self.model_name + "/range_finder/range_image" - self.position_topic = "/" + self.model_name + "/gps1/values" - self.orientation_topic = "/" + self.model_name + "/inertial_unit/quaternion" + counter += 1 + if counter > 25: + break + if self.model_name == "": + rospy.loginfo("Webots model is not started!") + return + self.input_depth_image_topic = "/range_finder/range_image" + self.position_topic = "/gps/values" + self.orientation_topic = "/inertial_unit/quaternion" self.ros_srv_range_sensor_enable = rospy.ServiceProxy( - "/" + self.model_name + "/range_finder/enable", webots_ros.srv.set_int) - self.ros_srv_gps1_sensor_enable = rospy.ServiceProxy( - "/" + self.model_name + "/gps1/enable", webots_ros.srv.set_int) + "/range_finder/enable", webots_ros.srv.set_int) + self.ros_srv_gps_sensor_enable = rospy.ServiceProxy( + "/gps/enable", webots_ros.srv.set_int) self.ros_srv_inertial_unit_enable = rospy.ServiceProxy( - "/" + self.model_name + "/inertial_unit/enable", webots_ros.srv.set_int) + "/inertial_unit/enable", webots_ros.srv.set_int) self.end_to_end_planner = EndToEndPlanningRLLearner(env=None) try: - self.ros_srv_gps1_sensor_enable(1) + self.ros_srv_gps_sensor_enable(1) self.ros_srv_inertial_unit_enable(1) self.ros_srv_range_sensor_enable(1) except rospy.ServiceException as exc: @@ -74,7 +81,7 @@ def listen(self): rospy.spin() def range_callback(self, data): - image_arr = ros_numpy.numpify(data) + image_arr = self.bridge.imgmsg_to_cv2(data) self.range_image = ((np.clip(image_arr.reshape((64, 64, 1)), 0, 15) / 15.) * 255).astype(np.uint8) observation = {'depth_cam': np.copy(self.range_image), 'moving_target': np.array([5, 0, 0])} action = self.end_to_end_planner.infer(observation, deterministic=True)[0] @@ -82,13 +89,13 @@ def range_callback(self, data): def gps_callback(self, data): # for no dynamics self.current_pose.header.stamp = rospy.Time.now() - self.current_pose.pose.position.x = data.point.z - self.current_pose.pose.position.y = data.point.x - self.current_pose.pose.position.z = data.point.y + self.current_pose.pose.position.x = -data.point.x + self.current_pose.pose.position.y = -data.point.y + self.current_pose.pose.position.z = data.point.z def imu_callback(self, data): # for no dynamics self.current_orientation = data.orientation - self.current_yaw = euler_from_quaternion(data.orientation)["roll"] + self.current_yaw = euler_from_quaternion(data.orientation)["yaw"] self.current_pose.pose.orientation = euler_to_quaternion(0, 0, yaw=self.current_yaw) def model_name_callback(self, data): diff --git a/projects/opendr_ws_2/README.md b/projects/opendr_ws_2/README.md new file mode 100755 index 0000000000..4379f69595 --- /dev/null +++ b/projects/opendr_ws_2/README.md @@ -0,0 +1,88 @@ +# opendr_ws_2 + +## Description +This ROS2 workspace contains ROS2 nodes and tools developed by OpenDR project. Currently, ROS2 nodes are compatible with ROS2 Foxy. +This workspace contains the `opendr_ros2_bridge` package, which contains the `ROS2Bridge` class that provides an interface to convert OpenDR data types and targets into ROS-compatible +ones similar to CvBridge. The workspace also contains the `opendr_ros2_interfaces` which provides message and service definitions for ROS-compatible OpenDR data types. You can find more information in the corresponding [opendr_ros2_bridge documentation](../../docs/reference/ros2bridge.md) and [opendr_ros2_interfaces documentation](). + +## First time setup + +For the initial setup you can follow the instructions below: + +0. Make sure that [ROS2-foxy is installed.](https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html) + +1. Source the necessary distribution tools: + ```shell + source /opt/ros/foxy/setup.bash + ``` + _For convenience, you can add this line to your `.bashrc` so you don't have to source the tools each time you open a terminal window._ + + + +2. Navigate to your OpenDR home directory (`~/opendr`) and activate the OpenDR environment using: + ```shell + source bin/activate.sh + ``` + You need to do this step every time before running an OpenDR node. + +3. Navigate into the OpenDR ROS2 workspace:: + ```shell + cd projects/opendr_ws_2 + ``` + +4. Build the packages inside the workspace: + ```shell + colcon build + ``` + +5. Source the workspace: + ```shell + . install/setup.bash + ``` + You are now ready to run an OpenDR ROS node. + +#### After first time setup +For running OpenDR nodes after you have completed the initial setup, you can skip steps 0 from the list above. +You can also skip building the workspace (step 4) granted it's been already built and no changes were made to the code inside the workspace, e.g. you modified the source code of a node. + +#### More information +After completing the setup you can read more information on the [opendr perception package README](src/opendr_perception/README.md), where you can find a concise list of prerequisites and helpful notes to view the output of the nodes or optimize their performance. + +#### Node documentation +You can also take a look at the list of tools [below](#structure) and click on the links to navigate directly to documentation for specific nodes with instructions on how to run and modify them. + +**For first time users we suggest reading the introductory sections (prerequisites and notes) first.** + +## Structure + +Currently, apart from tools, opendr_ws_2 contains the following ROS2 nodes (categorized according to the input they receive): + +### [Perception](src/opendr_perception/README.md) +## RGB input +1. [Pose Estimation](src/opendr_perception/README.md#pose-estimation-ros2-node) +2. [High Resolution Pose Estimation](src/opendr_perception/README.md#high-resolution-pose-estimation-ros2-node) +3. [Fall Detection](src/opendr_perception/README.md#fall-detection-ros2-node) +4. [Face Detection](src/opendr_perception/README.md#face-detection-ros2-node) +5. [Face Recognition](src/opendr_perception/README.md#face-recognition-ros2-node) +6. [2D Object Detection](src/opendr_perception/README.md#2d-object-detection-ros2-nodes) +7. [2D Single Object Tracking](src/opendr_perception/README.md#2d-single-object-tracking-ros2-node) +8. [2D Object Tracking](src/opendr_perception/README.md#2d-object-tracking-ros2-nodes) +9. [Panoptic Segmentation](src/opendr_perception/README.md#panoptic-segmentation-ros2-node) +10. [Semantic Segmentation](src/opendr_perception/README.md#semantic-segmentation-ros2-node) +11. [Image-based Facial Emotion Estimation](src/opendr_perception/README.md#image-based-facial-emotion-estimation-ros2-node) +12. [Landmark-based Facial Expression Recognition](src/opendr_perception/README.md#landmark-based-facial-expression-recognition-ros2-node) +13. [Skeleton-based Human Action Recognition](src/opendr_perception/README.md#skeleton-based-human-action-recognition-ros2-node) +14. [Video Human Activity Recognition](src/opendr_perception/README.md#video-human-activity-recognition-ros2-node) +## RGB + Infrared input +1. [End-to-End Multi-Modal Object Detection (GEM)](src/opendr_perception/README.md#2d-object-detection-gem-ros2-node) +## RGBD input +1. [RGBD Hand Gesture Recognition](src/opendr_perception/README.md#rgbd-hand-gesture-recognition-ros2-node) +## RGB + Audio input +1. [Audiovisual Emotion Recognition](src/opendr_perception/README.md#audiovisual-emotion-recognition-ros2-node) +## Audio input +1. [Speech Command Recognition](src/opendr_perception/README.md#speech-command-recognition-ros2-node) +## Point cloud input +1. [3D Object Detection Voxel](src/opendr_perception/README.md#3d-object-detection-voxel-ros2-node) +2. [3D Object Tracking AB3DMOT](src/opendr_perception/README.md#3d-object-tracking-ab3dmot-ros2-node) +## Biosignal input +1. [Heart Anomaly Detection](src/opendr_perception/README.md#heart-anomaly-detection-ros2-node) diff --git a/projects/opendr_ws_2/images/opendr_node_diagram.png b/projects/opendr_ws_2/images/opendr_node_diagram.png new file mode 100644 index 0000000000000000000000000000000000000000..70b202ad3c00271e7075a2f54dddace2ece41584 GIT binary patch literal 51820 zcmeGEXFQj0{6CDpn@U4y8Bt29M3j~%el`h35S|K0zc-;L|~_4o`a=leX5<29b+b(}s5a*~^<8L250$|h+k2}KHp3SX~W zziutQld%V*VBZ+k0s#5-fS?c9J` za&mo|+^*z00cEq3+sc~l-*%^%W;Ey2#wpWko=a9doy_*+6Wg1;x}ogmO~WHJ+D`k! z_HC)BOfS0UN2ykxswm5Ebaf!o6`gOI^FmVaMTQkU0L|NHjtRsZ*^ z|GNf=&Hoz*|F2wwp8Dy(Ie*0^E-sG8xH3X2PH|FrE5*|-(YCkpesr`Bx0`X?LdF{W zt@kT_ePI`rEJhj{3WaRvwhEl`d!vhDD!8n zt*t{sLTY6@9?eZcE|33ITq@B zD{KZ_{(U@ivtoi$tJ`$j*jO2>q^_>6rtMe0`%eajeh-B{d{~-R#=BlY zy>>IRTGjKFn+@IRSCJo2mCv93^<->u#6vWJ)$LC;k7?~oXXfe~CIj`!(o#EV(`+wz?!eOp^w z)!tsccs^Q0Sljt`T=-F4F`=cI?&@5J5rMXor*Hqy^Y=9wM_)*acKY>5`c$Y)3G4Dq z*-X{)LKVlMLo$M|m6KG){B#!BxN-;Ilz;jp(=y$|?QyEqpMjp1c8AvWiwh&mOO~8k zImd^>H*n`29Y&O2#d8F7KNfVjrs+0WS7Ddf-OMa-7GUYRH`(obth9B}`U4UY z5-ph)`ajF>k70>l>Pu{!+^gWb-Lgj}p&xzQ3{R zIWkguQBrj7qNeLuNu5}cw>L7dxF<}9rDMK*nhh~9G(Y;0CU~4Yzv&)biDGdY;jQ9c zo0G?y>Gz-aUa@Lz?M}LL668@UMVA+{y8{JHha5_Lw~>okocUoGrnNmx+u0;w{`I?G zk7uT`3bl^x^Y_jkDVR59u9|YIdd!!kE~S?6?Afz=Gm5#q0St%a+b=Ip zm&Hm4e-CsSOe^cE6dXBk#w6rug_uo{n^q0RbMA9v#h={|4o+`rSjijoKXIb>C7j1( zk5&qoUIwY!YuJN8C8=nc2Nt4`A|GguBd*`EFJk&Ps)+x4y6 zww2)X^v1eM#=cs*A8VR!HcKhVw(fo~hag*y{rYQuw#tJAndT!MyuYM2;gUWAq&iji zqrtMr^0dduD9Zt)OS*AYpb!0lQdH206T=6hU3m|Np;p*6Xob}ZUhms-%;<}^L%Y*~ z?VM^`vRZ9EZ=O%6`189)AB)0c-BqlXVXB-rS+Bvz4(d&;~-ouOpz_1|=> z)^7Cn_GUO{R6IXY7(JN#`^A&*MiY~L)qAhy4rRuQcq}cLrcWd~W5u3n<=WHI-VY1g zQMkM~QuC1CZS(_G@VL&>cv$R0U5Y#3wd~HjZ?9POeT%WKxp0zwQj*0fw%%O_QOA$$ z3wBOYN$wiZS$0-jp6Lz>9#2XsV3d8xDcz4+XZdHOLzIo}O?rAaiPVh@ys`BKnbYH+ zxyvp}?h387wXwN<`#l%^R?;jesSVq9>?p&}Kd!A37oKe)>^?Uo%Gq)I{d#AaV z$GJ^Dd#eb?Cti{l=5wCx53R4qs?Q8N6h5h4UYahHJQV8N)zuZwqkryuRCF|}W&Wf} zMsi-B&?%#(h1rYh>gxI(B>wB`Gt3%d^R9fj`Ibkizdl*R^|!2!j3D>n!-w5xKC;#~ zR!xyRjt!PetVyE?KG5j+Tc;^MzD<0W7Zm}=U45(O@h zzsp-K-`P$gwyH8IQ8tW=FH?2XVEwDh^hv5Ifk>3MaSE)CKW^Y3Dtl*cza89x!)>|o z89lwdk_f#F{iI4c%}n#licx4aio^J%fb`|W%A2kCPgpcdTzj20eM8vz+`qkcX**UW z6da_HKP_YHDh!ZDQik!qiYpd|gAb#tw9nma(U1JQmGwO)Z?9 zoCJkaOgY}!4>hZosU-9-4omrMKhl4l`F-U~`9GxZqHJPf;=fDqL_#zwlgY6kjC`gc zAr4#Hl{YmcMqR2yoi{b_Sjq`tPu08SVtJ#pKUdxx_{IoB|TEp)$k6w3)I=gQ0 z$W1Y_vy`dC_4NCT{soK4F0ZlnyED&tw=>JYcL!F#Y{K4l5>{b>`OE zi)v~+w{6>IQkFOV=`fF8vG=aliuOx(KDCMZNr{;*D;ib*r>SXA4yIS_@4+pV9*mE8 zkhz>5y=bo3o~kQOezfj2kKw0~x4m>MXsvuxlVODm>{v{tWVhCAYiXx%PtV+c@`Nc; zz|I$|eP9S#_wr-g;I1+j7jwm^f4!=(nTNYzrpuj)=6`!kKF9y|*pHh9Ifuf7RhC;imbP=}jfyEQH#Zb)r)_ySC^Yfq zSnw3nwd|PE_phHlV_W(YFM3u*MI~=(ZXjbS_i$#k@+|A}7^}(j!qWUmS)@bzJJq?B zFAx6%SI8IA3Q-^K)n?I@v#~usZbH{FTj|i1Rk&y&A#wik@q=;#zhn8A7bf-|I1sP? z&7u12jMrTqKq(i6CTlP8I1IOXH*Pn5p_tI-!js6s!QnmPHsUgt+p);S#)cyQ zZl<+YWJhnbhdU{nN3vSql(|s`5B*oITrSi#-KM2Aj<7x#P}b%;@pY`#wu=4O-2U_7 zThO6i+H1zkMRsAsdgSKaemyeodSo1xP``wxMv_Hwq@&PdVLYtRpENk>7V=;9lop_e z*gSNg$iU~By)@?}P&to`%?+fKQL(yip{2DfT(r+aHxkS@l88O>kvcU?&3P+{P|y4q zrV5U&)@5hsr-sagUZ3^bK8Cs;C!AvCvN%5-)S&Hh1nZOjx%+Bcj-5ZlvHfzwleMPv zuM3yvX4*#Fcf5Ni7$sYiqEqPD<#XV{97=VtOUreR%sQL;pMx|JTe=y&tx&7C}Y60QRp9f@}Dz5`EZ zFN}Zg#_|S`tCa?^vToSHKrpjkl3zw*SK8V(bt@n3nzJR_B4_u}_CnibGYk z8L)ivl`caW4dfy`2Icq2%{a_Frqn8{m|Aka-bmKprTWK*Tl(WaE36x|9O~wbgsgrz z{k$*PrL#P#6Ff&YZ*gw2Wnhunx|p^?F~VJ*LY0(oDZUg?XfK-{9=aO6U{Pf4v42`c zA!xyLhgCd+J6{%AD{NW#E9xy&*!kR=;ei84sPz0kr>K+dBuTf=2cXiUybOV&Zqs?_-n6> z{`ioF9x%tUHkC%pq0J7>n(^JcE;`=eILW^q$u!k{ac+)fWhyJ-kkeb9wtN3;RrWvr z)W`?%=$F<`AN9=dllPdrDPdKQ?>qbMVrO+G(rJY9~8fZLg2C z3(N6$-X_#!`V(JS0Mm{JP1_3#T{{ZB-@JJf>jrdHUFfk~UQyvw87Ww148YF8$?0$1 zT{;x%zQSJenj6jERPoF=BNf1|zt-D(27H&5>4f>qUfNyZ?I;(qLO{&I1?_JPKX;KW zRXVhdmiBWPcTnzd&S8O3C1`$4qn6RX_+u6$-~bUS z-@il0)mk|4*N^+5p&4ZjGr+Zv?ejYphaE0nyhwY@xbl3n*XCV}G{S!rj+-|gRR{Q* zot<@Q<$ga*_dnMfMhSmAN8>T`(X>oSNvSyG;3d6ZUmg*%yi>&0@#nAl#i1^&n`2L? zsve*lkL%3nRTR|7spLml#P%xtk zfqKqI37r~Z%xZTw`!m>N>(w10;^s^g08;Uz7YB_UO#BI#$JQUSv9Tdnwwr5LYHxVJ zF589X8>IX3Zr$(Rm{+ej_sd5y6A-sIke>~Jd|Lq1$@aLhm6NjnxS-WKCzQ(vHt5!9 zhX)eE!1B-T(Ap|5JgF4&@ZtWUrluyI>wVwIGOHg^dyM-a{7^<#_PJTZYodb?p!^IU zbm~QF>e1oC74_8g!QD8nXKnlk~subTDkV{?Q!GYf1 z9`jiqe&DbNe|>!tX8(l*rX%{0>86dcGBRB^H*k3uE={JG=nta{9yj?OTR*lLeADS} zCtrgmeUR{!+CLp&qDoYBX`xqCvN+TDjEa{1pK$#^AJXn{*_z5({0S>=ftL0!9b~~h zb8vA5CcVx%#Nx3qW-4290@Q%3aA7dS$6um^1uX`#MAl$wf4l51LHk$r9fX->59bW%br#*Jn=&*uE}I5(o0&}U(CSvZ zvC7Z*zd-!k=0Ojn`#*Zo$bZ6$27IIu6=DGuetYkzUJGX?;^bU!Wu!isvdMgu``quJ zGix?%FGl=6*`9q?f?zf%5iAWl9&Q2MIip1z{&Mq-dq>%xfmIO=hJRu7qyFr8kIDSf zSdhoQ9zec$b!b+EXfgzBNB!od6ckm0{K?M)`mkHPtM*vvX637FO{#^+F*9W8!Nhd(ntxrA zYMJMXRS(>vUB{)-T@EL7ICQv~92-TW*q_bGk{_N0+i5 z$)7HT!nRkh<<;lJ&VH~ux*gh%xmJT8K#AJ*TrwySVY7ETm*!4zHmNB)-1fO3 zmR$siq7IfqyW4U^BlG+e2{((o9dTjWj2c_M&C5d_OUcdI*wZKWU)JoO!w=jpJUMZpnd$h|d(C-HwgC-92{A>1=F_%> zCR!KRfuv|&R9)iS#B_qw1mS%-Ii>8wk4`UEx#76XhD}o$1a-_;#ry}My*$YdCu`do zbr2ORgs5WB$!wa_NSc6l@FjTQq0@j28RuuGp{#H-?Vu6?xgBVvoiq__Qg%>}|K z80VX{Q?&D+>6Zm&412(`cwmDH^4&PefLrY19K zYx)3ab_J?G-tXmDO+tr%wGarr^r-^}4iK3ZRpYS*)X^pfJZ8P)(o|=sC~7Zb&^)NN zYH~96z+{iIVdF{IH*dQipR{DTeED)+W?~iX!p~rN)4?pa`6<54-k(4B^?ZsuN0taC zkMulfR8nMEWaO@)=8UAV$96-4dKyw{las=ox9R(@e@!HLhSKFriYUMgJ<>pq$wQem z6>o1mbzBB3dYxT=Qvdfj+7_|e#{~zJSGCBCE*@uNBNX|qEvi68U+X(tQm;?-Mg((S zq-GZS&%dF12WawQjd{Myw4711@>6lX*&7<|94HU3N+VLteY=)&7b)5)#Sj zbPfhhZww(7xjodiI229KZ`q#rc52ADNBIy3$IF`V=1dEXfdaviykp5rkn>0Ea*X19 zeSQDxl?|`B$|Ol|i_4fEYRQu8#pXJPmF}RG-XZ{QYN`vghMWY^SRPp}Ar+jIb|b;p+W7)pIepI@|5QslI9oh*|2Kx44DuV7kdFoj^#@nI6QS06Sox z1lCXw3m{;K8GK6KY`$N$@UcoE#L>NMUC`>TeSatuXTLL63oR$(ZgB4N{$uknV;^Tq zTrJL2NU?d?Gwcj_^uWg6-kxj`9{O?AkeUfd!Wh6>ouhW}{tM5r4$?$Ztt;RTCHSFA zXyUwt1o6I1<`cOt<>t3o77Bx6JXvtygZ(g5=}3puLkk$Rs|EmA{ zGsctsb<(r5z0pAs5JlKBzeWh67N`6DmOc*>Y<%S8_i-JoKTO45PfS{FDAC-0va zPBf@5kB%LXgXI{0%GQXuJ0>wr5U(;nisaf2Y(Lp*C4L>+J%J2_*8Rl~fv0|jdwUa- zMv4@W9@YnjZqRgxhp3$3(A%A`l^{P9LwKi0JGc;ae{8En88x3;Ut8guuvBoeZQ(=x)(v@3*(M!ZhC}uq;T0? zLV`eU!UPGZ>4Fa#SQ@}ohCbGXkI;ApDEYe=?JqFBN`yF!ETE~ZUBDed!SW)f)}na8 z`X>PFg|M0Rd(RcQ3+j~l=zoqGe;>Ho9zw-Uhe}FH#)ugr%bmZKmKMA(Fh<;~6wR}n z?9U$YyAs;k?Mp1fB9rja*n5DNRbA}0iZcxld2M8P@P&~Rm}+@0WjQhPx{-uLuKkcb zqXKDb?m&{NW&kF>@lno)l82C)J{O;#W1l2@us*?5(eBi@uTM^nSTWPy&-`hk{QO*i zFVWtYi2qZZxqArcCXn#^_y&LP$V$2V`QhA}qPi5fAu~Rftn|x%1UMpm4op(9I6hUM z(Rk8wE%ykEb6?EQQ+LtS+B;{vO9TAKk`jzeBvD>^yALxr)?%Sd4%j^#$6AM%)G zpScSow!1R>Ya^#P4jz>5-+Rvg*vp6Ks<$kf}9AQI$TZDLMWrDdd9SWzFLy?LXr+z3}004B5&j-#^X{+(cs5p zDOhwnMtzAnesbR^AKtIEa_fJ$elj$10f8s}#BtNwtMR=ChKB6ZDwo!21+y;vpsEI} z7TC3+miwP5YV|F_Od&h}!Gj0HgeMZa^{0dJ61d|o*vNhmx=%#G&^1UXxm5x=FH|Mv z$SdE*6S+2(5vRtUvo3N#IC+!_9>R@EmZt{C3%O|R<+cRwK&VkH+?$Ui4_z0=%EpNE z#}{QgRr}7{nNy zH_imrf;d7J4Gj%*7dMvU=OmC(4g;-i%J?ojJG)Ek2x<)j!Mee7VDLlP%S-dq*ByNj zP5|(2$&VswbC>f0@MH;y37b1|@Zc$q=v8xMwF_pyTAnM;WY?IUu40ogci}SW@!H!z z(EIJ55C}*xT*=Y;2;w2Z)lf5PO%guOow)Mea|S@27DS0FZ?s5DAPbn4XqIim;C(JZ zE1%6diVNYZls_&oQqH2R=mFNj70;?M78p(x-qdbC?UF8-^7X?`x|Pe4|Fi*SB{+#d zlH2CM*kQniZaI68}DU@EBG%&2@HMUA!={ z2kYj{WW2nz*bT!f05ZTy#^YBNEW7!)wBa{Aj-x-&)L;h*2d(wYm;UbxZ&0&`kIp2$ zO3{4BtC&#BrIia^kGQTVUGfDg*OXga{`94lb#{@WPA*Ic7Z~zXpSo8fB5i0KLr1s% zO=(fQy)8a_yham`PhWm`L}NRI-Nv4QUQ8x2E+{8wnbsxB#b{wJic|gV)vz*T?l#1| zoF+^mYXF*|@mY%1%PoI(tM_|egO#fQF4i=8jR8*wHYFDU``GI#fpDQZ^2`V&$ za~y!dEZ}fv`$pYrNVfxdmy!Ve9Z{(CVMt+ej(^`I1|Z-CUc!i5;{X}4!oyi4EeK}f z|M)G&bcUuHgimFm%LCz%fh6C-!NInSFza&qHYPIbv|zIOCcsG<@-dkkNl z>KNu)ZbI!4II@f+`MheW!zXgh9f*VeP;dme-u-;QKbZP)s^%r-$B#W6LQ<%cC15mq zJ)c5}fUK6LcUDn5>zWoOz4okMdHAz(7RJC9MSj61yox0zk5iXPz;h3oI>3@WfBJMc zDAkjYH343kq6?igV=z3kF!ty57C-4cAHr=ln(*?aH_=_N`%_mBA3T`ASKPv6kt=ZQ zWyO_MKa2SSPMrc*#0!X-{cQdd^6B`jVqP2L0z zTP-IY;TVKm`ZrM8oqEqgE}xzt&(+yhwCX%`7ot%T`5Rg&VGe3}j@B7Rtxyc-|6I8L zNq(1QVb9N>68MX}xv9`BEv7mH;h_?J7Ly{#z9z1qgK(zQJsS}oec$6`Ne#@gA7YFY zbP$PMYW%tnbVd3TQC`dLzmGA*go523vy%%UkxEe$gU164u3`DIRu-Jk zJZUtptgSC^%&t;t-m2=c92OQP2|*IRv|+TShb5Ui6Z;5e*SFx#zz%W6xa=8~;(`m(+8Y!&&dCbL!Wb^RAy3NgfDGKp+0ft<7hI4QB?H${Q! z(R72juxy+nsSDu{l<3x~Eao_lb!oX@+Ak_9y8at1&u6f;`6_FZ5?YZ;WXunX75@JjvjZ+W@1=eE^odxo6`2?}X+&G!d@Dclj{9U9uf&CZ#(GlW zQ>qkNLNiI&Q%`SJ%4Ot5LEOhW-16kusexvs9V42C9d*3JpY%wXIAZnA8$OzAURk>9 zj-PVN_{0PO0~H8bFQ?~>n=y!aNF_yE>&Ty^gX=iCxY~OLvVgkeK6_+dt?u$%%1S7R zgsMe`1_`($kwq+sITkt!v(oB2^HSI*PR;D2MwJnlEi?d3<*KT%L-4gEA*wwGR!5Bt zUwtF=SU*`$$Su|A+KX0_cEM0jplr38>1tvS34+G{54t@nmvwTko1pA*5C!G1$}4C+ zfy5O_)Z&JsLk5P(Z4i5eaHEjqOEeTEWp(w(+sB70Gl6FodFz4xuGYV5+Pum?FE7t@ zkG9swg2zRoA0!t^wZI?Fv6PZQ!2;EXxyh|a*|#;L`TIj}{KV!>eR4zXD$(`Y^>DS< z5%srH8|Vh!tY^|4`TvP6Ed1LZQL_yzH*DCjmz&#?wN2OPH@GS!+axmDOLgQ-r*0OR6`5FJ{-B#H>0+C&#hqD+hS^`T>%nqZfD6OFd(gBGR1-swx#ww~(j0HT$ zB&63M6A$IydPGxKjl5_83C|V?$=~~lPTlNZ_-VW#{9P^d6azc2w#a0qng(>_xt9C4 zv{6Ih?HCja%REJuMIh(Nex4aNpR(-nJ*XVE?L0CDy;rT!-HlT_&oQes$E{I~@47hz zT+F5lG-alsjmc;(5#Gsnw2T2o&qrKp|~>V+r zG*6Z%2nWQypNQL>QVZ%rdbFf_AH-Nc%yyltiV`+!)j?;$Y6YNeDz}hsgRwau(8FRV zR?GW)ant)SE4=g8+D*O$*^z~C7-?VXqw-vQHGa8Fc~!wRd~GL8@$fs@EzB6cW19kqZDQ7vYLp+#Q>n9B?o>XM=KR^Hq2~6QJ*NjKkKgEN;G8 z@@eR_`n$EhettlF41y7hS-D6V6f9&8)~vO#lk$F#v_keOXr9Eu#SHYxuCFIM{E0G- z86<`7G$^V*&x1q+MCO~S$Q1B1Y#XLEBnkeT9(kIejOzVdr_jBcXh&0kXvAg2!D&tnj?mPGBhfCsOms zc<+1;zEFE-W2iow6<_rW&HDA&rZJc1F#!&?5vRAt5Kxda?T-D2@*Q4 zshMkU5x^Qw;=m>uIES>!@t1dVOtE*(BVRHtTF#*wJcAE8WJbQQXvriK-fQf?PJQ;M z-@5QMoM-Pptuq_rTLZ2`$g8WE;rZ9l@-0_U3?9AG~T- z{p^mh>k6tnXut+OT`)ig$7fM-Gb*!RTWF*kT?h#cWp8+?shz=fY-jqLH$>>E6Dv;C z)iZ2*w@z09jzq3N<4#JodSF?EJ zOKd!}0zLEtg8xd4MdzE!tEy7sY12jdV3w}kxN{2`VfEw6;Q%b+MKivZ2PRLVVkd?| zU16IMx1Wr9lHn&fHc-R=$`4SvT#4t69Q1R}qo*|5l4T-U1!YCQ;sHC6BN;yy;wgse zCp9odJ9ME3eHBO=E0XW(Mez#60-Xa`-MW2y{cOaio(pCh=u}^+T^yQTLbyG!RmD6y zG%H_75*}WQ27qZ#PFN(~@g~mFOsCfY4U7mpL&m!N5lEdT69D~nNe(*~lpYn<13LOXm1WkzB_}jiY%tJK zSSTMAOeQUenh0&Wdtssq2MdtKC5QvPg_$uP_l22o44sih8v8)STD**@WC^%B5fA{Z zOBsxd8H^)n(Ys&v=2ASb1FB1{e4nUMm)Yi2uVXlqc(c$ODE1zsKO^IG zKVWd%X)mQ`_T-b(4dJjV-&(YsfHP5W@)`rhbI3qKK}|_%YnAA-EAg`aj*Wpy1-pSX z`CpPd#eU9zCoKtPOe@sH*EF&tzd&mVbqep+y5AmC(@azDf8!%_ap?4QZ(NfNC(I!H@raAtay?xsID$ILuJ*Mq{))O z#d`PE#%*RBwuXVykm3vp7`5~4IOi%8&OBK?<5l%WB>u+atGwb0AjWhEn)^k7~PpI*T|S3KXmPGGRQzL$NynjVnym_*bD>> z^PtboK-bd;NtbrQgih`s=J$y3wh0<;*Pdlhak)<69N1CCqz2c*JUqduH|>>O&+9-lNvfcc{uvm zWsOnQ%c9^P3rBGNXWgk-x;J21W!Nt!GFg}h@uFgxAJaJ{-n4tW-qy(Zal8GNtp;px zW;ysM7SBn{*JAcCO*+`MVA76zO9OW?k>?K*UE+tlS3pi6JcILO{b9`xB7kkdtPIA< zR#|z%%1Q%q*+*#=*`f(5faDweBLIw+%rlduZrSuE?&K2Xr7A?AibB~80RJF7w7T&p zZtXmNaySKpyMErw8|XV=3Y#{VLVF?R4#)wFsXok5yF2Z_26p0be4ZS7c^9Aa33^56 zcy#C_z-?X5!U4+rW#f^&?q~c<%gX|ZimMZ-50$LE`ShmV4u#VJdOq7Uj=yXS-N{+4 zrf<%(o%5nT=M^7z-`S^nt4cUd3~s&nVdYIiF>2{i*BPswjEsjgEU52@ z#VGO8`K*RHRup_des#bN1_p+>hcR(+B^#~YoCH*aOt{Y3*|~O|REz>gNXklOW##ZE zPd>J`GGn%T%a=<^O1HBtJGubWq%fv%A=_zooP8hNdJyC-OiX@_jcOlkzI^$zI-l0U z!b11T6@_O}=y=pl%W?7XVYAFv&CCvOU8?!^Ew$9~yduX|azhFs;sJbHjSLK~0B$zE zG2B2$Pyc8UsN(|=_|fZq3OgAX4qgi;kUDO^P9&gPWIC$t#5r*Yx>FICb{`NE5X=+A@?J4@XNkv^l z<6~`Y2;<6w@-G}9PemS1k+zm!Tvtzzz*B#u+OhUqh}sf# zm>`UR6n+1$_>NkUqv!P5vujYut!!;Sf@fRJ{V`d+X3d^``&MIHi$C0=g6@BEW`6#; z$MTW__9OEXx!Z#`Zr)tEZ{NPrjW^|^gwD9T7ihkNLsrhWT19#0*x!XUUDe;;5AIJ! z*s0~^<>ekf(=CkKbK&F1kFu|1m6f;c-@hMh@&3~>`MH+b+O3R?jG<1A|274;AH=B3 zvG4W|$dhR3tK=g1KfL-Pq7w78IW@g*84~0pzm##EI=etfEH#H`3D5q@<*-)XQuala~(~6T0HU zcKljv_>{@tz_`fEc7NU7yLXQsKQ4jcZKvNrcaGyf*BkRHn45EUb$9RC?&mb?=j%KA z>noGd8(T07#POPA$Byyy^S>`Dx_NHR)4#hQ7Q+!>YRTd6?_azyJ7J9x3Cd|ecpjrK zJ24+eKQ%QaQy(anpb2mGCJtL&fy1_L^X5{30us`AT3zy;b?ZC}3q`eV1!Hd$DQRl{ zWkIan;E&mE^Cxhg=+^x72n3bWul;>|)*oPg^v3}Jzt3~`-%#M&df~zaa#tTd+(<7> zP2ECJA;T7ZeSMr#@EaU7Z_TmWkaexCtDJ$6v8btup|Y|PAahqlWMp-$G<~z`Q-wqZ z%juSy!?x|lHdRTUj{@7@+liS=CXmq)iS0ao+$I6={cS| z-`S6xil&0t{-~&^(LaBR<7C59uY6e9n&(8k#rFVmsYYKJQO!^O z>AsKs_;gxVH|FWZqZsu(S{N6{Zd_UjUp2B=;%^$B<$@%a`sU3|=#!wJp9c@z{xLhz zn{C^>IiB^w{rfj?+qduB*@M~E3QGfn;@`LU1qCm4@42J6hV$~9)i-Y3*nOKrV;dGg z8X)t|Yx?sEbxKU)F(?tpjWpA`L#|5;*f>U5lKa7#C|C`{B*_>hLCES z8%rTSYzFbImA&`3!pPj&P3Q9s8s%{a)?iSp6wHNOrv@3ytQ>nQcNrKNbzw+fs2iHn zrp{l#);c&i;Im^8(Y01mF-k|5mX=1Rr&Ti<|E9bWV_lKL&K*0vF#XSKJy$qCEC?x& z`NHGl7vC6dm6<%Zk4_ir=;+kcImkuOJ#Z9l9WdW^5>n8eUbYA!geC#6*#ra_J3Bkq z(a@BkPmIk@7;6^*?MxW`uBnjuS6&k7?n~W9PbW76M~L6E>sy>y=|_lCP(arLX%~Sc zQMP)ChcRwyYirXrG(5Y>>%YnrLU%DSMuF}A6%?CrK?g4Mo^QyK!S;M678aIIN`7Jq z)-MwhbOD-o@>}hO{t$p=AS8?5u74}eqTYh;c2MIiiFjTBajWTJEgbUPEhNNz|Necb zFI_#h*RLC_{f=_F`qHIK>{n&~E9bljq^nRnfN}$A_nxD0u*geGNu4$@*msPfA9zSg zPVStQ)zO=`ZmrlVDS7YTts^wQi2u6)y03VM5r^yI93KvA1R*9U6mTc+4)^&@=xCQ7 z%50Shh=**^m}SX&cO%11*ckwQ8?3CX-lOYb8diz~7V@VQez6sf?;>tH;b@nam;dOo zwzBfXp(n`LKl=M+8n(s$>p}YHj5q$vTHT`6Tg2|&yZ8Ql2sI6jBt$sv0#_fhI`A1N z6ikt1=jJ}Q>?k1EA}}!ULD230TG=!65Iqk6z`%3SB5tEoLmC7pzzaNp`QW&X%0LQq zmX?-6?h891Ar*t`9yP7ykdTzr$KcWNtq0Lu5AD7Use@|W`t@g|q}Dxq{@lrFaA@ce z!yoj;cwLFV9XQ4{!&`#w?bYg)5VhW8yz}(BGp?>e>HBRZBv!+!^D8cv*tdUwS4!d1 z%@;5B3keAwz4D&o=;HDmyzG<6#=l*UW2-OSOB?Y{6b(|P;CRMB!NTXp9Gg2BBJ=+J zd&-9oAAtLqFb&~5IndAz*8ouPWNlqtXfg{5c8Fhi_)fIoPIx2)<-|UE^oU@v(a}-Q zq@=^=&z}!@zVBa;_kBR?BY{l5lbn16;}I*x*6Hc%lf;Ff5FzTp0!+GY)26dmuX0eH z0-S^XZ3KzmylT}dcGeaDEm31jo)_;4xZ0{@o#Yz zR&T+*rl;S6ghd`bB7&LRB2Hr!VV1!UT(A>LQnve|021>KqEL6RF>U786U(PDF)`+o z|6PP5R4O#!#SoGWRYm;FnH40z@bga?*B=Zy^bE^{y5O0X#v>OkvWbR<236>g#u4ny z!TTtM{yTYA{`&O`hn}|U?<13pPj4#5uQjV|<4o51`Fdt4^_f2M!$8F#In7WY{mV?=?j-k4{Xi+Orxn z+8ZFmQIwVI5??-Ex%<>92G!S^XYB1W-zdj$+{by!Zanx^_$J^DKL!Rih_e( zNu@Vw-{Hd>PF`z$kM5>tsJ2!Kvk4nPa0!RN^h5lo*KgEmSy@56W>!HkSMjB4KoUam z-kY71L%alKZP7Sq;(alTx-$bq@?**yG~=ats3|8<7=p71bAdMm9fo(GJh=mfPZB}& z4UJyNW$Jv>Aw>>8YS0)Q2YQN&;ne>=NW|;!ORYwUh!A$!?&ITwbI#k~;l-INAC&nC zMF9hwRQAKIJ_y2t8j+@-$9q0kL)*e>@AasXufFH7iny*r|GIkh>ihco?VC4mCb00_ zxpSfY7k4q7yv9t30Yq}}CaI==r46yureR!KM=>#Gm^7i>_4DWLz+Hj>+;luVJfBp& z6jv#zs!F1iZDD5ihi7NrQLqa*kb{%+FW!z3r*d?1>cn<_QVIR904knQN{qo7+%3p7 zNmJ89&<}3`_>1kMQ#J~%qZV5SXyJ!R*DFQ0*OOD+p#$%uPR&8_IFu5txQI~#`pj!> z)D+-I^ov+A8<5k}r%zKb7V+``o9FxYr*STK`=uA>&wz@L!Z*Oi9{c_0<0Hl3nT6S7 zI6LCA?Xb#jQPDz~f&I64b8>D34%jK|d>nl5X-Y~En06PY5J-nZNk0407q2XW-CYEk z4&}oOqCN-KRaQ)FOl$OBR(S&=vz|=>b#tl%4 zakP82URt);r$t4OJQTR1K4k@k-QgpM;V&cLmr9||8Prqh4&9}j*iFR}eERzOu3)-_ zBm=H})S!F|u7WBPRpapdKQLZve#oxY0`37Ja6pL=|JGkMF;O!ds=YB-PdGFHrrPDp%BG7UkI!PeaZ*?Gmj{?5T}HwwSJ(l{it-mP-q#E|3NF~c ziSt%inChL7V2qLNYz$--4Mcxdk$R^i12X_4@HCVXXRUWOXOFVZpJEZXel@gDv`&gUOJ%xWCj;lrgm*D)2$+1=<=y=-xd>KjZjdWz*O+jLV7wn0<#;Nn+d;w6_}zt zFpi-xP-oxSLW2rA6fI)N9OdOL!lgTZ{*0B_bTH)2hbgu%<>l{nJQml0L6WWyN3#gJ z(4kfmm=gq=;Ale(>Jus-nzJ6f)NeI4N#Alfbf^GdxqC%09vn}X5mP?CFl!4;aBNvz zTzt=csnhE|01?L|BqT@-6=)l@{6Y~vEKSe#X6>rId-r~D1%#w{LJz)z*_qJSJAff< zFkpnqt0kuCkq)o;_yceWJ4Z%nh|@cHfe0389OHa3Dwo$R<7EVpp1u71Hn_UF9!h#_ zx~WB>@tv)4_~XYaYE8Gd0&Y#~1k6Hl&DbV*rhiD_&p+2kJ$!g~W`>-n`|#zl_$uv-Twg`@3^#Z3L@+(7Z;a1m3w-Yr7m13L8PQPO&B3|K9-eH ziHL}#W@RluvjLU${aq7}INr4@&A5uiR5Lz=I#437Z3U_^dsnHqM03zKn1o|e8YGJ{Zh*0(QnddEjphj!~OM&SJBTv0^>d=7$ z&`E?&+uNUDW@i30$px82n4mBQ1|PWp)KM+puejX(-PiXN;mtnP!F%l3*v|@j0|Nty z1S_G(C}$U{Le@fc=>i&jsgcF?DmfV{k9wl3v$FuML>LU>rl`H01 zWL*OIGw1i38f?r)GqVAkLY=nO)~!+A4aXQZ>6hBxGxGNy(W@m*|1L4+q~U zK+nRCf}7k0W=00Br>CdC({nI`Mp9A|{6QCD1ZZFxUJ6u=0egr9F`6T^{|lw}gq}ta zSJ&6?r<4Pu9(wL;T7@FCo?-+W{m7o%_t55#&SzS7aB<{Xc8Kh+0!-mIf%j_wdKL>0 zKVm%YNq)YtkFT%Zcr?HD&ENvmuV*PKRZR`~E)F&@{;7qRMn|o_=Q>!P*@amVSzJsG zR$cf4lF0x=nz2Clc?Uq#MhRW5n-FB*2gKS{jz~kgXjXh}XLhmUFRw4uH0`on22w zAS0hI^naQ!%OF%<2bk#S#Be6*eJ~#%A3F>wwl!AHN^#zG_`$r_TKOM5e*EchM@!3-3J367xn>wjK6ugPbGXLjJ>$B)x6w9Z0^{rsv5^)cLrjIyBG7LKkv zm~jIv{aHdnMXMmV-p^Q%nb8|~V?|^z6{WhN;Q$O3+Pvvyw9EY+fan$A^WHf*0+Q}1 zV*B>&IrCZ@8_0~QCP=h_(-D{yx%WyVE6}?{FRB&i%EDdIxAx=39bFl!aM6#9&5|nd zh}V!B#_ncG99T$;m|B2cpl<-`?hU>hoT*>7S-NCg;-Y}amsdeN(02P)g9Jr z*RC~y*9cZ2+1Y(00CQu!P@*dZHP4>C17F*#v9a;!Jl3cr_y`BbZg`Ofb4G(-F+|3E z%I0=(^y0Ox%}UD36_9iJd68dx;c9laoisHyEiNmIFJ2mjki$PBJN2utPYT-bd0t-L za*Tt^)R`e2Zr|&2cy-+an{g!{Qrce7)85dUIN2RGll{tP9--9RM)AZ9h~k>m@1~F= z#f^>Ol-)pM`^egXUPa_U>?{YRlI84$IuHx~D zhe{N7Vyb@QW0hix^#uc%MiD`;P{qX7?I|cIh*%W!XXKMPDzB@%XKZ}@(AY;N3SJne zYhbYH4!Z@_X6Bi$?uSzQn4gPDx8Ox#>-XG-R;C!UfYVIuIQ{sm*5TI|hC4x)I1G}v z;`G#uq$F8@^68S^r!VmSG?|s2(4AfsyBhLreRC3Ih*z(89ky4o%hUJ*&(=?yRjR^y zZfy}O$FlI3xy_8~p8JKPQAcXr)D?3I3Pha#^wCPjLo7X{lqsYSB$HY+SC_02c0X8Z zp8;B3+$Tq8=Lc}txCKVUR=K{x2|cFY;Ajc6wY9a5YvreYz@=Q|H+f@-X^;?UOj?JE z^{PEmx*KyZ9|sp7tzNtK!P~cQ73~07J$AkW%?Z1@<*7m=iGJITG^(#Ud5(Los_9yC zadGY2yZ6?m7k~0EHNU`VFM`Vj$T{#iJ24F$xOH08ZCWooegd;a<73^D+HIgvaV+WA z^_i&Q-EVzx@bi^d_wFlQ6}0*3EjA0Eh`UiGQrP7oaP>i@yS4m>?@N_~*vWhyK{Z1I z+$Lh=gz>qt{XZ*OuoWOeiha8>A+er2S8e~;Z8J)@hMqQBaQkfr9| zMCQZH3P$Sgt}ZE@D3J*b=2N`HA?AwtS=ev^l#;uqQ^ghBHyEk^(}0FtGwv2)0%8k= zBnz)^3N5z5M&xu{DpiO9)$_Pi(Qq7xDjp1Dydv8o;4u89Jj8&bwz|4=XkM{Otm$&n z_vCABITGUHx6-R>YJ#_jNtFfeT4VU<&mYJ6l`B{BPlg(BC>eO3wm;{L2QTQGF=KO}&5N0&reJV76%Ly&&%Mt`Lo_Wkq7OkIs_~eN!R3%CHH>Ykj zirp>3E8Fg&Ybs)DgsBw-N%mb3is=*mZ%0p@*bmgO50Wc8`l8f6I-BJsH(uzp;HO0! z%TouIQ9Psp`H#^r08&S+I4vwJ9H{#EUW^rHtG;}>G9_Nx4u)GEk`C9`sE`bnu$(i5 zH})ts9Ni=MNebJ>JA=va0E#<0qG2@oN$q>UsLz2S#A&;6@I(I_M(V}I#YdMji2)Ut z^z!9BqUJ$|95ot;`D~qcsnO=chY$PM+3(=?@CXuO;^L=P=|NrSVh$QSzl7tf!D!!X zs4>8hPR{5U0b3M>*1czmu=w_L#VMC5PJN1;j0_tTi}N`cTfQ@! z)V_TpB2lq;HR*4>cZ>~M<33^GNL(P5OHjiDTRyZ`pmOZfr=H}giw?-_-LplpwLxD#8?ypD@2omk!a`=d zy_Lx@c2W1KBi^kUu%#g8TUnVGj|52Y26zvk6goP9eCGwD6*4Ec zf*7-9w|Q*)Wzga7t4ny^o*Ay2}_IVbs^E{Roz<`|N9u4LD`_ zqc7Wa*5(1ml#BJ!t}uN*v9&$h`c^RBl&3oWpVFMsjgO27$Y{O@8cog*A;i1IYQnMyau{u{`?hN z=Pls$;5>f(1%u)cUoOf}xjA6jGN&ip8IM6K9+|!<+wo!B{r@lTNcC5gg5)dX&TFy! z{qtm{Og+xhi*#PTtu2_?AtABtWP?KA&`<@P%^udA08_xGgT;g=h}0Rg9>4HxbY8vbXm5%-_z>E8Oi%ta5K#Mg4LbV{9es;Cn{iM&o> zD2@sW=3^54M>!uo9(|8CD!Z&t6viG~x>Rpd;2Vr3^iib_ov1=0INlGdWlwaz?Uxiy&!YHpR4z<}FMJ9IT4`#3WZeT`0IScPREeK(H-^B)n5}`M;FVG$Bm;Cy55fexApe*e!khNmX0F!dz zu*4j>HFS3wWV^Z719t`s3@>^z1j_D(z*Mjs@KcU+*w9ZGSr4f-YdqJ}(Nu(cX(Oea zcbWUM|5kgRIIkl$J)diE$KGr0=ccahHPY2qQcx%eJ>OhP= zSqk3{MYBk)eIaDBGxjhw(j3ZRg)^a~Cd2gFixu~1fkoBNH+Bhr`pc3#Z0l;*iZD@J zvE9$i+cMw-+3UX5lSeOzorUG#v{>pq$D_W}4;-ZkiHOxP;Q z2@qWXR8#Km?nm1D%|$eDmn{%yBLSSpQot@ue1Ky4-fm(a%6A4PrUz)iae0q1gHWh= zxaZL@143W+$k(bWpUf&aG`N=kNrCR`SWLp7(uG?=gzH1V&4GY5+bvY zpFV;|&VsbZhL12{{sse7xdhtdX#V7%Ki5r7pMZh190%JSfwW&CO6Wis75p7Ex5hxt zLf7Bj>n#|pM4=E72PHTmqu#$4MqkU=I4Krpe*l9JHWfE{HH8>6;CB1mb^Lzg|GWSs zmEx4=lDQQTad8jv6>jExdKNRXs$Pa||6EL+!{VGZ3{|ugkbD8JXk&Ulji}x-Fo?j@ zp%8;gNy*uLCg@UnLkefqdkiSZny4ns6e!(SO~qk}>K)cdH~0s-%Q^~?J_2}De-00S zy+w;;3UaE#h?aiC1_iVfy$*AN@dgEIQ34ryt8nR3$B+?p@ZM$o5Zto|;GDo+KX1-Q z34x|V{2ronXljOk*9jnCCfrev$VtS&0mB)gwSggS;|{mk`HkaP5F&Frc@7=ST@8`R z$(0K?+nFgayKt+iq9A_u^eH7`W@msW1Vs39k1hjSYwl&TX$gC}6=Y=A;5oY8 ztJ4w%J0xt`vtfczKF4O1G|~Ny5m{>~U+!^12q;8>Tn+o!pW#+U@Lp$dwa*`vzVmY( z_tKk8hy>6-Xd=Fc&EnAV<>N41dVMz*cm%+l7P&p9=W9iUf5=U3?T@X+Yyw+%QV0P2~kQGNQ0`^$FOru|t zs)_@(3z5CMEmri-PyufBws=W!oG5$FUbxUTl$n3!t`bVq154Y~ z^uEr%+2nK3^_w?UZe+uS1uas|@VFxFK85caVISTCHw~;W+~_K1zYWssWHVSgi`A#< z>*}gOF`lkdJpdz-+h{Z#YP*GmsEUK8-(Ia6Y0K5%X;nKn%i(U`(uVd8{w?32|9nY* zNa@;nv5EEvz>qX}3-I{YuA>(JC?N&NE9~Ts=L5@uWlI5SdtO6YkUtfzv2ak%E`w4^ zN5Rr4qr=P|O{B`AqSXi#yq9BZ=DfFkh0g+-21lcCNe{?;%1rOYZ>6i#&PZpuN`Md>&G7(@G7`32}czR9zbIJ6Dx&9{ywV`st8k8j zOmi!ii7WxlxSj%;NjZ7a&ZN0Id~n?%;qV z+5LL>#|Kr8Jsbt%o+sYYNR{D+j={-pUenyh0)*FP%Y3J=`tDdoQe~)!^|{}(WTf!@ z#(F5|D_5@E1^W}86dHtSC_`}eH)Oo?AD{T$A&nA>Z@GE#K=YlmZJaCaumdryl@sO{ z6zsN*l7){X)FOuMSobYjbRz71uT?_X{rJy|XTwo%eY`K;+HxS8iHQk5o82W96`wI% zWh*~AmTfAA24fNu5`KSBH8eO#hcZnskfvblGry2fpabu^YrJpODkfS~-P8Ki!!4hM zg?U4;fbel-zN{vHS+S`~O?LX|d}x`&;qJ-hX5}si*}fNnVbOb%e4I?Sip%v2YGSzid&T`8pU_9Mf_r13QtNf*&FL8#$7!n` z?H0BD+5^I%5amI)$9s)ztNqT4ld4(cLocR{n>xJD9i^7uTckOd!p4E_u}?1^vNABZ z;xZAVE%KfpvzxynpBQrGj&uwZkN-0Vb>}z6p8bM%Zw4^-2&^C{rIoEaL5Q+!%7N$T zz@?DoC}_pVm^YWmI6fE@P7$r8rsg-u`>cBphwfHD8)<#>Dssny@xtZM3$iXgzE(p8l>Riq=z ztC*w9pt<4`1voR&P^Rta>+6yItQ#TTVi0Lz2IC3FkkWRGi?f0tTT4e*h>1flHkCkn z{|Ipqund#lx^zF9pBXY2eN-tGbpM*^6{+$tNJ6vX`jspgViM?gnEst~980uO0pQvx z)245{U!c+9?{`Ptj`x2c=1JL^JHuySB-wlGQ#Ulk(A4W-3~~g;y6-FcIW_qC`E^s7 z(^92%FbXYTf`yI<)6kWJp7)+VZZpDibl2}AaQ&|I!C9EBXj_NQAggG zsRC063^FTt=(TI`2WOp1=}3x3W^#l~3(DvhlN!x{6BfJK&gR}}a$8GlCm8L{4h-RO1OSQsJSQM1SiZIX#FQhtzQ#tZ zuYms~5XCPmp5IE~L!0l2{vGv;&u#aew|k=0vN5%l)-F=1J7B+qe5mm4S9N_HIKH6_ z@mn7qL@dMfLt416BJu1X71ss7-SzMF_YanfvE2&#i3vy>UhV=Y20Ty*OA(BP*>$9} zWtxq2OXz%}`b11XkPtg27`-QuS}j)VgP_~Ty(fJce}+X`$}YDIRmGQuJeTFXPul)X zy%*k$K+^C(&;IDf0%AR-U@S0xt(Uhqv4Ekgj~*y*2LIkD@fpKU`;7PR-}|>lXnrf% zNRbuKjseitM)Bg7He~&QVlLJd4NNa@KE6V9LoW)4>$Stq=%qDOP>pbq)*VM#YA04W%ug6dc7ZwReCV(P%!_R<{@7}pC2Bv(n z7o?=T{9b0JO!P%cEcLW+T?4Z zH?NM<*e{kfy;3_J|Npu=O(CO~h=IwC;<`3sPgx-dA>(;q-%bYoL1JK27^?Ojex25<> zHKYlX6e_rXEk~$=Bl079cDy5uZ^4emmoMo!;|jF0u7?N33jAD8!@}q(ESvUSxpxXq zR-_=6!%o~WmN59=T)hlADgV6~2xnr}K{S#^eYb*^mh^}2l=S<9;77cY6ZyLv*%lSi zBq_YC*XS(bIFowy3e;qb%Y5CDB8^k6cI`@TgHKOwxGH-3G7xNUn|;JEc|czi2enyIRsn{Ust0q1sJq-Nty z6)ULuG7h4XP?+O6s5`E$dO-FazkaQ7ZC!mvMrOh)!1v|LOn zNUE)=>Dsv)&M;!chbJpQH1QV1X<|O_Stb*2IM~rWgyjRqNbvKwz}3O+$XGEAwF*+a zMMJe9KmRXVDGAIL;L^u%+vA=H!a+vq8{h}V*LpEZKeZSLfP(J5b!D=gn}=F59?w=j ztslfga$qK&63ONvX3kEb`5)&^g`vXD`B&2yO7Pr>_wp-C%X5v_bF zsr735JIu~w038R_QYqlUi02l|HrxrMz?%v^>E|n!3Ao@%9<`PhJ6ex_**0AW6@J!x zKZ+Z0)AQZCqdE##-kWnkg~y@gx=C3^!zm_V@L^*~q%<+vf&+#-g=XufCC~t*i{N3LmqdAed+b^=xG}nX(U?+2nbZ8?_g$TCIUKxR=06y&FX{zS;82}}V$_H%Sp>(v6(@aN4vh`z@(@>Hs*fbP*P!g(~n5P(2F-v zlU#2|ze-g7)yQ@y9V8U7jDo5yT9=}#30U*iW>gbT^&_lpiUpoj%!Lv%J;c&F}oEolY+Km zbZ1D^%4Y*hbG-(gGlhwb4e)I=wYGKwwNf6n(~-{&6B)kVQ#821rb2dhc;$e==e07h z1Itl>DX$bte}BLE?b{zw)?dGI!wr3ffx;(Gy?{c&Z?p)}9qo5JNv9aqdWe7zyuH6*{sEITF?!)Uc7T1g4Tk8!3U2=te9+n^Cf;zR z!1{UvN$5gT5&R~BgCPI~9J_W=@a0!4h#3H(*}$K|4!06ox9&y6KKBP;CX;M$c-g>L zh`Ej&NVfMuy6*wn27n?B2b7=i3WQE8<64_{mu;hbsrrl)5g8Rlizx31-UL2(($xRL zxgDB03324bA|T&Kd?D6n%#%4&g`Vu6K>g&v6IbKxxE`edSMDjyP53?JYXU03UUp+{ z2nR(c`QE*Iw0n*LM1ya;_st0Zf3H2Wpy_ue^m7$r|Boc)%N80`+RxWA;f4~^^@|16 z;cnsKn*EFk!LSmz<3{lfH-Um1!-zy3`~(nGBuzmpL)zsN(H0qXsX44$W|)9*sm1)^Heh?3{Vc?dXL3*c(p&!3z}?|2#?{ z<=6U{$)xI)cxpEX{+){29gq-SLqL0F%4RTHzNpG2t!}RoJbKplK={#x>u3F76ug>h za)@!lvxLx&CSwN6;8>Q+@<}-G-eLN1l#!`v^7U|FOY<(uA{n+@fas$!-luM6&2hkXZum*E6KW zIHJ;$v}st+q<+8e%9wR$;D@dF{@@~-j+rp@=b)Urt?hd^)o81xUPOqeLCz2Z`NL8W zgpa17f8#R-HETCRHio4IyfFuSlIzP)pE~6~KZA_-@WY1>pRCD5)jj!WgyIil7lJUh zU(n3iSpsun?|tfiH@_PRYd0c*Uc)d@NlcGXEA`NX7WE|?BclviRAuM{BRkPo_ms;8 zHD>DfHbbA=kutM<8|YE)id1_p0CP{5_Vi0XU*89LdCqagaZypyNDRlj;cyIuHs&=HwzDvdqZ&IGSJmA7g7W<~%_e zC-qEBFbqensMZ)!4s&p>#PO3*o0)kGu+klMB55SVXJNehZrBK=2Q8Shie)r|tZmaz z$rI$;YRjIAlU7y&NV{)s$8I1g5@GngmlA35D zK>@|R4Y~B-<&8h7vuRbzr>E@DGw{4!suJ`QgI zesRr-+f6mFK8=5}qC+20g+_1YL4eX^D&%CK{% z`mdeDjSRHS^G!qbnf}musRxp_zg=e!p@tmPNy$8r;D-PQ5TJC!rL3f6Ens4&`|nkc z3QIg;9b(yWYU)Kh6bf+In(3p6~Zr?K1YNA{PB695q=daAJkcndocQlg@C;%4`5Ubk-J#(ikCjz+?W2GE4P z$P@Dfo{jlVz)gcodP5f;$5JHp67-&(#fN){wi>XioNzAaqALrv-kxYoL{qB5gLUe1&GLj5uqILz;3gbKrccm@^LBJ^T|(x)tdt@6Jp6D z*48e-Td^0gViMn|FG?6+6hhBH8TsN<_=0Qug&lH~rzl!{foG5K!xXoVf)F6loN4wl z;&9E598CP!1zfPDV$b9_1ZFO9SlcW&$M1)*v=3azN(#DTu4asVk4t8|51HgwMRcA; z_oMGQ$V@9KSMnT>?>~4@cS`@}P5z^881uY~o5yJc7L(&u;vpQ;9t;%DUAtVz0-x?S zQG&`R4`=j4{Ou^M>8|8Ei_bz-B)&}iNABU#+hAWx@f}J~my`hBl;S&`g?U&%&;ebM z@`vGgLC>)_T$-MsciiEK-ptI*Onkp-cZSu+hnkOOSs!>**J*-a(*?$PZx570E`$_O z;&#HpB1T74g3nM*K7%E5g)91A{V^#;(ruhrg<**K+}IdIl85D?bsSxoXHWk3%-ozW z2BJ?t_DM>M+xX6RYX^=Gu?et@lA0yWe$tS5d(l}LcLU9NmEYGC6&2<5Dr|@I4Xqq< zD4iYx;n18Iz%D*Cx}So-^dNWPl0lTHU-WxUB@K;lzG&;I zfy<-@tx>PwU4&343(HvvqKO^b33rAXunH?&;hB&hv24a;z<-C5+}LwI3uD-J@awMS zJ9dl<=8?}|zj`C3dcYj%OWB2`sk`A5VR?b>^DPuzLUTN0Dk}#%K5wG6j)PM~BpCZ_ zgQ5I*kVlMQ@N0Bb9+I;hMsA#^JS8W~%WK2QS;Sv*yuhoG69omBgPe1$&*T&k@b8_K zT}zH|7n&`1qeSAu#6~o4cnSb65L+Y)7x>+uuNU0}8vqNKb_@f9so*&Sgz{g*!xv54 z^Dbgy+eO!3ee9?r+{OW#$awc?n0Y>9=t5tA|1*SMKVWq~aEL*DZ~el<!b+9Q|I$a3@LL2_b1vk{B>N($KbXp|4S=n)DXT_IHC za0`9H7V}dm>n?;>R9wjF%@Dv%et4F1cXoERF5bGE9R?siH`{L4_82jj3)p%$ko76f z&HqrM0UmWxgT~~Mw#`yBNys9W$O6zl%*r}GHaWSMS1V7q#TNc}`_qa32bOAp|>;J&y+LL<=aR<$02d2a0&%TlQm#+0s z2^%Tpcr0)U^lcE;Te*IV7Bw5I&((>|nRVyjKabJRN6$#u|yxi5*6#+2` z&vrFzD7WzeH|yJ}?gcAegz1auB7Ou3M=zo9i1UIiXdcEBW5y$BC%ow$pK=%G5~8TmAGJ3Iu;V9mzr(y-{I^4~rh6#(B^R6~D57 zI`GlCnAJs>L^8UC_tytOVe7=Nf|u(#wI9ZVPY6FpAUfmlt2{gmFh>S-z>V6(N8 zQQH@zM7(|kuzAL;z@FsIXhwbuFuH;125Xnqv$s2E(&{wY&@Qi_iGg;ui4ilNBap_52 zhz67n6!8zdymZHbmIBZ+8z6^%s#Lx5?UxnUE@L{x*H~)$?B9p;m^Ar{Sg(ggwE$Y# zagG8oVx-!Eg6U%2F8!Z&=YN2I>!VYXJWuBB0OuBO5ZGUAf zc)$x49|!g^R9FCWNL{~veXqf(S+d&H1IC7392_)Y`hftH46aF?8_Sl|s{OqB3R?7^ zwV1)v027v=*e_nf;-=D^@~klmRVEJQ>^$V10~6m{9)OWXnFaLJEhRQDzh4h~{; z!JU5rHuESkjo_KzN42^+Bls4Ro3Qv2r3*DQPY3$8}{KB@>B z(sgwkF4FD;(Yn}poBw9hv|xIz)_8b)|FDrsaee(ZbU*J8LZmzd9LuA&XMKzXJPD*{ z6UdSQ#3U36%N8>@VH{!fCOzvIWO3~W#9xjqIeGqkLdCmVF%XoMQ-%D;p)+Al&PGrOYfzq;TU(z-K?_;JKWS&o z&Tn9x(UiBH@Eo?b!k|dUz#SqWenP)53|^Rr5|&mUtw5n*JkbnD7*gyL_JZ^n%7u$P z1ft>)6=g>CR|woeq2RH5;+&9i2*B9n?O$;?XWfa?mW)<`a!1A_=_N#2y^%Q_0hI`b zTZ_^f^Gv}INQr^13VofON*`MkxVL$p+D#f2|6yZ$0{(|+ee9CXU0rJdQY~4qs(Pqty z_;t_@K}_+2C|XOwjJNg9%ZoN5wPCwS;|z9FxWPZCY$%SL|K55~4nh~ki2weOOd+32~N zM^gJm-5H2=3PUq6?f@=Re#FJ9@Yo)k%aSFMvP)~F|(QJIVZnyd| zBTa{4L2p)370KCB`9Gi_;*k{Q zUM|nF|G>6H$D@n}_mL+J(S3r23_hH)7~>vBmeeJHDCNLly-*3cSS2KC-7rFqfczX0RBnrTRVgGHIiLpwsmNy7so)mil7w~ z5ux5Dczy*yUHt$ixA^cw{oPG(LDeeg%a^ZQi38Pe3q*uH*7+r-fvZb*D?|pPXFB5g zn*kreqKIj@`CajP+FVK6$?g8pU~hP~<_x8k*P%SE{r>l3eyw~Eq2P;a^9rlV}V$(pa-F3Z5p(A1;*xc#3vQEaewWB}8k@{m8Hv=#?c$&14# zLJNE=DIc-%2N)KFaFAG9WW9RZGm`E)$sLrO$dXoT1jEMgsx zA2Ijz!oXVhGT_A|vJ)cssQwd}j1S0WV^0rx%afR+@HvUrmAYr&zB6Rq=OiYaH@2uK zDRnZknKq@KG|kjNT&}?ZF~*$LQ*S`ys+jjJHlt<(w0b#meg%aR`;hMpBy{SD>iMH; zySQZ`T#`GqZ7mjcya9T4cjJ22I!q0OYAD^b8PgG|a8M~YIpuQ3A$)&MI(}PUSD|m) zmMu|OcdTIZ{kA7|TIBcpxGY4WK1$Ls%{q<6qfgNxln7YDgYv2F;wrgynXO4SZFVv) z*g=*KZ@6r+gkxTZ2eTCl*}+$j53e_&rP*}>o#_WK^a^EK0~IcbKD@sMFudv7g{udt zF3Zk%i$;Al;A&b`R8>?04~YTAXJXhv687EHI$c5d61Ed#1HIpDzs7!fJLU%}R9hgx z0582;T8$CbDJXL4ptq!MS_Lb@AIzgAi`PPc_$|sRk3xwTN0^FDbf!?Mrvm)_VKtTW zjfNJ_klFJ&?AysmRgk`aus`Ag?qUk|!5$LFdIW6qs_yiWOvc&|MH1i0+LS>A-^=6` zgwTbi0z(*qB(ldqhe=pU&WTKkydO;dV1@O9+yGHzO8#Xs)6B6tMj8qb1iN6TmVV zHUl-uOp{g|1EGFUJQo9PM8YsKSqXfJ|FdV>-a^oR;^1kILl?xr_pEp5OPCSo?Z=Ly zYA8N`@X>FfY9=7^O~^_Lg<*T;qloJAZj1ix!Q|SEM$H6x$F5>?k{9N*rGWP~L{k9h zuWGPwSVwp47Ebg+r1!fr7~cM1-i{%@ds$Ug5Z@W!I(Rox2s&kBO>6)ry!}#HK?$zK zm$+t8x;MMa_q=f{c%ejm@Yz)lV0TLQ*pDgwmoN=bD-07`3A)9g^~6&qgmvDZKLZIVhA66Aa_rt=xylZ4?6yKJ6ogfg`+i`^EBUBzLN=_lB2e?{dj&A9-!S^> zvZ^Xqe-ncZ09Yg9!6XdjFT>L))vJMYXTLgevoe3O0)woc@0^R0IG(&pb_M%hexl?f zZbYGJdZwQFpMS8|Hqn*i1`79je3)7bCq%4=Vr)L>)nGib;rm-1&#BS&c4OAVLFe&c zGMw}83^rT@)1rteL6z8Q*}7~598e`OpNK!9{{yFkHrc^}v!SsO9aCs-Ien2@*kR6% zo^6AteIFdDPNj8BQE6%X`NMdGzR?(TD}}&;wU{BWrdVf(tig?Wef$FRta>2d87=48 z7>rR$LiT80D!{o!)@APShavup zp~9CWXFhK72d4ZL7_N!lw>spDL=cM_6MQR#Z)4NJH>6xnS$wuf7*2TmarI6(TIS%_ zGGRBsr5X7i2d~e~L5#Xm#L%2NbQ&N*>1~ zv1$_&p4x|Q!@Zmh;BJkP`3JC7;&QLKNEm<75g86_M%2!;+)6}rU}({i>`M2lk;*c* z`*(#|HV$gcVfSD0;tXC0W9GzO{V-C!hgahZ1lTU#8ump-|lK32oUrM_T_d z?o|K{pqEut1~9X|1}siGhGSt`mj-q~xoO&VtU4_tGniI>xE32uV=%)G;=86J#l!Cx zKpk1%M4m0#u0?KrMpeI+A-LF`;`ZsNsecSf-ltGj{<0EDVlczMcAiJUnSlZ$f0Jr1 zd+PRW+pb85PL0rZ!6vx%!C7b|08^I8o$%GxzBs)0ay3DTF*JfH6j zvMr70tqJ-qxt6r7$1P3~h=tlYM9j>}Dt+6|xz!Ky%G<0x#8$adKOS2!Vxb6WN}B(4 z6=Rc67yxs*Y3B+s3kUY>k;ZJ0*uH^EHXJvwBYVrC-j;4JxXu$78}Q zj#YALHXPw0!;yWgQ+54T=M3$9J{7?m@W*9&+GVb_eTfXozJ*r?1SguDjR5}XvRce_ z)O9=2;fm1}i6nfH;Tf2286ht*dkS5EczgBW_=(>T*yK7hMqQIl$e&`dd8DF?ZJ(8^?%aq` zcUF~Lqn`O7J~Qo|>KYp2Q&Sbt&-|78k@>)Cx~=ddk-B1VJ1eVGO)@)$A(Zb7&fWnY zo_D7BGt+d7IpxJ$mSl_V6XvJnHVG_PpEHBftg8*FuiMz36qhsUIE5d@Uid}SVT~9{JR=2|cY@CSvO-f+ zR&RI$rhXYfP8hcp!xXYGn3-Q;U=3z|55fb4%}CEM7oW3VZKf6sKRt`E0-k`foZJ&^ ztdiQhZ=c7^IrDhLR}-|dV6|HSwnXtjPCiHeJLG%g;@Q=I>o9`p@qj$v)FX3=Au=Od ztH9t_t%EFv7kAIub?Ae0PUI`AL{BLKTzq|R&@nifc~9d{Y2|#OG@5e`?R;P^F zM>tJRP8M5BwEs@YV(TeBdTOs68Genxf8glRccq==hnAHE4@bVoP!}ZO^S%(U)y0Nx z898Et969-uVWW;Rf065j=LL*52t&aP4JRXcV{7AZQ4(C1mk3J7&mmU|Ps0dP`bq3n zzIrtg%%TyB*kEx7YP3#!(df4*PXJ8;jB7-xN1zDm-!1J|MayB|GC+Etn;T3?QZPqN z(eTtL4@^xRloO33WHlQ@`c8^#wWUay%8^Fp2=0u-${4+K`P#Lx%hc>_b$sPR8@K10 z7{M&#N27!K8-cCY<}vyiM~_qVS0c=Vkw`$V^=N}~6EN~?eCsU|E>co%n9#FsnZdud zUm1H_U?C4?ol1|7FXKJsi1k!J*dBf@uOSJaLB|F9o&5fk?C}Y$%f@49+3fadSEyX9 zU`iFF3{F`T6)3I~(%}{v)JMQ0bt0S&7$j<^GK;K$Z&i+f?qHKwx?KtdE)p$@lJO5< zuFHi|B2ARaBtJs%-;M=&wHaR=MQyT zWFh6~uJS}}NkjYPwW+%|1#bek1KZkgMK`Y6FdzH@Sz`5qR!j!!+u)%9)}ivP8x|k> z?Z=Q7QrJNcO*BD@t(4ug-ig5c1&P%Z%38OG)NLRmfr}$SM4uO#>Zk|zYuwQR-VEokj4vVZhd@qvr2m%L{fGSk)2cGN>w!xJ+RlrUqJIfMrwNWS300Cxz zN(l%GT62h*otS|D0{!gDq;Js7dUxiZQS2!S;hd)5x;Li0) zx5ddQs1YOctgFr)+KP(4Xtv&lpM-u#6KnZ?RcF7rQd_Qm9~w~ zS_rc=Fz!&_GDqyv%lj(N!j&i*GYNEb1!*ky4-3MgK9Ad{EkDhL6CXB!W;8Ov8o@dg z4e6+Q2YX?r%D@H$RJ8`^Sp&?qED*`OrxRwvO}=cj(5D7at+gisa4d9gpcGuGM71t1ehtAs>3}LsgH{Il zfo&`9dl$Dx@nOU1gU>xw!jV{Ow=RW&<<@pJTN%8SlMysgEeNo;2_SNI-Germ~M;4 zPGLA~4q!O@;h@22UePeQ6<@bp_qqhH6r!54mR1V1hluiv!$$EJzbz6xDB3|(@<)8|ygAK>8Ns2;orFuvF761}M z-EgO1*4B#;Z+Mb!lg)5T48~Bg3Y(w6`HD|S@Z)3^w^s(BMvNCprh-aUi`0gtWm5Zb zdH1-6Y0k))ZKBIkF8cui&FMwg0_Ke>m?zXZ7FzApSC7PpdHM+`wy8&|9m*>zbT@u{ zr;MU+--YK412-uqMIfl8eyrS;`2Ol)7|Xf$?(L4QQ&m;fAMCHIVY_s9YSsGdIW8Kj zJHq4ugraA{-Y}qQK zVC!Kn%z{Iugm?-Xj+RZw>yn;c3~*AS=AFByZFWa;a{sKLf7!WM9h;v1M_EhV55}J) zHXi_}a~Ccs8yd#fOV>C|Y|IS~4^M{if`}r~!$qaN^Uw3JoQK1BUA55hFgb~?m6cM^ z4)GR5^u+fd?rE8&^w`^E^azhW*pbwK_l}rlmRjV=`ihM6rRuE z&2_~XF*>UBwsJ0Wr3iOa~zbwEWA-|~D_K8;L1H=PS z1rP7&`@ja^l%A>G8U6spB!H00+CJvBkO~ioh|Fzm(-98&4yKoYUpVF7gse!H1%2aK zy26zmnX{=$?F;;G+55Nq6kC2Tyt-Q5^kUTqrTVCp+h&Eem=ri|Ho1j$KydeNH@iGa zsIs>wt2Zc1^n(!-@7EVcWeA*v`NtYuiTfMib&3h|No^*4)zlSdeqIRh^?e1}g}?4A zeubmQN@N==>sQ7y%B~BJqYmx0V6y9=!_c!!TqT^K@@}*I#XAo)NU=Kg&)-FkkR&+Hi+5pbn-=FBnqD3%JY`;`GtRhI6`}ZHLQJ6gg zds8E}7=ht2>(84Xp|kd@R72Fx2n;%S#p2T>AxG>{C#S=3b}>xkG5~ezFy?klu6s#) z2RmJTNqv+5RKZtb;ddcJ`dWQo50BMwH(lK5NlFA#Z_7#+zg%9>2O)$1`+-&CT)9hu z==e@gf*vo*Ib6{H-|AD))ID*r;b}?wdV2TY?9~r~q#J6^s^vI#-y}CXyM-Da7UuM+ zw_v$*%vM0JY-_7fZ0&v=TTDIt0iv11a7X1Pz{{ z^Z9<7%VU^VNN2spJow+upm+|cR{)YThAgrK&@;(E-RB4gPsPq6WncJJ5?~UDL;OdS zr~#T1z|CWzrVd~Ea3?8=s{=|uO8M@81AYv;lTxjPw*XAH+T)AK&mH&TYp~GO?UGGl zH&}1^-}$Aa0vf%Ej{d9;z11gFZh2`n7W%qi>iBL_jxlQ5Mw;e~o%w>H2uWy>Rwl*h z0pLWJ#X&7~xc>nKt*|AHYI_)c00*El%44|F)54BwKx;v`0kW>%l^hVm9ws2kI+*&h zt{(m$eG{bR90&gdM(WFwMX8S~7qJ_yAMyjqXBY(EWIURO((z4B&Q(|z0zrWTNl$io zT3lG&^FJ>D`v6M+TFgf#a@DQR5B_NwBSmh~0_0F$S)!(pGmOJ~%;5Xf4BHYDQA)BKHxp0BfYVf9eb+;Jc!;?b6AUcL83pohcIZfPjXa$o6zJ zF`U4)QUXY`s=usC_)3*rU$@m3z!&gEAvlu(5FCWd-(J;jhmk9}xCeyIgESow5|RL% zEy#T4FxC`<|5CKG%L0hB#^e86rSL;}0a4lrpLa3}elP==Or?3qxkCL@=NPFQh%AP- z-Uyt>L2$7~;~7I^wpKq2Xb@!aiDdET=dd?A3apYc(AZ8%=NwoS+yc>Ql!_!>kJpEU zGWgov4ys9sqrd~k8hzIoTmyw*i z^f=rHsuU46h*kg!x*sx;F?B=Bk^f>^s5S~GFU|=RR^jH?HhLDG7hPt{wJzYZ4l~IX^;-pFyxQ}*Is1GO-n^L`gYQvWV03YpY98hQ+ytUK z*?L(`%>X|+vAA}+$0Dl&9JI-I)6Ap`I32^!&mYeEoRY|C3EpEqU~=-iW!0*j9-zY# zssw^ULIr5bq+!IKo7fjYQ7-#`!XhReRI3+p;4^+qOV~rsNE(JI@--qJ zP1WtMf{jc%UT~;Ak-d0vvXz~YQ6JTFm(35;%+`gTWBGifpc%Y)_UsWPpmp~7J9H{w zC%lN0rPT~zO{OFoIiYx@THtlwL>pC6x5X+k8^~+>d3cP*zhAT)$?9FvFzE1MajMmu zuc)iHHv|(rR*4mLm_H{E1x#o}FF7`3S7!AD7JhumnfdkDx!W1nKZM{xJ%k`_=&OCI zuc_1q>?kY6B)wJQWZ0$8yz|kb=im(?R}~RN?m7uxs}VCR_gufy-dlqRVNzu>nuLn* zCIXbR9&DthtbT!k+kZ=VKX`Bi7x_XpwXv@5Ihw#$JF?$`=fMt#-MH9?<#<|iZz+C& zH(#+PFI?~MhpmSk9{czl!?d-uq$Nlu%(1o2L&vdWSvw#uDM_$V)4`-*z8&8ScjyJc zkZa@Uoey83r;MmJ;NQ~m=2TOY(6iCE$TUJoxcOV~tZ^wf0%|8^V^pP_BAtfk|1z^a zi`%E_vBlp%@F#KAjoRg*Ox={M9zk8F)fb2xZ+yI8?Rh4~P2~$YLAarzy-mY}A^du58a3 zggwOU+ilRl)au5;XEJQJ#+-iA7k@XkslJ{yHmHY+n>sOx%Hnjdh$foi)-t87vKLz( z56YhjFza_h*;;{W)>se3Z`-f&HuLYQ*47zl%P^x3>G+5@3{36RsW{#nTKh^TH^l_U zRnSP~ahYvl$@sVhUS`A47-i;TRc-?E}!y)qsIJvLXL;;h#y1Uu3q2=NS)#}6JLH5q(6zT!6O+pI9TSon+g z%FeYfFKV^kh8QVRFplWy{J3&U-<;zr`ynibFx22Ze0VD17(aiF+018z#=6n0G02tD z=*G!ktlVMTRF0BWy&RdU4$m;e#0)5Y1aH-KQm_|0AFsSCzVQ6WNko=c?NcXA;*yi6 zLL~VmCAEAc=H6)8c^3X-&X$dO@2tp_FxHfPO1aQkoE{l@5@yHrhGqOs26($)uhu`b zLwqrr=(iIz4dFY3=`3&QgcY>_DxPr?+klx?#2jW*B)8*>_(lU*2x{bW5|-H4YPLH_qiRdaiAF6= z&168#M(QwxIHHCvw5<6*pz`!Dl%q0S$7-2|ae3$QEv;{HDlg-G0Q?h~wslSA-=`$; z)XW%S7jj4~o>d)~Kcjiv=|8nEm+I7)yLnyW$R!eoFh_C-%4t-tB%wFEeE^#%VArN$ zmU}rPBO^X3sUGreLSLVOQ9Jg48;VYaJB++dk`EEOh{PZQm;oDXZI|5&yS_-zI%JWS z79#ay=9N>!!UL?-OSZmx>XbV~j@K%UysUK12)>sgKoB(JJdg9R1e1ok&yQ`YSG9+U zbO?W7fxXN0hxRz^!^U3$qMqomCesd`5tiVHsgp8nc7(dXF9eI)Bx^0UWfPcA&PfX! zD0=wY%7^bHBqz(E6d{Fw1~y{O9@-e?NJw~Mlt|8-Gi;qg7aQT-k^f-q4~Ht5=!ExP z3#u!LiijzVeRHW(Xs~FW$}tlgZLL?;O2UK!$knL1;((B_MJ*rBO+xR0fQrWfLY0tn z8QIMckECIu2VNXNu(T-`?1`Gg+vF_WZit3sLqr$;;xpU)`OW~ks{k08XjU{3;y4gi z%?!Z6dWd>dstWY7Y-qbliv%UWq4RRz;2;nqnV?KQ0)qeg${3w#eL_QZ>wgW_6KC^& zIVvwU=AMxQ0Ui`zg}Tp^Ck?n?rnx@f@3fjUNa%Q(W_53ut%2P|5Hk-na>=m{=fZfi zI=C2y8hzg2>XVFHx4r>o21jEpc*R7D zk}tp#uKQwn_7b|YT6^>zp&cg8o55s4Z45&}F1b7_Pd+n#45WRdUz%q^7=4|+{w+dI zY+e%n+7wN@CoEK&y zrX7IehMQ1ZsLk)(BtBQhld@7=CPGLkT;m%WR~jE}<1wDk2M=!44zJ)vw1%0SZGe{g z4h-r$=q&&^>1fW0&s*eI z=9+~Z1vts);B?xyeS7dkrDzqlaMlqshz`_2GhkT5aF-=J4ZG0=TfBNIhX{sw*1Vb$ zNH)WHaxV4{3Thpg+W3wj2=!L@jGy$57(7Fd#B*Z7X~=FJ7D8!u1UCF*kVmLgY*cP8 zo72>|aKWqHJU`FULC9G`h|S&#VZqsCg7t7F81jZG*cvKTreKDh<_UT6;?}T@Fw5(- zv>GOz3b1M>dM_PDvRtRjmE^_s@Pe*~#MVzwlCFV-jj^@2@a;GWNGq62fsUmiiX|ca zHWjFs4Mcllf;9PyW;R8N88sZ#$O1U_U^7|e!M5t(S6_vaux`Yyv^?Ff3Cw z(Ai`%i50i1G*#koU+0^&Y18g)_7Gb{ooy;=O1`!pEj`bU3rgaw#2MDoV1{kU+ZzW5 zfe*sk$s+kH)fLv*6*VBHXNYO){4(W6RuSqh+( zNTGufgD41y6OTxJaN-z?wuyf4#*}fR!LWHWYGw90B15Mpp+Tm4bPn@*^aekEz<>r> zMex8$CP=*G4<<4GC`|(ZS_F`&1I|iDt%vH5)Z&aj#?@SkA_8rs$!(Aw*gG9n^0*pv#igu4IBs*isTzJ7l%p`8qu~*c&)v@>X1v$B?)^=OdP z`0?hQ>uZVMXDCqq7rUc%sF5m3| zW{%5$5i>ssR^Mn$-)$O6L3_t7u-X`g0NIQ>0;VOxQj2;9f(&s6F>e;eiihBcd1AxM zH-T4!I*HnT%|T}9wZEOP*rZ5(ir(MJ^TH%+PpxX_>eg3LlcfcIZh=vt5 z&^h2+xBSX>O8MA1mfn}q5%#X;m;n8qO2oRERrPB({o(%a*+(EGS5YTD+3ZWyC2W63 zWDJ3opSV6d9+vVo(#p$Xi|{&q5Fs>O&V6LZD{H5!KRoKN%IfC7ug z2e?N6ajQQ@Nt-)>&OWrEw4y>8PyF$q34J@mUZ&B>^EL2Vwf%ZptqPd-_tAf1=WS;hv-#s*2c8Dp1&b zl#qxB!K2}{5zCLG(7(g3D#|=DJiOCubmCi(8D*s0hV6t_-d(HIX_`^TNX@)aheKn? z1XN*8F*a}M$v{kboaPyvml#eE9Mw0__&>_951lEQfsm?A#1|ZCS#it;yL$&=$ZD_v z!CatYh+&7O)5C}Mu+sIq)qjI({EzvY@`iEx-Hs0GbB1Cod&X=LCrqY$VS|u+U+IV8 zKj-piznUdZqSZ+p^cYl`cR*TtUf+0~#~!=tIo-RgbD{b-Z}rsmlk!R+(t1>aZpFbq z-Xayqll)%lrQ`TRAOk%wIQ%v?-E5CMx%Wz8nh=))Z&nYMm|Q8(UmP~efWtyhUw@6U zD$dg3?7mkKC!HCkT^xyr2Z+U692^OpsUERDZXl*Vn__qKmRhyz~Qf(nB3g8mc%l2sEerX zCN7c5J1xBjggPe|zdeX&|&aIL2hx zw~3DHR(Lng*t21)_?F}eUG!f(IaC0dd-?A}c(Sg+A^vt%G|aPe^pXQ(wkxKdqEdVp z{us`Tw;uHywzX?3qj!>UXRS+w>k;C5soJS6*qEfzdrVSy7wVu!z*nArwN2HpJw`U% zggXagF#Lg*1z;S=MMNVu=ftOZ0Gs4SOvXws#?@QrV}W{u1u`eBFxqq-`0YD_7E4$#J!}Mc?glb^P;9hBnYhEb}L7n3`7; zvz1X;P4I2YKsRDf3_P8hq&$RdYn&2;bjzk1mZ!D)?D_K(gEX@Q+6>=jIS2o8a>i9` zn=;>6PwE@0^=RmQxR9vBckNFfbI!eLU4NN63BnSXd!>mMVPm?l7beZ+9fKwGn8CHX zSCLd2#!;*+^sET5x>~p)%4FC7g4L(*#q2J|hX9`E?n}Xmmq|}Mhm`Tdtm$h}p=2A7 z?a3;P&=A&mPFC^3y-=n;+Wan7@K3CnxgwT@rmI$BW7nbB+8tKH6%E1sdXG%r=e=C$ z;sWYVYe0qU{OM$*882_C1eDeWCw6w(Y+$r|fx{5~l4Sbv6q4Z~tx|7KzvYJ_ zb!X}j2$!U8;#!82ZIkU)Y~upEC_kHHnAf2W1E@(mz_FW0Bbf5EuiY3O@ZGh;zqs?8 zN#>8&T1(?WzR`c{JQpIw0NeK*_`+&r0GKv?w^xIa_&P|2jLS{Mo!Vh!2T@~GM$m^?6yDek}z_MMI8>FtH>#eKv?i? z8;8{LKSe1a?;}3-JX+^t1#zFj@dI*IBPG3vc%R`KyAnCnzBF}=h%0FFB8#k);Z}g< zTO;xedK$oNMPi&mSV{NldhmS2FjMpP`t&27+AK5)&#yRy-!@5KS5(Zc6O5j6l(1T1&} zeEM6gHr6Q21ED74Dt*u1B!TID`0EUD`p8ek#lJBqE&|-FF^dnv)8{mrJ8e~Yyq8-@ zNCiU9Cb8eH#3tSPgdGW))R0g+{$G3N8r0;K#&K407!h?xtc&YHYKtI3lz=8QauIJd zD-o;+N&rCwso{PpkV`uha226}Pz-2A6aj(# zA4#0o+sQ2Qu*H&3WJVoacG|zyEU{BE@NdCMMrMaq-&lUx~*e?No&iiWd1G zA#VdmrBd05LI%WV2Y3*4jBewzJDkM`ca%S)NVnphfn;?_E808x~we< zQa5)RIM%YC4IVJDy6@fH;gn?1aA6_=1#J-zxjn~=zGhARDhBEi?m1#qKD%RtKg?%z z-tA2&3u^ejmIdw}io?aIVMlnqo+x@|jihNuG`Jifrxw({SLZ{0`k0GiZ+>r%;JR5wlekXbZ0Kj(Gmjtdw#;qaXfZ=8;jKdp(v!vjIIo~8EM+- z(v~>JO-A8s0Lb=}4*Re!+m6&P*g2n(dKOR7GYzupWzAZnkF1%@c295{awMUUjPDIf zR+!n_cX|52Xd>`VCieq9+8^y>Dw$HCXSr44q0j4(+&38+%9Ge21{^9Ix|_ z6S^ZMr7*lbLlm*4@7mMb2(rvkMnr60duAI>jw777-RK0?9 zB-;qV2_3RO#-?M=nB;5G?dZXv1eVBPJiYz^gM(1}yO-}j5$&*$@YQZ!gh$|8Z!eMKgWVpsWq@NI` z5zHJD8m++=`_u4a=#L^^U|!R%Ack)R3gUQ9e4zjdD~gI_h{m<;j?t=Z5z^oY-ACrh zaG%XKYJ|;I5pE+x@H6{)Q2b!YEoV?{pxRDsLKKMX z+p)gwBLrte)B;QQwADXlj$J5HQYDf`D^f#jr-6*Pf+i2vfOFr!qi4u$LZY_bcWj}< z%e&~(Bm)DGiTps{&@cqsB>1AuMHs?^8`^_C{m@HL@DD`IJOGnXvb4_%nt}-)YcGVG zf!K4CA3!GZD2SqGvAm>bW(JXq5E*%S07#<*E8Zlp#;_Q*EBNCK(%~f`*ALg|PF)?H z-2ct0ArN-fgk%?x?az{`dMrJ>W4^>4vtYM)bSw@>Zu8km@{rT}fxF0Hs+@f$0jgQD zF}UjfOvJc4w)uH^l}JaUzak+N7hSe{p zXD?MNmi}LiMayLSg!sB=|D8ha>#*H4Neq07BzSov1;CjDN#O-#BHA4SZ})N zq%FC+%5SBp1>up8zgHh2g2OWk$;xVAzfR3^`FQR8czOruG$QEZxeh`R3Kocx=cppz zhx6u@%`gORXAjU?E78zefXsQ{GZA`akPYi&J7WZUu53jtl4=Y(q_xZ3B}1XwtHxcr zi(*miDugBILQxTD?~_LS7d@^FvtUSqELg}NBg@1LbuWM8#(`GL6r$|HiTO<|szD?z z0%S+nd7P!^t^9%l{YA4j9Dgk|_+7wZ+tEDT%ppb0SUzDk2E@GJzk zylHgTm1czV0)P;T^7<~|DXt7kC966tKz+1=CQKH2FtZFm8Ez&~vMWQU;Cm8$QeJlJ z5cEJ#Tuvn)1XlOf{E{2$B`s%i%BY4Ro<;$MGs)WALdk-DMOTLUlG+#=eL6@(BR2C_82BWl0qqk1i?GhG?8X1>eM1a5)vnw9p73X|jA>_Y%&#b>b3)na}9sP>L- z=FkCkweZ|&(EQX1{&5i4cS+wWX`e28zf+2O(6S>?l@Wpud(!X@EPg61VZ|U)6MH-V z<3uA937)QbZFkU&Io|o-fV^MxZGR*C_x7XS4xr{v{RI-L)jiFvU-0Uw&>rI~jQm(z z?}YjxFjz!rSG+8Z)eNfdhic@nGF3ezA6~AR-8X%Wegfo`xLG|vzdg{*Wb<~6-rRk( a^2Ex0;grjlu*;K{7HiA)%@;TN9{D#IL)g6l literal 0 HcmV?d00001 diff --git a/projects/opendr_ws_2/src/opendr_bridge/opendr_bridge/__init__.py b/projects/opendr_ws_2/src/opendr_bridge/opendr_bridge/__init__.py new file mode 100644 index 0000000000..06c41996d7 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_bridge/opendr_bridge/__init__.py @@ -0,0 +1,3 @@ +from opendr_bridge.bridge import ROS2Bridge + +__all__ = ['ROS2Bridge', ] diff --git a/projects/opendr_ws_2/src/opendr_bridge/opendr_bridge/bridge.py b/projects/opendr_ws_2/src/opendr_bridge/opendr_bridge/bridge.py new file mode 100644 index 0000000000..3deb3f8207 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_bridge/opendr_bridge/bridge.py @@ -0,0 +1,629 @@ +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import numpy as np +from opendr.engine.data import Image, PointCloud, Timeseries +from opendr.engine.target import ( + Pose, BoundingBox, BoundingBoxList, Category, + BoundingBox3D, BoundingBox3DList, TrackingAnnotation +) +from cv_bridge import CvBridge +from std_msgs.msg import String, ColorRGBA, Header +from sensor_msgs.msg import Image as ImageMsg, PointCloud as PointCloudMsg, ChannelFloat32 as ChannelFloat32Msg +from vision_msgs.msg import ( + Detection2DArray, Detection2D, BoundingBox2D, ObjectHypothesisWithPose, + Detection3D, Detection3DArray, BoundingBox3D as BoundingBox3DMsg, + Classification2D, ObjectHypothesis +) +from shape_msgs.msg import Mesh, MeshTriangle +from geometry_msgs.msg import ( + Pose2D, Point32 as Point32Msg, + Quaternion as QuaternionMsg, Pose as Pose3D, + Point +) +from opendr_interface.msg import OpenDRPose2D, OpenDRPose2DKeypoint, OpenDRPose3D, OpenDRPose3DKeypoint + + +class ROS2Bridge: + """ + This class provides an interface to convert OpenDR data types and targets into ROS2-compatible ones similar + to CvBridge. + For each data type X two methods are provided: + from_ros_X: which converts the ROS2 equivalent of X into OpenDR data type + to_ros_X: which converts the OpenDR data type into the ROS2 equivalent of X + """ + + def __init__(self): + self._cv_bridge = CvBridge() + + def to_ros_image(self, image: Image, encoding: str='passthrough') -> ImageMsg: + """ + Converts an OpenDR image into a ROS2 image message + :param image: OpenDR image to be converted + :type image: engine.data.Image + :param encoding: encoding to be used for the conversion (inherited from CvBridge) + :type encoding: str + :return: ROS2 image + :rtype: sensor_msgs.msg.Image + """ + # Convert from the OpenDR standard (CHW/RGB) to OpenCV standard (HWC/BGR) + message = self._cv_bridge.cv2_to_imgmsg(image.opencv(), encoding=encoding) + return message + + def from_ros_image(self, message: ImageMsg, encoding: str='passthrough') -> Image: + """ + Converts a ROS2 image message into an OpenDR image + :param message: ROS2 image to be converted + :type message: sensor_msgs.msg.Image + :param encoding: encoding to be used for the conversion (inherited from CvBridge) + :type encoding: str + :return: OpenDR image (RGB) + :rtype: engine.data.Image + """ + cv_image = self._cv_bridge.imgmsg_to_cv2(message, desired_encoding=encoding) + image = Image(np.asarray(cv_image, dtype=np.uint8)) + return image + + def to_ros_pose(self, pose: Pose): + """ + Converts an OpenDR Pose into a OpenDRPose2D msg that can carry the same information, i.e. a list of keypoints, + the pose detection confidence and the pose id. + Each keypoint is represented as an OpenDRPose2DKeypoint with x, y pixel position on input image with (0, 0) + being the top-left corner. + :param pose: OpenDR Pose to be converted to OpenDRPose2D + :type pose: engine.target.Pose + :return: ROS message with the pose + :rtype: opendr_interface.msg.OpenDRPose2D + """ + data = pose.data + # Setup ros pose + ros_pose = OpenDRPose2D() + ros_pose.pose_id = int(pose.id) + if pose.confidence: + ros_pose.conf = pose.confidence + + # Add keypoints to pose + for i in range(data.shape[0]): + ros_keypoint = OpenDRPose2DKeypoint() + ros_keypoint.kpt_name = pose.kpt_names[i] + ros_keypoint.x = int(data[i][0]) + ros_keypoint.y = int(data[i][1]) + # Add keypoint to pose + ros_pose.keypoint_list.append(ros_keypoint) + return ros_pose + + def from_ros_pose(self, ros_pose: OpenDRPose2D): + """ + Converts an OpenDRPose2D message into an OpenDR Pose. + :param ros_pose: the ROS pose to be converted + :type ros_pose: opendr_interface.msg.OpenDRPose2D + :return: an OpenDR Pose + :rtype: engine.target.Pose + """ + ros_keypoints = ros_pose.keypoint_list + keypoints = [] + pose_id, confidence = ros_pose.pose_id, ros_pose.conf + + for ros_keypoint in ros_keypoints: + keypoints.append(int(ros_keypoint.x)) + keypoints.append(int(ros_keypoint.y)) + data = np.asarray(keypoints).reshape((-1, 2)) + + pose = Pose(data, confidence) + pose.id = pose_id + return pose + + def to_ros_boxes(self, box_list): + """ + Converts an OpenDR BoundingBoxList into a Detection2DArray msg that can carry the same information. + Each bounding box is represented by its center coordinates as well as its width/height dimensions. + :param box_list: OpenDR bounding boxes to be converted + :type box_list: engine.target.BoundingBoxList + :return: ROS2 message with the bounding boxes + :rtype: vision_msgs.msg.Detection2DArray + """ + boxes = box_list.data + ros_boxes = Detection2DArray() + for idx, box in enumerate(boxes): + ros_box = Detection2D() + ros_box.bbox = BoundingBox2D() + ros_box.results.append(ObjectHypothesisWithPose()) + ros_box.bbox.center = Pose2D() + ros_box.bbox.center.x = box.left + box.width / 2. + ros_box.bbox.center.y = box.top + box.height / 2. + ros_box.bbox.size_x = float(box.width) + ros_box.bbox.size_y = float(box.height) + ros_box.results[0].id = str(box.name) + if box.confidence: + ros_box.results[0].score = float(box.confidence) + ros_boxes.detections.append(ros_box) + return ros_boxes + + def from_ros_boxes(self, ros_detections): + """ + Converts a ROS2 message with bounding boxes into an OpenDR BoundingBoxList + :param ros_detections: the boxes to be converted (represented as vision_msgs.msg.Detection2DArray) + :type ros_detections: vision_msgs.msg.Detection2DArray + :return: an OpenDR BoundingBoxList + :rtype: engine.target.BoundingBoxList + """ + ros_boxes = ros_detections.detections + bboxes = BoundingBoxList(boxes=[]) + + for idx, box in enumerate(ros_boxes): + width = box.bbox.size_x + height = box.bbox.size_y + left = box.bbox.center.x - width / 2. + top = box.bbox.center.y - height / 2. + _id = int(float(box.results[0].id.strip('][').split(', ')[0])) + bbox = BoundingBox(top=top, left=left, width=width, height=height, name=_id) + bboxes.data.append(bbox) + return bboxes + + def to_ros_bounding_box_list(self, bounding_box_list): + """ + Converts an OpenDR bounding_box_list into a Detection2DArray msg that can carry the same information + The object class is also embedded on each bounding box (stored in ObjectHypothesisWithPose). + :param bounding_box_list: OpenDR bounding_box_list to be converted + :type bounding_box_list: engine.target.BoundingBoxList + :return: ROS2 message with the bounding box list + :rtype: vision_msgs.msg.Detection2DArray + """ + detections = Detection2DArray() + for bounding_box in bounding_box_list: + detection = Detection2D() + detection.bbox = BoundingBox2D() + detection.results.append(ObjectHypothesisWithPose()) + detection.bbox.center = Pose2D() + detection.bbox.center.x = bounding_box.left + bounding_box.width / 2.0 + detection.bbox.center.y = bounding_box.top + bounding_box.height / 2.0 + detection.bbox.size_x = float(bounding_box.width) + detection.bbox.size_y = float(bounding_box.height) + detection.results[0].id = str(bounding_box.name) + detection.results[0].score = float(bounding_box.confidence) + detections.detections.append(detection) + return detections + + def from_ros_bounding_box_list(self, ros_detection_2d_array): + """ + Converts a ROS2 message with bounding box list payload into an OpenDR pose + :param ros_detection_2d_array: the bounding boxes to be converted (represented as + vision_msgs.msg.Detection2DArray) + :type ros_detection_2d_array: vision_msgs.msg.Detection2DArray + :return: an OpenDR bounding box list + :rtype: engine.target.BoundingBoxList + """ + detections = ros_detection_2d_array.detections + boxes = [] + + for detection in detections: + width = detection.bbox.size_x + height = detection.bbox.size_y + left = detection.bbox.center.x - width / 2.0 + top = detection.bbox.center.y - height / 2.0 + name = detection.results[0].id + score = detection.results[0].confidence + boxes.append(BoundingBox(name, left, top, width, height, score)) + bounding_box_list = BoundingBoxList(boxes) + return bounding_box_list + + def from_ros_single_tracking_annotation(self, ros_detection_box): + """ + Converts a pair of ROS messages with bounding boxes and tracking ids into an OpenDR TrackingAnnotationList + :param ros_detection_box: The boxes to be converted. + :type ros_detection_box: vision_msgs.msg.Detection2D + :return: An OpenDR TrackingAnnotationList + :rtype: engine.target.TrackingAnnotationList + """ + width = ros_detection_box.bbox.size_x + height = ros_detection_box.bbox.size_y + left = ros_detection_box.bbox.center.x - width / 2. + top = ros_detection_box.bbox.center.y - height / 2. + id = 0 + bbox = TrackingAnnotation( + name=id, + left=left, + top=top, + width=width, + height=height, + id=0, + frame=-1 + ) + return bbox + + def to_ros_single_tracking_annotation(self, tracking_annotation): + """ + Converts a pair of ROS messages with bounding boxes and tracking ids into an OpenDR TrackingAnnotationList + :param tracking_annotation: The box to be converted. + :type tracking_annotation: engine.target.TrackingAnnotation + :return: A ROS vision_msgs.msg.Detection2D + :rtype: vision_msgs.msg.Detection2D + """ + ros_box = Detection2D() + ros_box.bbox = BoundingBox2D() + ros_box.results.append(ObjectHypothesisWithPose()) + ros_box.bbox.center = Pose2D() + ros_box.bbox.center.x = tracking_annotation.left + tracking_annotation.width / 2.0 + ros_box.bbox.center.y = tracking_annotation.top + tracking_annotation.height / 2.0 + ros_box.bbox.size_x = float(tracking_annotation.width) + ros_box.bbox.size_y = float(tracking_annotation.height) + ros_box.results[0].id = str(tracking_annotation.name) + ros_box.results[0].score = float(-1) + return ros_box + + def to_ros_face(self, category): + """ + Converts an OpenDR category into a ObjectHypothesis msg that can carry the Category.data and + Category.confidence. + :param category: OpenDR category to be converted + :type category: engine.target.Category + :return: ROS2 message with the category.data and category.confidence + :rtype: vision_msgs.msg.ObjectHypothesis + """ + result = ObjectHypothesisWithPose() + result.id = str(category.data) + result.score = category.confidence + return result + + def from_ros_face(self, ros_hypothesis): + """ + Converts a ROS2 message with category payload into an OpenDR category + :param ros_hypothesis: the object hypothesis to be converted + :type ros_hypothesis: vision_msgs.msg.ObjectHypothesis + :return: an OpenDR category + :rtype: engine.target.Category + """ + return Category(prediction=ros_hypothesis.id, description=None, + confidence=ros_hypothesis.score) + + def to_ros_face_id(self, category): + """ + Converts an OpenDR category into a string msg that can carry the Category.description. + :param category: OpenDR category to be converted + :type category: engine.target.Category + :return: ROS2 message with the category.description + :rtype: std_msgs.msg.String + """ + result = String() + result.data = category.description + return result + + def from_ros_point_cloud(self, point_cloud: PointCloudMsg): + """ + Converts a ROS PointCloud message into an OpenDR PointCloud + :param point_cloud: ROS PointCloud to be converted + :type point_cloud: sensor_msgs.msg.PointCloud + :return: OpenDR PointCloud + :rtype: engine.data.PointCloud + """ + + points = np.empty([len(point_cloud.points), 3 + len(point_cloud.channels)], dtype=np.float32) + + for i in range(len(point_cloud.points)): + point = point_cloud.points[i] + x, y, z = point.x, point.y, point.z + + points[i, 0] = x + points[i, 1] = y + points[i, 2] = z + + for q in range(len(point_cloud.channels)): + points[i, 3 + q] = point_cloud.channels[q].values[i] + + result = PointCloud(points) + + return result + + def to_ros_point_cloud(self, point_cloud, time_stamp): + """ + Converts an OpenDR PointCloud message into a ROS2 PointCloud + :param point_cloud: OpenDR PointCloud + :type point_cloud: engine.data.PointCloud + :param time_stamp: Time stamp + :type time_stamp: ROS Time + :return: ROS PointCloud + :rtype: sensor_msgs.msg.PointCloud + """ + + ros_point_cloud = PointCloudMsg() + + header = Header() + + header.stamp = time_stamp + ros_point_cloud.header = header + + channels_count = point_cloud.data.shape[-1] - 3 + + channels = [ChannelFloat32Msg(name="channel_" + str(i), values=[]) for i in range(channels_count)] + points = [] + + for point in point_cloud.data: + point_msg = Point32Msg() + point_msg.x = float(point[0]) + point_msg.y = float(point[1]) + point_msg.z = float(point[2]) + points.append(point_msg) + for i in range(channels_count): + channels[i].values.append(float(point[3 + i])) + + ros_point_cloud.points = points + ros_point_cloud.channels = channels + + return ros_point_cloud + + def from_ros_boxes_3d(self, ros_boxes_3d): + """ + Converts a ROS2 Detection3DArray message into an OpenDR BoundingBox3D object. + :param ros_boxes_3d: The ROS boxes to be converted. + :type ros_boxes_3d: vision_msgs.msg.Detection3DArray + :return: An OpenDR BoundingBox3DList object. + :rtype: engine.target.BoundingBox3DList + """ + boxes = [] + + for ros_box in ros_boxes_3d: + + box = BoundingBox3D( + name=ros_box.results[0].id, + truncated=0, + occluded=0, + bbox2d=None, + dimensions=np.array([ + ros_box.bbox.size.position.x, + ros_box.bbox.size.position.y, + ros_box.bbox.size.position.z, + ]), + location=np.array([ + ros_box.bbox.center.position.x, + ros_box.bbox.center.position.y, + ros_box.bbox.center.position.z, + ]), + rotation_y=ros_box.bbox.center.rotation.y, + score=ros_box.results[0].score, + ) + boxes.append(box) + + result = BoundingBox3DList(boxes) + return result + + def to_ros_boxes_3d(self, boxes_3d): + """ + Converts an OpenDR BoundingBox3DList object into a ROS2 Detection3DArray message. + :param boxes_3d: The OpenDR boxes to be converted. + :type boxes_3d: engine.target.BoundingBox3DList + :return: ROS message with the boxes + :rtype: vision_msgs.msg.Detection3DArray + """ + ros_boxes_3d = Detection3DArray() + for i in range(len(boxes_3d)): + box = Detection3D() + box.bbox = BoundingBox3DMsg() + box.results.append(ObjectHypothesisWithPose()) + box.bbox.center = Pose3D() + box.bbox.center.position.x = float(boxes_3d[i].location[0]) + box.bbox.center.position.y = float(boxes_3d[i].location[1]) + box.bbox.center.position.z = float(boxes_3d[i].location[2]) + box.bbox.center.orientation = QuaternionMsg(x=0.0, y=float(boxes_3d[i].rotation_y), z=0.0, w=0.0) + box.bbox.size.x = float(boxes_3d[i].dimensions[0]) + box.bbox.size.y = float(boxes_3d[i].dimensions[1]) + box.bbox.size.z = float(boxes_3d[i].dimensions[2]) + box.results[0].id = boxes_3d[i].name + box.results[0].score = float(boxes_3d[i].confidence) + ros_boxes_3d.detections.append(box) + return ros_boxes_3d + + def from_ros_mesh(self, mesh_ROS): + """ + Converts a ROS mesh into arrays of vertices and faces of a mesh + :param mesh_ROS: the ROS mesh to be converted + :type mesh_ROS: shape_msgs.msg.Mesh + :return vertices: Numpy array Nx3 representing vertices of the 3D model respectively + :rtype vertices: np.array + :return faces: Numpy array Nx3 representing the IDs of the vertices of each face of the 3D model + :rtype faces: numpy array (Nx3) + """ + vertices = np.zeros([len(mesh_ROS.vertices), 3]) + faces = np.zeros([len(mesh_ROS.triangles), 3]).astype(int) + for i in range(len(mesh_ROS.vertices)): + vertices[i] = np.array([mesh_ROS.vertices[i].x, mesh_ROS.vertices[i].y, mesh_ROS.vertices[i].z]) + for i in range(len(mesh_ROS.triangles)): + faces[i] = np.array([int(mesh_ROS.triangles[i].vertex_indices[0]), int(mesh_ROS.triangles[i].vertex_indices[1]), + int(mesh_ROS.triangles[i].vertex_indices[2])]).astype(int) + return vertices, faces + + def to_ros_mesh(self, vertices, faces): + """ + Converts a mesh into a ROS Mesh + :param vertices: the vertices of the 3D model + :type vertices: numpy array (Nx3) + :param faces: the faces of the 3D model + :type faces: numpy array (Nx3) + :return mesh_ROS: a ROS mesh + :rtype mesh_ROS: shape_msgs.msg.Mesh + """ + mesh_ROS = Mesh() + for i in range(vertices.shape[0]): + point = Point() + point.x = vertices[i, 0] + point.y = vertices[i, 1] + point.z = vertices[i, 2] + mesh_ROS.vertices.append(point) + for i in range(faces.shape[0]): + mesh_triangle = MeshTriangle() + mesh_triangle.vertex_indices[0] = int(faces[i][0]) + mesh_triangle.vertex_indices[1] = int(faces[i][1]) + mesh_triangle.vertex_indices[2] = int(faces[i][2]) + mesh_ROS.triangles.append(mesh_triangle) + return mesh_ROS + + def from_ros_colors(self, ros_colors): + """ + Converts a list of ROS colors into a list of colors + :param ros_colors: a list of the colors of the vertices + :type ros_colors: std_msgs.msg.ColorRGBA[] + :return colors: the colors of the vertices of the 3D model + :rtype colors: numpy array (Nx3) + """ + colors = np.zeros([len(ros_colors), 3]) + for i in range(len(ros_colors)): + colors[i] = np.array([ros_colors[i].r, ros_colors[i].g, ros_colors[i].b]) + return colors + + def to_ros_colors(self, colors): + """ + Converts an array of vertex_colors to a list of ROS colors + :param colors: a numpy array of RGB colors + :type colors: numpy array (Nx3) + :return ros_colors: a list of the colors of the vertices + :rtype ros_colors: std_msgs.msg.ColorRGBA[] + """ + ros_colors = [] + for i in range(colors.shape[0]): + color = ColorRGBA() + color.r = colors[i, 0] + color.g = colors[i, 1] + color.b = colors[i, 2] + color.a = 0.0 + ros_colors.append(color) + return ros_colors + + def from_ros_pose_3D(self, ros_pose): + """ + Converts a ROS message with pose payload into an OpenDR pose + :param ros_pose: the pose to be converted (represented as opendr_interface.msg.OpenDRPose3D) + :type ros_pose: opendr_interface.msg.OpenDRPose3D + :return: an OpenDR pose + :rtype: engine.target.Pose + """ + keypoints = ros_pose.keypoint_list + data = [] + for i, keypoint in enumerate(keypoints): + data.append([keypoint.x, keypoint.y, keypoint.z]) + pose = Pose(data, 1.0) + pose.id = 0 + return pose + + def to_ros_pose_3D(self, pose): + """ + Converts an OpenDR pose into a OpenDRPose3D msg that can carry the same information + Each keypoint is represented as an OpenDRPose3DKeypoint with x, y, z coordinates. + :param pose: OpenDR pose to be converted + :type pose: engine.target.Pose + :return: ROS message with the pose + :rtype: opendr_interface.msg.OpenDRPose3D + """ + data = pose.data + ros_pose = OpenDRPose3D() + ros_pose.pose_id = 0 + if pose.id is not None: + ros_pose.pose_id = int(pose.id) + ros_pose.conf = 1.0 + for i in range(len(data)): + keypoint = OpenDRPose3DKeypoint() + keypoint.kpt_name = '' + keypoint.x = float(data[i][0]) + keypoint.y = float(data[i][1]) + keypoint.z = float(data[i][2]) + ros_pose.keypoint_list.append(keypoint) + return ros_pose + + def to_ros_category(self, category): + """ + Converts an OpenDR category into a ObjectHypothesis msg that can carry the Category.data and Category.confidence. + :param category: OpenDR category to be converted + :type category: engine.target.Category + :return: ROS message with the category.data and category.confidence + :rtype: vision_msgs.msg.ObjectHypothesis + """ + result = ObjectHypothesis() + result.id = str(category.data) + result.score = float(category.confidence) + return result + + def from_ros_category(self, ros_hypothesis): + """ + Converts a ROS message with category payload into an OpenDR category + :param ros_hypothesis: the object hypothesis to be converted + :type ros_hypothesis: vision_msgs.msg.ObjectHypothesis + :return: an OpenDR category + :rtype: engine.target.Category + """ + category = Category(prediction=ros_hypothesis.id, description=None, + confidence=ros_hypothesis.score) + return category + + def to_ros_category_description(self, category): + """ + Converts an OpenDR category into a string msg that can carry the Category.description. + :param category: OpenDR category to be converted + :type category: engine.target.Category + :return: ROS message with the category.description + :rtype: std_msgs.msg.String + """ + result = String() + result.data = category.description + return result + + def from_rosarray_to_timeseries(self, ros_array, dim1, dim2): + """ + Converts ROS2 array into OpenDR Timeseries object + :param ros_array: data to be converted + :type ros_array: std_msgs.msg.Float32MultiArray + :param dim1: 1st dimension + :type dim1: int + :param dim2: 2nd dimension + :type dim2: int + :rtype: engine.data.Timeseries + """ + data = np.reshape(ros_array.data, (dim1, dim2)) + data = Timeseries(data) + return data + + def from_ros_image_to_depth(self, message, encoding='mono16'): + """ + Converts a ROS2 image message into an OpenDR grayscale depth image + :param message: ROS2 image to be converted + :type message: sensor_msgs.msg.Image + :param encoding: encoding to be used for the conversion + :type encoding: str + :return: OpenDR image + :rtype: engine.data.Image + """ + cv_image = self._cv_bridge.imgmsg_to_cv2(message, desired_encoding=encoding) + cv_image = np.expand_dims(cv_image, axis=-1) + image = Image(np.asarray(cv_image, dtype=np.uint8)) + return image + + def from_category_to_rosclass(self, prediction, timestamp, source_data=None): + """ + Converts OpenDR Category into Classification2D message with class label, confidence, timestamp and corresponding input + :param prediction: classification prediction + :type prediction: engine.target.Category + :param timestamp: time stamp for header message + :type timestamp: str + :param source_data: corresponding input or None + :return classification + :rtype: vision_msgs.msg.Classification2D + """ + classification = Classification2D() + classification.header = Header() + classification.header.stamp = timestamp + + result = ObjectHypothesis() + result.id = str(prediction.data) + result.score = prediction.confidence + classification.results.append(result) + if source_data is not None: + classification.source_img = source_data + return classification diff --git a/projects/opendr_ws_2/src/opendr_bridge/package.xml b/projects/opendr_ws_2/src/opendr_bridge/package.xml new file mode 100644 index 0000000000..9527c98d24 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_bridge/package.xml @@ -0,0 +1,21 @@ + + + + opendr_bridge + 1.0.0 + OpenDR ROS2 bridge package. This package provides a way to translate ROS2 messages into OpenDR data types + and vice versa. + OpenDR Project Coordinator + Apache License v2.0 + + rclpy + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/projects/opendr_ws_2/src/opendr_bridge/resource/opendr_bridge b/projects/opendr_ws_2/src/opendr_bridge/resource/opendr_bridge new file mode 100644 index 0000000000..e69de29bb2 diff --git a/projects/opendr_ws_2/src/opendr_bridge/setup.cfg b/projects/opendr_ws_2/src/opendr_bridge/setup.cfg new file mode 100644 index 0000000000..9d9e5c012f --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_bridge/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/opendr_bridge +[install] +install_scripts=$base/lib/opendr_bridge diff --git a/projects/opendr_ws_2/src/opendr_bridge/setup.py b/projects/opendr_ws_2/src/opendr_bridge/setup.py new file mode 100644 index 0000000000..1287443a85 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_bridge/setup.py @@ -0,0 +1,26 @@ +from setuptools import setup + +package_name = 'opendr_bridge' + +setup( + name=package_name, + version='1.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='OpenDR Project Coordinator', + maintainer_email='tefas@csd.auth.gr', + description='OpenDR ROS2 bridge package. This package provides a way to translate ROS2 messages into OpenDR' + + 'data types and vice versa.', + license='Apache License v2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + ], + }, +) diff --git a/projects/opendr_ws_2/src/opendr_bridge/test/test_copyright.py b/projects/opendr_ws_2/src/opendr_bridge/test/test_copyright.py new file mode 100644 index 0000000000..cc8ff03f79 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_bridge/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/projects/opendr_ws_2/src/opendr_bridge/test/test_flake8.py b/projects/opendr_ws_2/src/opendr_bridge/test/test_flake8.py new file mode 100644 index 0000000000..27ee1078ff --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_bridge/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/projects/opendr_ws_2/src/opendr_bridge/test/test_pep257.py b/projects/opendr_ws_2/src/opendr_bridge/test/test_pep257.py new file mode 100644 index 0000000000..b234a3840f --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_bridge/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/projects/opendr_ws_2/src/opendr_data_generation/opendr_data_generation/__init__.py b/projects/opendr_ws_2/src/opendr_data_generation/opendr_data_generation/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/projects/opendr_ws_2/src/opendr_data_generation/opendr_data_generation/synthetic_facial_generation_node.py b/projects/opendr_ws_2/src/opendr_data_generation/opendr_data_generation/synthetic_facial_generation_node.py new file mode 100644 index 0000000000..094a20cfd8 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_data_generation/opendr_data_generation/synthetic_facial_generation_node.py @@ -0,0 +1,173 @@ +#!/usr/bin/env python3.6 +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import cv2 +import os +import argparse +import numpy as np + +import rclpy +from rclpy.node import Node + +from sensor_msgs.msg import Image as ROS_Image +from cv_bridge import CvBridge + +from opendr.projects.python.simulation.synthetic_multi_view_facial_image_generation.algorithm.DDFA.utils.ddfa \ + import str2bool +from opendr.src.opendr.engine.data import Image +from opendr.projects.python.simulation.synthetic_multi_view_facial_image_generation.SyntheticDataGeneration \ + import MultiviewDataGeneration + + +class SyntheticDataGeneratorNode(Node): + + def __init__(self, args, input_rgb_image_topic="/image_raw", + output_rgb_image_topic="/opendr/synthetic_facial_images"): + """ + Creates a ROS Node for SyntheticDataGeneration + :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 synthetic facial image (if None, no image + is published) + :type output_rgb_image_topic: str + """ + super().__init__('synthetic_facial_image_generation_node') + self.image_publisher = self.create_publisher(ROS_Image, output_rgb_image_topic, 10) + self.create_subscription(ROS_Image, input_rgb_image_topic, self.callback, 1) + self._cv_bridge = CvBridge() + self.ID = 0 + self.args = args + self.path_in = args.path_in + self.key = str(args.path_3ddfa + "/example/Images/") + self.key1 = str(args.path_3ddfa + "/example/") + self.key2 = str(args.path_3ddfa + "/results/") + self.save_path = args.save_path + self.val_yaw = args.val_yaw + self.val_pitch = args.val_pitch + self.device = args.device + + # Initialize the SyntheticDataGeneration + self.synthetic = MultiviewDataGeneration(self.args) + + def callback(self, data): + """ + Callback that process the input data and publishes to the corresponding topics + :param data: input message + :type data: sensor_msgs.msg.Image + """ + + # Convert sensor_msgs.msg.Image into OpenDR Image + + cv_image = self._cv_bridge.imgmsg_to_cv2(data, desired_encoding="rgb8") + image = Image(np.asarray(cv_image, dtype=np.uint8)) + self.ID = self.ID + 1 + # Get an OpenCV image back + image = cv2.cvtColor(image.opencv(), cv2.COLOR_RGBA2BGR) + name = str(f"{self.ID:02d}" + "_single.jpg") + cv2.imwrite(os.path.join(self.path_in, name), image) + + if self.ID == 10: + # Run SyntheticDataGeneration + self.synthetic.eval() + self.ID = 0 + # Annotate image and publish results + current_directory_path = os.path.join(self.save_path, str("/Documents_orig/")) + for file in os.listdir(current_directory_path): + name, ext = os.path.splitext(file) + if ext == ".jpg": + image_file_savepath = os.path.join(current_directory_path, file) + cv_image = cv2.imread(image_file_savepath) + cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB) + if self.image_publisher is not None: + image = Image(np.array(cv_image, dtype=np.uint8)) + message = self.bridge.to_ros_image(image, encoding="rgb8") + self.image_publisher.publish(message) + for f in os.listdir(self.path_in): + os.remove(os.path.join(self.path_in, f)) + + +def main(args=None): + rclpy.init(args=args) + parser = argparse.ArgumentParser() + parser.add_argument("-i", "--input_rgb_image_topic", help="Topic name for input rgb image", + type=str, default="/image_raw") + parser.add_argument("-o", "--output_rgb_image_topic", help="Topic name for output annotated rgb image", + type=str, default="/opendr/synthetic_facial_images") + parser.add_argument("--path_in", default=os.path.join("opendr", "projects", + "data_generation", + "synthetic_multi_view_facial_image_generation", + "demos", "imgs_input"), + type=str, help='Give the path of image folder') + parser.add_argument('--path_3ddfa', default=os.path.join("opendr", "projects", + "data_generation", + "synthetic_multi_view_facial_image_generation", + "algorithm", "DDFA"), + type=str, help='Give the path of DDFA folder') + parser.add_argument('--save_path', default=os.path.join("opendr", "projects", + "data_generation", + "synthetic_multi_view_facial_image_generation", + "results"), + type=str, help='Give the path of results folder') + parser.add_argument('--val_yaw', default="10 20", nargs='+', type=str, help='yaw poses list between [-90,90]') + parser.add_argument('--val_pitch', default="30 40", nargs='+', type=str, help='pitch poses list between [-90,90]') + parser.add_argument("--device", default="cuda", type=str, help="choose between cuda or cpu ") + parser.add_argument('-f', '--files', nargs='+', + help='image files paths fed into network, single or multiple images') + parser.add_argument('--show_flg', default='false', type=str2bool, help='whether show the visualization result') + parser.add_argument('--dump_res', default='true', type=str2bool, + help='whether write out the visualization image') + parser.add_argument('--dump_vertex', default='false', type=str2bool, + help='whether write out the dense face vertices to mat') + parser.add_argument('--dump_ply', default='true', type=str2bool) + parser.add_argument('--dump_pts', default='true', type=str2bool) + parser.add_argument('--dump_roi_box', default='false', type=str2bool) + parser.add_argument('--dump_pose', default='true', type=str2bool) + parser.add_argument('--dump_depth', default='true', type=str2bool) + parser.add_argument('--dump_pncc', default='true', type=str2bool) + parser.add_argument('--dump_paf', default='true', type=str2bool) + parser.add_argument('--paf_size', default=3, type=int, help='PAF feature kernel size') + parser.add_argument('--dump_obj', default='true', type=str2bool) + parser.add_argument('--dlib_bbox', default='true', type=str2bool, help='whether use dlib to predict bbox') + parser.add_argument('--dlib_landmark', default='true', type=str2bool, + help='whether use dlib landmark to crop image') + parser.add_argument('-m', '--mode', default='gpu', type=str, help='gpu or cpu mode') + parser.add_argument('--bbox_init', default='two', type=str, + help='one|two: one-step bbox initialization or two-step') + parser.add_argument('--dump_2d_img', default='true', type=str2bool, help='whether to save 3d rendered image') + parser.add_argument('--dump_param', default='true', type=str2bool, help='whether to save param') + parser.add_argument('--dump_lmk', default='true', type=str2bool, help='whether to save landmarks') + parser.add_argument('--save_dir', default='./algorithm/DDFA/results', type=str, help='dir to save result') + parser.add_argument('--save_lmk_dir', default='./example', type=str, help='dir to save landmark result') + parser.add_argument('--img_list', default='./txt_name_batch.txt', type=str, help='test image list file') + parser.add_argument('--rank', default=0, type=int, help='used when parallel run') + parser.add_argument('--world_size', default=1, type=int, help='used when parallel run') + parser.add_argument('--resume_idx', default=0, type=int) + args = parser.parse_args() + + synthetic_data_generation_node = SyntheticDataGeneratorNode(args=args, + input_rgb_image_topic=args.input_rgb_image_topic, + output_rgb_image_topic=args.output_rgb_image_topic) + + rclpy.spin(synthetic_data_generation_node) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + synthetic_data_generation_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/projects/opendr_ws_2/src/opendr_data_generation/package.xml b/projects/opendr_ws_2/src/opendr_data_generation/package.xml new file mode 100644 index 0000000000..a7f13acc60 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_data_generation/package.xml @@ -0,0 +1,25 @@ + + + + opendr_data_generation + 1.0.0 + OpenDR's ROS2 nodes for data generation package + tefas + Apache License v2.0 + + sensor_msgs + + rclpy + opendr_bridge + + ament_cmake + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/projects/opendr_ws_2/src/opendr_data_generation/resource/opendr_data_generation b/projects/opendr_ws_2/src/opendr_data_generation/resource/opendr_data_generation new file mode 100644 index 0000000000..e69de29bb2 diff --git a/projects/opendr_ws_2/src/opendr_data_generation/setup.cfg b/projects/opendr_ws_2/src/opendr_data_generation/setup.cfg new file mode 100644 index 0000000000..893b4dda07 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_data_generation/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/opendr_data_generation +[install] +install_scripts=$base/lib/opendr_data_generation diff --git a/projects/opendr_ws_2/src/opendr_data_generation/setup.py b/projects/opendr_ws_2/src/opendr_data_generation/setup.py new file mode 100644 index 0000000000..486f88a013 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_data_generation/setup.py @@ -0,0 +1,40 @@ +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from setuptools import setup + +package_name = 'opendr_data_generation' + +setup( + name=package_name, + version='1.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='OpenDR Project Coordinator', + maintainer_email='tefas@csd.auth.gr', + description='OpenDR\'s ROS2 nodes for data generation package', + license='Apache License v2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'synthetic_facial_generation = opendr_data_generation.synthetic_facial_generation_node:main' + ], + }, +) diff --git a/projects/opendr_ws_2/src/opendr_data_generation/test/test_copyright.py b/projects/opendr_ws_2/src/opendr_data_generation/test/test_copyright.py new file mode 100644 index 0000000000..cc8ff03f79 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_data_generation/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/projects/opendr_ws_2/src/opendr_data_generation/test/test_flake8.py b/projects/opendr_ws_2/src/opendr_data_generation/test/test_flake8.py new file mode 100644 index 0000000000..18bd9331ea --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_data_generation/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/projects/opendr_ws_2/src/opendr_data_generation/test/test_pep257.py b/projects/opendr_ws_2/src/opendr_data_generation/test/test_pep257.py new file mode 100644 index 0000000000..b234a3840f --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_data_generation/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/projects/opendr_ws_2/src/opendr_interface/CMakeLists.txt b/projects/opendr_ws_2/src/opendr_interface/CMakeLists.txt new file mode 100644 index 0000000000..9c158812e5 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_interface/CMakeLists.txt @@ -0,0 +1,50 @@ +cmake_minimum_required(VERSION 3.5) +project(opendr_interface) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) +find_package(std_msgs REQUIRED) +find_package(shape_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(vision_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/OpenDRPose2D.msg" + "msg/OpenDRPose2DKeypoint.msg" + "msg/OpenDRPose3D.msg" + "msg/OpenDRPose3DKeypoint.msg" + "srv/OpenDRSingleObjectTracking.srv" + "srv/ImgToMesh.srv" + DEPENDENCIES std_msgs shape_msgs sensor_msgs vision_msgs +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/projects/opendr_ws_2/src/opendr_interface/include/opendr_interface/.keep b/projects/opendr_ws_2/src/opendr_interface/include/opendr_interface/.keep new file mode 100644 index 0000000000..e69de29bb2 diff --git a/projects/opendr_ws_2/src/opendr_interface/msg/OpenDRPose2D.msg b/projects/opendr_ws_2/src/opendr_interface/msg/OpenDRPose2D.msg new file mode 100644 index 0000000000..184f3fd11b --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_interface/msg/OpenDRPose2D.msg @@ -0,0 +1,26 @@ +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# This message represents a full OpenDR human pose 2D as a list of keypoints + +std_msgs/Header header + +# The id of the pose +int32 pose_id + +# The pose detection confidence of the model +float32 conf + +# A list of a human 2D pose keypoints +OpenDRPose2DKeypoint[] keypoint_list \ No newline at end of file diff --git a/projects/opendr_ws_2/src/opendr_interface/msg/OpenDRPose2DKeypoint.msg b/projects/opendr_ws_2/src/opendr_interface/msg/OpenDRPose2DKeypoint.msg new file mode 100644 index 0000000000..72d14a19f2 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_interface/msg/OpenDRPose2DKeypoint.msg @@ -0,0 +1,22 @@ +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# This message contains all relevant information for an OpenDR human pose 2D keypoint + +# The kpt_name according to https://github.com/opendr-eu/opendr/blob/master/docs/reference/lightweight-open-pose.md#notes +string kpt_name + +# x and y pixel position on the input image, (0, 0) is top-left corner of image +int32 x +int32 y diff --git a/projects/opendr_ws_2/src/opendr_interface/msg/OpenDRPose3D.msg b/projects/opendr_ws_2/src/opendr_interface/msg/OpenDRPose3D.msg new file mode 100644 index 0000000000..a180eed5b0 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_interface/msg/OpenDRPose3D.msg @@ -0,0 +1,26 @@ +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# This message represents a full OpenDR human pose 3D as a list of keypoints + +std_msgs/Header header + +# The id of the pose +int32 pose_id + +# The pose detection confidence of the model +float32 conf + +# A list of a human 3D pose keypoints +OpenDRPose3DKeypoint[] keypoint_list diff --git a/projects/opendr_ws_2/src/opendr_interface/msg/OpenDRPose3DKeypoint.msg b/projects/opendr_ws_2/src/opendr_interface/msg/OpenDRPose3DKeypoint.msg new file mode 100644 index 0000000000..179aa9e348 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_interface/msg/OpenDRPose3DKeypoint.msg @@ -0,0 +1,22 @@ +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# The kpt_name according to https://github.com/opendr-eu/opendr/blob/master/docs/reference/lightweight-open-pose.md#notes +string kpt_name + +# This message contains all relevant information for an OpenDR human pose 3D keypoint + +float32 x +float32 y +float32 z diff --git a/projects/opendr_ws_2/src/opendr_interface/package.xml b/projects/opendr_ws_2/src/opendr_interface/package.xml new file mode 100644 index 0000000000..df4e89b826 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_interface/package.xml @@ -0,0 +1,24 @@ + + + + opendr_interface + 1.0.0 + OpenDR ROS2 custom interface package. This package includes all custom OpenDR ROS2 messages and services. + OpenDR Project Coordinator + Apache License v2.0 + + ament_cmake + + std_msgs + rosidl_default_generators + + rosidl_default_runtime + rosidl_interface_packages + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/projects/opendr_ws_2/src/opendr_interface/src/.keep b/projects/opendr_ws_2/src/opendr_interface/src/.keep new file mode 100644 index 0000000000..e69de29bb2 diff --git a/projects/opendr_ws_2/src/opendr_interface/srv/ImgToMesh.srv b/projects/opendr_ws_2/src/opendr_interface/srv/ImgToMesh.srv new file mode 100644 index 0000000000..3d6d15717a --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_interface/srv/ImgToMesh.srv @@ -0,0 +1,21 @@ +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +sensor_msgs/Image img_rgb +sensor_msgs/Image img_msk +std_msgs/Bool extract_pose +--- +shape_msgs/Mesh mesh +std_msgs/ColorRGBA[] vertex_colors +OpenDRPose3D pose diff --git a/projects/opendr_ws_2/src/opendr_interface/srv/OpenDRSingleObjectTracking.srv b/projects/opendr_ws_2/src/opendr_interface/srv/OpenDRSingleObjectTracking.srv new file mode 100644 index 0000000000..e7b3c29517 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_interface/srv/OpenDRSingleObjectTracking.srv @@ -0,0 +1,17 @@ +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +vision_msgs/Detection2D init_box +--- +bool success diff --git a/projects/opendr_ws_2/src/opendr_perception/README.md b/projects/opendr_ws_2/src/opendr_perception/README.md new file mode 100755 index 0000000000..1fce5f935d --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/README.md @@ -0,0 +1,885 @@ +# OpenDR Perception Package + +This package contains ROS2 nodes related to the perception package of OpenDR. + +--- + +## Prerequisites + +Before you can run any of the toolkit's ROS2 nodes, some prerequisites need to be fulfilled: +1. First of all, you need to [set up the required packages and build your workspace.](../../README.md#first-time-setup) +2. _(Optional for nodes with [RGB input](#rgb-input-nodes))_ + + For basic usage and testing, all the toolkit's ROS2 nodes that use RGB images are set up to expect input from a basic webcam using the default package `usb_cam` which is installed with OpenDR. You can run the webcam node in a new terminal: + ```shell + ros2 run usb_cam usb_cam_node_exe + ``` + By default, the USB cam node publishes images on `/image_raw` and the RGB input nodes subscribe to this topic if not provided with an input topic argument. + As explained for each node below, you can modify the topics via arguments, so if you use any other node responsible for publishing images, **make sure to change the input topic accordingly.** + +3. _(Optional for nodes with [audio input](#audio-input) or [audiovisual input](#rgb--audio-input))_ + + For basic usage and testing, the toolkit's ROS2 nodes that use audio as input are set up to expect input from a basic audio device using the default package `audio_common` which is installed with OpenDR. You can run the audio node in a new terminal: + ```shell + ros2 run audio_capture audio_capture_node + ``` + By default, the audio capture node publishes audio data on `/audio` and the audio input nodes subscribe to this topic if not provided with an input topic argument. + As explained for each node below, you can modify the topics via arguments, so if you use any other node responsible for publishing audio, **make sure to change the input topic accordingly.** + +--- + +## Notes + +- ### Display output images with rqt_image_view + For any node that outputs images, `rqt_image_view` can be used to display them by running the following command: + ```shell + ros2 run rqt_image_view rqt_image_view & + ``` + A window will appear, where the topic that you want to view can be selected from the drop-down menu on the top-left area of the window. + Refer to each node's documentation below to find out the default output image topic, where applicable, and select it on the drop-down menu of rqt_image_view. + +- ### Echo node output + All OpenDR nodes publish some kind of detection message, which can be echoed by running the following command: + ```shell + ros2 topic echo /opendr/topic_name + ``` + You can find out the default topic name for each node, in its documentation below. + +- ### Increase performance by disabling output + Optionally, nodes can be modified via command line arguments, which are presented for each node separately below. + Generally, arguments give the option to change the input and output topics, the device the node runs on (CPU or GPU), etc. + When a node publishes on several topics, where applicable, a user can opt to disable one or more of the outputs by providing `None` in the corresponding output topic. + This disables publishing on that topic, forgoing some operations in the node, which might increase its performance. + + _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._ + +- ### 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. + - In the middle, OpenDR's face detection node is running taking as input the published image. By default, the node has its input topic set to `/image_raw`. + - To the right the two output topics of the face detection node can be seen. + The bottom topic `/opendr/image_faces_annotated` is the annotated image which can be easily viewed with `rqt_image_view` as explained earlier. + The other topic `/opendr/faces` is the detection message which contains the detected faces' detailed information. + This message can be easily viewed by running `ros2 topic echo /opendr/faces` in a terminal. + + + +---- + +## RGB input nodes + +### Pose Estimation ROS2 Node + +You can find the pose estimation ROS2 node python script [here](./opendr_perception/pose_estimation_node.py) to inspect the code and modify it as you wish to fit your needs. +The node makes use of the toolkit's [pose estimation tool](../../../../src/opendr/perception/pose_estimation/lightweight_open_pose/lightweight_open_pose_learner.py) whose documentation can be found [here](../../../../docs/reference/lightweight-open-pose.md). +The node publishes the detected poses in [OpenDR's 2D pose message format](../opendr_interface/msg/OpenDRPose2D.msg), which saves a list of [OpenDR's keypoint message format](../opendr_interface/msg/OpenDRPose2DKeypoint.msg). + +#### Instructions for basic usage: + +1. Start the node responsible for publishing images. If you have a USB camera, then you can use the `usb_cam_node` as explained in the [prerequisites above](#prerequisites). + +2. You are then ready to start the pose detection node: + ```shell + ros2 run opendr_perception pose_estimation + ``` + The following optional arguments are available: + - `-h, --help`: show a help message and exit + - `-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`) + - `--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 + +3. Default output topics: + - Output images: `/opendr/image_pose_annotated` + - Detection messages: `/opendr/poses` + + For viewing the output, refer to the [notes above.](#notes) + +### High Resolution Pose Estimation ROS2 Node + +You can find the high resolution pose estimation ROS2 node python script [here](./opendr_perception/hr_pose_estimation_node.py) to inspect the code and modify it as you wish to fit your needs. +The node makes use of the toolkit's [high resolution pose estimation tool](../../../../src/opendr/perception/pose_estimation/hr_pose_estimation/high_resolution_learner.py) whose documentation can be found [here](../../../../docs/reference/high-resolution-pose-estimation.md). +The node publishes the detected poses in [OpenDR's 2D pose message format](../opendr_interface/msg/OpenDRPose2D.msg), which saves a list of [OpenDR's keypoint message format](../opendr_interface/msg/OpenDRPose2DKeypoint.msg). + +#### Instructions for basic usage: + +1. Start the node responsible for publishing images. If you have a USB camera, then you can use the `usb_cam_node` as explained in the [prerequisites above](#prerequisites). + +2. You are then ready to start the high resolution pose detection node: + ```shell + ros2 run opendr_perception hr_pose_estimation + ``` + The following optional arguments are available: + - `-h, --help`: show a help message and exit + - `-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`) + - `--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 + +3. Default output topics: + - Output images: `/opendr/image_pose_annotated` + - Detection messages: `/opendr/poses` + + For viewing the output, refer to the [notes above.](#notes) + +### Fall Detection ROS2 Node + +You can find the fall detection ROS2 node python script [here](./opendr_perception/fall_detection_node.py) to inspect the code and modify it as you wish to fit your needs. +The node makes use of the toolkit's [fall detection tool](../../../../src/opendr/perception/fall_detection/fall_detector_learner.py) whose documentation can be found [here](../../../../docs/reference/fall-detection.md). +Fall detection uses the toolkit's pose estimation tool internally. + + + +#### Instructions for basic usage: + +1. Start the node responsible for publishing images. If you have a USB camera, then you can use the `usb_cam_node` as explained in the [prerequisites above](#prerequisites). + +2. You are then ready to start the fall detection node: + + ```shell + ros2 run opendr_perception fall_detection + ``` + The following optional arguments are available: + - `-h or --help`: show a help message and exit + - `-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`) + - `--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 + +3. Default output topics: + - Output images: `/opendr/image_fallen_annotated` + - Detection messages: `/opendr/fallen` + + For viewing the output, refer to the [notes above.](#notes) + +### Face Detection ROS2 Node + +The face detection ROS2 node supports both the ResNet and MobileNet versions, the latter of which performs masked face detection as well. + +You can find the face detection ROS2 node python script [here](./opendr_perception/face_detection_retinaface_node.py) to inspect the code and modify it as you wish to fit your needs. +The node makes use of the toolkit's [face detection tool](../../../../src/opendr/perception/object_detection_2d/retinaface/retinaface_learner.py) whose documentation can be found [here](../../../../docs/reference/face-detection-2d-retinaface.md). + +#### Instructions for basic usage: + +1. Start the node responsible for publishing images. If you have a USB camera, then you can use the `usb_cam_node` as explained in the [prerequisites above](#prerequisites). + +2. You are then ready to start the face detection node + + ```shell + ros2 run opendr_perception face_detection_retinaface + ``` + The following optional arguments are available: + - `-h or --help`: show a help message and exit + - `-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`) + - `--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`) + +3. Default output topics: + - Output images: `/opendr/image_faces_annotated` + - Detection messages: `/opendr/faces` + + For viewing the output, refer to the [notes above.](#notes) + +### Face Recognition ROS2 Node + +You can find the face recognition ROS2 node python script [here](./opendr_perception/face_recognition_node.py) to inspect the code and modify it as you wish to fit your needs. +The node makes use of the toolkit's [face recognition tool](../../../../src/opendr/perception/face_recognition/face_recognition_learner.py) whose documentation can be found [here](../../../../docs/reference/face-recognition.md). + +#### Instructions for basic usage: + +1. Start the node responsible for publishing images. If you have a USB camera, then you can use the `usb_cam_node` as explained in the [prerequisites above](#prerequisites). + +2. You are then ready to start the face recognition node: + + ```shell + ros2 run opendr_perception face_recognition + ``` + The following optional arguments are available: + - `-h or --help`: show a help message and exit + - `-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_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`) + - `--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`) + +3. Default output topics: + - Output images: `/opendr/image_face_reco_annotated` + - Detection messages: `/opendr/face_recognition` and `/opendr/face_recognition_id` + + For viewing the output, refer to the [notes above.](#notes) + +**Notes** + +Reference images should be placed in a defined structure like: +- imgs + - ID1 + - image1 + - image2 + - ID2 + - ID3 + - ... + +The default dataset path is `./database`. Please use the `--database_path ./your/path/` argument to define a custom one. +Τhe name of the sub-folder, e.g. ID1, will be published under `/opendr/face_recognition_id`. + +The database entry and the returned confidence is published under the topic name `/opendr/face_recognition`, and the human-readable ID +under `/opendr/face_recognition_id`. + +### 2D Object Detection ROS2 Nodes + +For 2D object detection, there are several ROS2 nodes implemented using various algorithms. The generic object detectors are SSD, YOLOv3, YOLOv5, CenterNet, Nanodet and DETR. + +You can find the 2D object detection ROS2 node python scripts here: +[SSD node](./opendr_perception/object_detection_2d_ssd_node.py), [YOLOv3 node](./opendr_perception/object_detection_2d_yolov3_node.py), [YOLOv5 node](./opendr_perception/object_detection_2d_yolov5_node.py), [CenterNet node](./opendr_perception/object_detection_2d_centernet_node.py), [Nanodet node](./opendr_perception/object_detection_2d_nanodet_node.py) and [DETR node](./opendr_perception/object_detection_2d_detr_node.py), +where you can inspect the code and modify it as you wish to fit your needs. +The nodes makes use of the toolkit's various 2D object detection tools: +[SSD tool](../../../../src/opendr/perception/object_detection_2d/ssd/ssd_learner.py), [YOLOv3 tool](../../../../src/opendr/perception/object_detection_2d/yolov3/yolov3_learner.py), [YOLOv5 tool](../../../../src/opendr/perception/object_detection_2d/yolov5/yolov5_learner.py), +[CenterNet tool](../../../../src/opendr/perception/object_detection_2d/centernet/centernet_learner.py), [Nanodet tool](../../../../src/opendr/perception/object_detection_2d/nanodet/nanodet_learner.py), [DETR tool](../../../../src/opendr/perception/object_detection_2d/detr/detr_learner.py), +whose documentation can be found here: +[SSD docs](../../../../docs/reference/object-detection-2d-ssd.md), [YOLOv3 docs](../../../../docs/reference/object-detection-2d-yolov3.md), [YOLOv5 docs](../../../../docs/reference/object-detection-2d-yolov5.md), +[CenterNet docs](../../../../docs/reference/object-detection-2d-centernet.md), [Nanodet docs](../../../../docs/reference/nanodet.md), [DETR docs](../../../../docs/reference/detr.md). + +#### Instructions for basic usage: + +1. Start the node responsible for publishing images. If you have a USB camera, then you can use the `usb_cam_node` as explained in the [prerequisites above](#prerequisites). + +2. You are then ready to start a 2D object detector node: + 1. SSD node + ```shell + ros2 run opendr_perception object_detection_2d_ssd + ``` + The following optional arguments are available for the SSD node: + - `--backbone BACKBONE`: Backbone network (default=`vgg16_atrous`) + - `--nms_type NMS_TYPE`: Non-Maximum Suppression type options are `default`, `seq2seq-nms`, `soft-nms`, `fast-nms`, `cluster-nms` (default=`default`) + + 2. YOLOv3 node + ```shell + ros2 run opendr_perception object_detection_2d_yolov3 + ``` + The following optional argument is available for the YOLOv3 node: + - `--backbone BACKBONE`: Backbone network (default=`darknet53`) + + 3. YOLOv5 node + ```shell + ros2 run opendr_perception object_detection_2d_yolov5 + ``` + The following optional argument is available for the YOLOv5 node: + - `--model_name MODEL_NAME`: Network architecture, options are `yolov5s`, `yolov5n`, `yolov5m`, `yolov5l`, `yolov5x`, `yolov5n6`, `yolov5s6`, `yolov5m6`, `yolov5l6`, `custom` (default=`yolov5s`) + + 4. CenterNet node + ```shell + ros2 run opendr_perception object_detection_2d_centernet + ``` + The following optional argument is available for the CenterNet node: + - `--backbone BACKBONE`: Backbone network (default=`resnet50_v1b`) + + 5. Nanodet node + ```shell + ros2 run opendr_perception object_detection_2d_nanodet + ``` + The following optional argument is available for the Nanodet node: + - `--model Model`: Model that config file will be used (default=`plus_m_1.5x_416`) + + 6. DETR node + ```shell + ros2 run opendr_perception object_detection_2d_detr + ``` + + The following optional arguments are available for all nodes above: + - `-h or --help`: show a help message and exit + - `-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`) + - `--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: + - Output images: `/opendr/image_objects_annotated` + - Detection messages: `/opendr/objects` + + For viewing the output, refer to the [notes above.](#notes) + +### 2D Single Object Tracking ROS2 Node + +You can find the single object tracking 2D ROS2 node python script [here](./opendr_perception/object_tracking_2d_siamrpn_node.py) to inspect the code and modify it as you wish to fit your needs. +The node makes use of the toolkit's [single object tracking 2D SiamRPN tool](../../../../src/opendr/perception/object_tracking_2d/siamrpn/siamrpn_learner.py) whose documentation can be found [here](../../../../docs/reference/object-tracking-2d-siamrpn.md). + +#### Instructions for basic usage: + +1. Start the node responsible for publishing images. If you have a USB camera, then you can use the `usb_cam_node` as explained in the [prerequisites above](#prerequisites). + +2. You are then ready to start the single object tracking 2D node: + + ```shell + ros2 run opendr_perception object_tracking_2d_siamrpn + ``` + + The following optional arguments are available: + - `-h or --help`: show a help message and exit + - `-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`) + - `--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: + - Output images: `/opendr/image_tracking_annotated` + - Detection messages: `/opendr/tracked_object` + + For viewing the output, refer to the [notes above.](#notes) + +**Notes** + +To initialize this node it is required to provide a bounding box of an object to track. +This is achieved by initializing one of the toolkit's 2D object detectors (YOLOv3) and running object detection once on the input. +Afterwards, **the detected bounding box that is closest to the center of the image** is used to initialize the tracker. +Feel free to modify the node to initialize it in a different way that matches your use case. + +### 2D Object Tracking ROS2 Nodes + +For 2D object tracking, there two ROS2 nodes provided, one using Deep Sort and one using FairMOT which use either pretrained models, or custom trained models. +The predicted tracking annotations are split into two topics with detections and tracking IDs. Additionally, an annotated image is generated. + +You can find the 2D object detection ROS2 node python scripts here: [Deep Sort node](./opendr_perception/object_tracking_2d_deep_sort_node.py) and [FairMOT node](./opendr_perception/object_tracking_2d_fair_mot_node.py) +where you can inspect the code and modify it as you wish to fit your needs. +The nodes makes use of the toolkit's [object tracking 2D - Deep Sort tool](../../../../src/opendr/perception/object_tracking_2d/deep_sort/object_tracking_2d_deep_sort_learner.py) +and [object tracking 2D - FairMOT tool](../../../../src/opendr/perception/object_tracking_2d/fair_mot/object_tracking_2d_fair_mot_learner.py) +whose documentation can be found here: [Deep Sort docs](../../../../docs/reference/object-tracking-2d-deep-sort.md), [FairMOT docs](../../../../docs/reference/object-tracking-2d-fair-mot.md). + +#### Instructions for basic usage: + +1. Start the node responsible for publishing images. If you have a USB camera, then you can use the `usb_cam_node` as explained in the [prerequisites above](#prerequisites). + +2. You are then ready to start a 2D object tracking node: + 1. Deep Sort node + ```shell + ros2 run opendr_perception object_tracking_2d_deep_sort + ``` + The following optional argument is available for the Deep Sort node: + - `-n --model_name MODEL_NAME`: name of the trained model (default=`deep_sort`) + 2. FairMOT node + ```shell + ros2 run opendr_perception object_tracking_2d_fair_mot + ``` + The following optional argument is available for the FairMOT node: + - `-n --model_name MODEL_NAME`: name of the trained model (default=`fairmot_dla34`) + + The following optional arguments are available for both nodes: + - `-h or --help`: show a help message and exit + - `-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`) + - `-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`) + - `--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`) + +3. Default output topics: + - Output images: `/opendr/image_objects_annotated` + - Detection messages: `/opendr/objects` + - Tracking ID messages: `/opendr/objects_tracking_id` + + For viewing the output, refer to the [notes above.](#notes) + +**Notes** + +An [image dataset node](#image-dataset-ros2-node) is also provided to be used along these nodes. +Make sure to change the default input topic of the tracking node if you are not using the USB cam node. + +### Panoptic Segmentation ROS2 Node + +You can find the panoptic segmentation ROS2 node python script [here](./opendr_perception/panoptic_segmentation_efficient_ps_node.py) to inspect the code and modify it as you wish to fit your needs. +The node makes use of the toolkit's [panoptic segmentation tool](../../../../src/opendr/perception/panoptic_segmentation/efficient_ps/efficient_ps_learner.py) whose documentation can be found [here](../../../../docs/reference/efficient-ps.md) +and additional information about Efficient PS [here](../../../../src/opendr/perception/panoptic_segmentation/README.md). + +#### Instructions for basic usage: + +1. Start the node responsible for publishing images. If you have a USB camera, then you can use the `usb_cam_node` as explained in the [prerequisites above](#prerequisites). + +2. You are then ready to start the panoptic segmentation node: + + ```shell + ros2 run opendr_perception panoptic_segmentation_efficient_ps + ``` + + The following optional arguments are available: + - `-h or --help`: show a help message and exit + - `-i or --input_rgb_image_topic INPUT_RGB_IMAGE_TOPIC` : listen to RGB images on this topic (default=`/image_raw`) + - `-oh --output_heatmap_topic OUTPUT_HEATMAP_TOPIC`: publish the semantic and instance maps on this topic as `OUTPUT_HEATMAP_TOPIC/semantic` and `OUTPUT_HEATMAP_TOPIC/instance`, `None` to stop the node from publishing on this topic (default=`/opendr/panoptic`) + - `-ov --output_rgb_image_topic OUTPUT_RGB_IMAGE_TOPIC`: publish the panoptic segmentation map as an RGB image on this topic or a more detailed overview if using the `--detailed_visualization` flag, `None` to stop the node from publishing on this topic (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) + - `--checkpoint CHECKPOINT` : download pretrained models [cityscapes, kitti] or load from the provided path (default=`cityscapes`) + +3. Default output topics: + - Output images: `/opendr/panoptic/semantic`, `/opendr/panoptic/instance`, `/opendr/panoptic/rgb_visualization` + - Detection messages: `/opendr/panoptic/semantic`, `/opendr/panoptic/instance` + + For viewing the output, refer to the [notes above.](#notes) + +### Semantic Segmentation ROS2 Node + +You can find the semantic segmentation ROS2 node python script [here](./opendr_perception/semantic_segmentation_bisenet_node.py) to inspect the code and modify it as you wish to fit your needs. +The node makes use of the toolkit's [semantic segmentation tool](../../../../src/opendr/perception/semantic_segmentation/bisenet/bisenet_learner.py) whose documentation can be found [here](../../../../docs/reference/semantic-segmentation.md). + +#### Instructions for basic usage: + +1. Start the node responsible for publishing images. If you have a USB camera, then you can use the `usb_cam_node` as explained in the [prerequisites above](#prerequisites). + +2. You are then ready to start the semantic segmentation node: + + ```shell + ros2 run opendr_perception semantic_segmentation_bisenet + ``` + The following optional arguments are available: + - `-h or --help`: show a help message and exit + - `-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`) + - `--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: + - Output images: `/opendr/heatmap`, `/opendr/heatmap_visualization` + - Detection messages: `/opendr/heatmap` + + For viewing the output, refer to the [notes above.](#notes) + +**Notes** + +On the table below you can find the detectable classes and their corresponding IDs: + +| Class | Bicyclist | Building | Car | Column Pole | Fence | Pedestrian | Road | Sidewalk | Sign Symbol | Sky | Tree | Unknown | +|--------|-----------|----------|-----|-------------|-------|------------|------|----------|-------------|-----|------|---------| +| **ID** | 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | + +### Image-based Facial Emotion Estimation ROS2 Node + +You can find the image-based facial emotion estimation ROS2 node python script [here](./opendr_perception/facial_emotion_estimation_node.py) to inspect the code and modify it as you wish to fit your needs. +The node makes use of the toolkit's image-based facial emotion estimation tool which can be found [here](../../../../src/opendr/perception/facial_expression_recognition/image_based_facial_emotion_estimation/facial_emotion_learner.py) +whose documentation can be found [here](../../../../docs/reference/image_based_facial_emotion_estimation.md). + +#### Instructions for basic usage: + +1. Start the node responsible for publishing images. If you have a USB camera, then you can use the `usb_cam_node` as explained in the [prerequisites above](#prerequisites). + +2. You are then ready to start the image-based facial emotion estimation node: + + ```shell + ros2 run opendr_perception facial_emotion_estimation + ``` + The following optional arguments are available: + - `-h or --help`: show a help message and exit + - `-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_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`) + - `--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: + - Output images: `/opendr/image_emotion_estimation_annotated` + - Detection messages: `/opendr/facial_emotion_estimation`, `/opendr/facial_emotion_estimation_description` + + For viewing the output, refer to the [notes above.](#notes) + +**Notes** + +This node requires the detection of a face first. This is achieved by including of the toolkit's face detector and running face detection on the input. +Afterwards, the detected bounding box of the face is cropped and fed into the facial emotion estimator. +Feel free to modify the node to detect faces in a different way that matches your use case. + +### Landmark-based Facial Expression Recognition ROS2 Node + +A ROS2 node for performing landmark-based facial expression recognition using a trained model on AFEW, CK+ or Oulu-CASIA datasets. +OpenDR does not include a pretrained model, so one should be provided by the user. +An alternative would be to use the [image-based facial expression estimation node](#image-based-facial-emotion-estimation-ros2-node) provided by the toolkit. + +You can find the landmark-based facial expression recognition ROS2 node python script [here](./opendr_perception/landmark_based_facial_expression_recognition_node.py) to inspect the code and modify it as you wish to fit your needs. +The node makes use of the toolkit's landmark-based facial expression recognition tool which can be found [here](../../../../src/opendr/perception/facial_expression_recognition/landmark_based_facial_expression_recognition/progressive_spatio_temporal_bln_learner.py) +whose documentation can be found [here](../../../../docs/reference/landmark-based-facial-expression-recognition.md). + +#### Instructions for basic usage: + +1. Start the node responsible for publishing images. If you have a USB camera, then you can use the `usb_cam_node` as explained in the [prerequisites above](#prerequisites). + +2. You are then ready to start the landmark-based facial expression recognition node: + + ```shell + ros2 run opendr_perception landmark_based_facial_expression_recognition + ``` + The following optional arguments are available: + - `-h or --help`: show a help message and exit + - `-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`) + - `--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`) + +3. Default output topics: + - Detection messages: `/opendr/landmark_expression_recognition`, `/opendr/landmark_expression_recognition_description` + + For viewing the output, refer to the [notes above.](#notes) + +### Skeleton-based Human Action Recognition ROS2 Node + +A ROS2 node for performing skeleton-based human action recognition using either ST-GCN or PST-GCN models pretrained on NTU-RGBD-60 dataset. +The human body poses of the image are first extracted by the lightweight OpenPose method which is implemented in the toolkit, and they are passed to the skeleton-based action recognition method to be categorized. + +You can find the skeleton-based human action recognition ROS2 node python script [here](./opendr_perception/skeleton_based_action_recognition_node.py) to inspect the code and modify it as you wish to fit your needs. +The node makes use of the toolkit's skeleton-based human action recognition tool which can be found [here for ST-GCN](../../../../src/opendr/perception/skeleton_based_action_recognition/spatio_temporal_gcn_learner.py) +and [here for PST-GCN](../../../../src/opendr/perception/skeleton_based_action_recognition/progressive_spatio_temporal_gcn_learner.py) +whose documentation can be found [here](../../../../docs/reference/skeleton-based-action-recognition.md). + +#### Instructions for basic usage: + +1. Start the node responsible for publishing images. If you have a USB camera, then you can use the `usb_cam_node` as explained in the [prerequisites above](#prerequisites). + +2. You are then ready to start the skeleton-based human action recognition node: + + ```shell + ros2 run opendr_perception skeleton_based_action_recognition + ``` + The following optional arguments are available: + - `-h or --help`: show a help message and exit + - `-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 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`) + - `-c or --output_category_topic OUTPUT_CATEGORY_TOPIC`: topic name for recognized action category, `None` to stop the node from publishing on this topic (default=`"/opendr/skeleton_recognized_action"`) + - `-d or --output_category_description_topic OUTPUT_CATEGORY_DESCRIPTION_TOPIC`: topic name for description of the recognized action category, `None` to stop the node from publishing on this topic (default=`/opendr/skeleton_recognized_action_description`) + - `--model`: model to use, options are `stgcn` or `pstgcn`, (default=`stgcn`) + - `--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: + - Detection messages: `/opendr/skeleton_based_action_recognition`, `/opendr/skeleton_based_action_recognition_description`, `/opendr/poses` + - Output images: `/opendr/image_pose_annotated` + + For viewing the output, refer to the [notes above.](#notes) + +### Video Human Activity Recognition ROS2 Node + +A ROS2 node for performing human activity recognition using either CoX3D or X3D models pretrained on Kinetics400. + +You can find the video human activity recognition ROS2 node python script [here](./opendr_perception/video_activity_recognition_node.py) to inspect the code and modify it as you wish to fit your needs. +The node makes use of the toolkit's video human activity recognition tools which can be found [here for CoX3D](../../../../src/opendr/perception/activity_recognition/cox3d/cox3d_learner.py) and +[here for X3D](../../../../src/opendr/perception/activity_recognition/x3d/x3d_learner.py) whose documentation can be found [here](../../../../docs/reference/activity-recognition.md). + +#### Instructions for basic usage: + +1. Start the node responsible for publishing images. If you have a USB camera, then you can use the `usb_cam_node` as explained in the [prerequisites above](#prerequisites). + +2. You are then ready to start the video human activity recognition node: + + ```shell + ros2 run opendr_perception video_activity_recognition + ``` + The following optional arguments are available: + - `-h or --help`: show a help message and exit + - `-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`) + - `--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`) + +3. Default output topics: + - Detection messages: `/opendr/human_activity_recognition`, `/opendr/human_activity_recognition_description` + + For viewing the output, refer to the [notes above.](#notes) + +**Notes** + +You can find the corresponding IDs regarding activity recognition [here](https://github.com/opendr-eu/opendr/blob/master/src/opendr/perception/activity_recognition/datasets/kinetics400_classes.csv). + +## RGB + Infrared input + +### 2D Object Detection GEM ROS2 Node + +You can find the object detection 2D GEM ROS2 node python script [here](./opendr_perception/object_detection_2d_gem_node.py) to inspect the code and modify it as you wish to fit your needs. +The node makes use of the toolkit's [object detection 2D GEM tool](../../../../src/opendr/perception/object_detection_2d/gem/gem_learner.py) +whose documentation can be found [here](../../../../docs/reference/gem.md). + +#### Instructions for basic usage: + +1. First one needs to find points in the color and infrared images that correspond, in order to find the homography matrix that allows to correct for the difference in perspective between the infrared and the RGB camera. + These points can be selected using a [utility tool](../../../../src/opendr/perception/object_detection_2d/utils/get_color_infra_alignment.py) that is provided in the toolkit. + +2. Pass the points you have found as *pts_color* and *pts_infra* arguments to the [ROS2 GEM node](./opendr_perception/object_detection_2d_gem.py). + +3. Start the node responsible for publishing images. If you have a RealSense camera, then you can use the corresponding node (assuming you have installed [realsense2_camera](http://wiki.ros.org/realsense2_camera)): + + ```shell + roslaunch realsense2_camera rs_camera.launch enable_color:=true enable_infra:=true enable_depth:=false enable_sync:=true infra_width:=640 infra_height:=480 + ``` + +4. You are then ready to start the object detection 2d GEM node: + + ```shell + ros2 run opendr_perception object_detection_2d_gem + ``` + The following optional arguments are available: + - `-h or --help`: show a help message and exit + - `-ic or --input_rgb_image_topic INPUT_RGB_IMAGE_TOPIC`: topic name for input RGB image (default=`/camera/color/image_raw`) + - `-ii or --input_infra_image_topic INPUT_INFRA_IMAGE_TOPIC`: topic name for input infrared image (default=`/camera/infra/image_raw`) + - `-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`) + - `--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: + - Output RGB images: `/opendr/rgb_image_objects_annotated` + - Output infrared images: `/opendr/infra_image_objects_annotated` + - Detection messages: `/opendr/objects` + + For viewing the output, refer to the [notes above.](#notes) + +---- +## RGBD input + +### RGBD Hand Gesture Recognition ROS2 Node +A ROS2 node for performing hand gesture recognition using a MobileNetv2 model trained on HANDS dataset. +The node has been tested with Kinectv2 for depth data acquisition with the following drivers: https://github.com/OpenKinect/libfreenect2 and https://github.com/code-iai/iai_kinect2. + +You can find the RGBD hand gesture recognition ROS2 node python script [here](./opendr_perception/rgbd_hand_gesture_recognition_node.py) to inspect the code and modify it as you wish to fit your needs. +The node makes use of the toolkit's [hand gesture recognition tool](../../../../src/opendr/perception/multimodal_human_centric/rgbd_hand_gesture_learner/rgbd_hand_gesture_learner.py) +whose documentation can be found [here](../../../../docs/reference/rgbd-hand-gesture-learner.md). + +#### Instructions for basic usage: + +1. Start the node responsible for publishing images from an RGBD camera. Remember to modify the input topics using the arguments in step 2 if needed. + +2. You are then ready to start the hand gesture recognition node: + ```shell + ros2 run opendr_perception rgbd_hand_gesture_recognition + ``` + The following optional arguments are available: + - `-h or --help`: show a help message and exit + - `-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`) + - `--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: + - Detection messages:`/opendr/gestures` + + For viewing the output, refer to the [notes above.](#notes) + +---- +## RGB + Audio input + +### Audiovisual Emotion Recognition ROS2 Node + +You can find the audiovisual emotion recognition ROS2 node python script [here](./opendr_perception/audiovisual_emotion_recognition_node.py) to inspect the code and modify it as you wish to fit your needs. +The node makes use of the toolkit's [audiovisual emotion recognition tool](../../../../src/opendr/perception/multimodal_human_centric/audiovisual_emotion_learner/avlearner.py), +whose documentation can be found [here](../../../../docs/reference/audiovisual-emotion-recognition-learner.md). + +#### Instructions for basic usage: + +1. Start the node responsible for publishing images. If you have a USB camera, then you can use the `usb_cam_node` as explained in the [prerequisites above](#prerequisites). +2. Start the node responsible for publishing audio. If you have an audio capture device, then you can use the `audio_capture_node` as explained in the [prerequisites above](#prerequisites). +3. You are then ready to start the audiovisual emotion recognition node + + ```shell + ros2 run opendr_perception audiovisual_emotion_recognition + ``` + The following optional arguments are available: + - `-h or --help`: show a help message and exit + - `-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`) + - `--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 + +4. Default output topics: + - Detection messages: `/opendr/audiovisual_emotion` + + For viewing the output, refer to the [notes above.](#notes) + +---- +## Audio input + +### Speech Command Recognition ROS2 Node + +A ROS2 node for recognizing speech commands from an audio stream using MatchboxNet, EdgeSpeechNets or Quadratic SelfONN models, pretrained on the Google Speech Commands dataset. + +You can find the speech command recognition ROS2 node python script [here](./opendr_perception/speech_command_recognition_node.py) to inspect the code and modify it as you wish to fit your needs. +The node makes use of the toolkit's speech command recognition tools: +[EdgeSpeechNets tool](../../../../src/opendr/perception/speech_recognition/edgespeechnets/edgespeechnets_learner.py), [MatchboxNet tool](../../../../src/opendr/perception/speech_recognition/matchboxnet/matchboxnet_learner.py), [Quadratic SelfONN tool](../../../../src/opendr/perception/speech_recognition/quadraticselfonn/quadraticselfonn_learner.py) +whose documentation can be found here: +[EdgeSpeechNet docs](../../../../docs/reference/edgespeechnets.md), [MatchboxNet docs](../../../../docs/reference/matchboxnet.md), [Quadratic SelfONN docs](../../../../docs/reference/quadratic-selfonn.md). + +#### Instructions for basic usage: + +1. Start the node responsible for publishing audio. If you have an audio capture device, then you can use the `audio_capture_node` as explained in the [prerequisites above](#prerequisites). + +2. You are then ready to start the speech command recognition node + + ```shell + ros2 run opendr_perception speech_command_recognition + ``` + The following optional arguments are available: + - `-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`) + - `--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 + +3. Default output topics: + - Detection messages, class id and confidence: `/opendr/speech_recognition` + + For viewing the output, refer to the [notes above.](#notes) + +**Notes** + +EdgeSpeechNets currently does not have a pretrained model available for download, only local files may be used. + +---- +## Point cloud input + +### 3D Object Detection Voxel ROS2 Node + +A ROS2 node for performing 3D object detection Voxel using PointPillars or TANet methods with either pretrained models on KITTI dataset, or custom trained models. + +You can find the 3D object detection Voxel ROS2 node python script [here](./opendr_perception/object_detection_3d_voxel_node.py) to inspect the code and modify it as you wish to fit your needs. +The node makes use of the toolkit's [3D object detection Voxel tool](../../../../src/opendr/perception/object_detection_3d/voxel_object_detection_3d/voxel_object_detection_3d_learner.py) +whose documentation can be found [here](../../../../docs/reference/voxel-object-detection-3d.md). + +#### Instructions for basic usage: + +1. Start the node responsible for publishing point clouds. OpenDR provides a [point cloud dataset node](#point-cloud-dataset-ros2-node) for convenience. + +2. You are then ready to start the 3D object detection node: + + ```shell + ros2 run opendr_perception object_detection_3d_voxel + ``` + The following optional arguments are available: + - `-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`) + - `--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`) + +3. Default output topics: + - Detection messages: `/opendr/objects3d` + + For viewing the output, refer to the [notes above.](#notes) + +### 3D Object Tracking AB3DMOT ROS2 Node + +A ROS2 node for performing 3D object tracking using AB3DMOT stateless method. +This is a detection-based method, and therefore the 3D object detector is needed to provide detections, which then will be used to make associations and generate tracking ids. +The predicted tracking annotations are split into two topics with detections and tracking IDs. + +You can find the 3D object tracking AB3DMOT ROS2 node python script [here](./opendr_perception/object_tracking_3d_ab3dmot_node.py) to inspect the code and modify it as you wish to fit your needs. +The node makes use of the toolkit's [3D object tracking AB3DMOT tool](../../../../src/opendr/perception/object_tracking_3d/ab3dmot/object_tracking_3d_ab3dmot_learner.py) +whose documentation can be found [here](../../../../docs/reference/object-tracking-3d-ab3dmot.md). + +#### Instructions for basic usage: + +1. Start the node responsible for publishing point clouds. OpenDR provides a [point cloud dataset node](#point-cloud-dataset-ros2-node) for convenience. + +2. You are then ready to start the 3D object tracking node: + + ```shell + ros2 run opendr_perception object_tracking_3d_ab3dmot + ``` + The following optional arguments are available: + - `-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, `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`) + - `--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`) + +3. Default output topics: + - Detection messages: `/opendr/objects3d` + - Tracking ID messages: `/opendr/objects_tracking_id` + + For viewing the output, refer to the [notes above.](#notes) + +---- +## Biosignal input + +### Heart Anomaly Detection ROS2 Node + +A ROS2 node for performing heart anomaly (atrial fibrillation) detection from ECG data using GRU or ANBOF models trained on AF dataset. + +You can find the heart anomaly detection ROS2 node python script [here](./opendr_perception/heart_anomaly_detection_node.py) to inspect the code and modify it as you wish to fit your needs. +The node makes use of the toolkit's heart anomaly detection tools: [ANBOF tool](../../../../src/opendr/perception/heart_anomaly_detection/attention_neural_bag_of_feature/attention_neural_bag_of_feature_learner.py) and +[GRU tool](../../../../src/opendr/perception/heart_anomaly_detection/gated_recurrent_unit/gated_recurrent_unit_learner.py), whose documentation can be found here: +[ANBOF docs](../../../../docs/reference/attention-neural-bag-of-feature-learner.md) and [GRU docs](../../../../docs/reference/gated-recurrent-unit-learner.md). + +#### Instructions for basic usage: + +1. Start the node responsible for publishing ECG data. + +2. You are then ready to start the heart anomaly detection node: + + ```shell + ros2 run opendr_perception heart_anomaly_detection + ``` + The following optional arguments are available: + - `-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`) + - `--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`) + +3. Default output topics: + - Detection messages: `/opendr/heart_anomaly` + + For viewing the output, refer to the [notes above.](#notes) + +---- +## Dataset ROS2 Nodes + +The dataset nodes can be used to publish data from the disk, which is useful to test the functionality without the use of a sensor. +Dataset nodes use a provided `DatasetIterator` object that returns a `(Data, Target)` pair. +If the type of the `Data` object is correct, the node will transform it into a corresponding ROS2 message object and publish it to a desired topic. +The OpenDR toolkit currently provides two such nodes, an image dataset node and a point cloud dataset node. + +### Image Dataset ROS2 Node + +The image dataset node downloads a `nano_MOT20` dataset from OpenDR's FTP server and uses it to publish data to the ROS2 topic, +which is intended to be used with the [2D object tracking nodes](#2d-object-tracking-ros2-nodes). + +You can create an instance of this node with any `DatasetIterator` object that returns `(Image, Target)` as elements, +to use alongside other nodes and datasets. +You can inspect [the node](./opendr_perception/image_dataset_node.py) and modify it to your needs for other image datasets. + +To get an image from a dataset on the disk, you can start a `image_dataset.py` node as: +```shell +ros2 run opendr_perception image_dataset +``` +The following optional arguments are available: + - `-h or --help`: show a help message and exit + - `-o or --output_rgb_image_topic`: topic name to publish the data (default=`/opendr/dataset_image`) + - `-f or --fps FPS`: data fps (default=`10`) + - `-d or --dataset_path DATASET_PATH`: path to a dataset (default=`/MOT`) + - `-ks or --mot20_subsets_path MOT20_SUBSETS_PATH`: path to MOT20 subsets (default=`../../src/opendr/perception/object_tracking_2d/datasets/splits/nano_mot20.train`) + +### Point Cloud Dataset ROS2 Node + +The point cloud dataset node downloads a `nano_KITTI` dataset from OpenDR's FTP server and uses it to publish data to the ROS2 topic, +which is intended to be used with the [3D object detection node](#3d-object-detection-voxel-ros2-node), +as well as the [3D object tracking node](#3d-object-tracking-ab3dmot-ros2-node). + +You can create an instance of this node with any `DatasetIterator` object that returns `(PointCloud, Target)` as elements, +to use alongside other nodes and datasets. +You can inspect [the node](./opendr_perception/point_cloud_dataset_node.py) and modify it to your needs for other point cloud datasets. + +To get a point cloud from a dataset on the disk, you can start a `point_cloud_dataset.py` node as: +```shell +ros2 run opendr_perception point_cloud_dataset +``` +The following optional arguments are available: + - `-h or --help`: show a help message and exit + - `-o or --output_point_cloud_topic`: topic name to publish the data (default=`/opendr/dataset_point_cloud`) + - `-f or --fps FPS`: data fps (default=`10`) + - `-d or --dataset_path DATASET_PATH`: path to a dataset, if it does not exist, nano KITTI dataset will be downloaded there (default=`/KITTI/opendr_nano_kitti`) + - `-ks or --kitti_subsets_path KITTI_SUBSETS_PATH`: path to KITTI subsets, used only if a KITTI dataset is downloaded (default=`../../src/opendr/perception/object_detection_3d/datasets/nano_kitti_subsets`) diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/__init__.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/__init__.py new file mode 100644 index 0000000000..e69de29bb2 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 new file mode 100644 index 0000000000..008b51d7b7 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/audiovisual_emotion_recognition_node.py @@ -0,0 +1,167 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import argparse +import numpy as np +import torch +import librosa +import cv2 + +import rclpy +from rclpy.node import Node +import message_filters +from sensor_msgs.msg import Image as ROS_Image +from audio_common_msgs.msg import AudioData +from vision_msgs.msg import Classification2D + +from opendr_bridge import ROS2Bridge +from opendr.perception.multimodal_human_centric import AudiovisualEmotionLearner +from opendr.perception.multimodal_human_centric import spatial_transforms as transforms +from opendr.engine.data import Video, Timeseries + + +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): + """ + 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 + :type input_video_topic: str + :param input_audio_topic: Topic from which we are reading the input 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 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') + :type device: str + :param delay: Define the delay (in seconds) with which rgb message and depth message can be synchronized + :type delay: float + """ + super().__init__("opendr_audiovisual_emotion_recognition_node") + + self.publisher = self.create_publisher(Classification2D, output_emotions_topic, 1) + + video_sub = message_filters.Subscriber(self, ROS_Image, input_video_topic, qos_profile=1) + audio_sub = message_filters.Subscriber(self, AudioData, input_audio_topic, qos_profile=1) + # synchronize video and audio data topics + ts = message_filters.ApproximateTimeSynchronizer([video_sub, audio_sub], queue_size=10, slop=delay, + allow_headerless=True) + ts.registerCallback(self.callback) + + self.bridge = ROS2Bridge() + + self.avlearner = AudiovisualEmotionLearner(device=device, fusion='ia', mod_drop='zerodrop') + if not os.path.exists('model'): + self.avlearner.download('model') + self.avlearner.load('model') + + self.buffer_size = buffer_size + self.data_buffer = np.zeros((1)) + self.video_buffer = np.zeros((1, 224, 224, 3)) + + self.video_transform = transforms.Compose([ + transforms.ToTensor(255)]) + + self.get_logger().info("Audiovisual emotion recognition node started!") + + def callback(self, image_data, audio_data): + """ + Callback that process the input data and publishes to the corresponding topics + :param image_data: input image message, face image + :type image_data: sensor_msgs.msg.Image + :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)) + 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') + image_data = cv2.resize(image_data, (224, 224)) + + 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: + 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) + video = self.video_buffer[1:][to_select] + + video = [self.video_transform(img) for img in video] + video = Video(torch.stack(video, 0).permute(1, 0, 2, 3)) + + class_pred = self.avlearner.infer(audio, video) + + # Publish output + ros_class = self.bridge.from_category_to_rosclass(class_pred, self.get_clock().now().to_msg()) + self.publisher.publish(ros_class) + + self.data_buffer = np.zeros((1)) + 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 main(args=None): + rclpy.init(args=args) + + parser = argparse.ArgumentParser() + parser.add_argument("-iv", "--input_video_topic", type=str, default="/image_raw", + help="Listen to video input data on this topic") + parser.add_argument("-ia", "--input_audio_topic", type=str, default="/audio", + 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("--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) + + args = parser.parse_args() + + try: + if args.device == "cuda" and torch.cuda.is_available(): + device = "cuda" + elif args.device == "cuda": + print("GPU not found. Using CPU instead.") + device = "cpu" + else: + print("Using CPU") + device = "cpu" + except: + print("Using CPU") + device = "cpu" + + emotion_node = AudiovisualEmotionNode(input_video_topic=args.input_video_topic, + input_audio_topic=args.input_audio_topic, + output_emotions_topic=args.output_emotions_topic, + buffer_size=args.buffer_size, device=device, delay=args.delay) + + rclpy.spin(emotion_node) + + emotion_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() 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 new file mode 100644 index 0000000000..b4c20114c8 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/face_detection_retinaface_node.py @@ -0,0 +1,148 @@ +#!/usr/bin/env python +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import argparse +import mxnet as mx + +import rclpy +from rclpy.node import Node + +from sensor_msgs.msg import Image as ROS_Image +from vision_msgs.msg import Detection2DArray +from opendr_bridge import ROS2Bridge + +from opendr.perception.object_detection_2d import RetinaFaceLearner +from opendr.perception.object_detection_2d import draw_bounding_boxes +from opendr.engine.data import Image + + +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"): + """ + Creates a ROS2 Node for face detection with Retinaface. + :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 + image is published) + :type output_rgb_image_topic: str + :param detections_topic: Topic to which we are publishing the annotations (if None, no face detection message + is published) + :type detections_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', + where 'mnet' detects masked faces as well + :type backbone: str + """ + super().__init__('opendr_face_detection_node') + + self.image_subscriber = self.create_subscription(ROS_Image, input_rgb_image_topic, self.callback, 1) + + if output_rgb_image_topic is not None: + self.image_publisher = self.create_publisher(ROS_Image, output_rgb_image_topic, 1) + else: + self.image_publisher = None + + if detections_topic is not None: + self.face_publisher = self.create_publisher(Detection2DArray, detections_topic, 1) + else: + self.face_publisher = None + + self.bridge = ROS2Bridge() + + self.face_detector = RetinaFaceLearner(backbone=backbone, device=device) + self.face_detector.download(path=".", verbose=True) + self.face_detector.load("retinaface_{}".format(backbone)) + self.class_names = ["face", "masked_face"] + + self.get_logger().info("Face detection retinaface node initialized.") + + def callback(self, data): + """ + Callback that process the input data and publishes to the corresponding topics. + :param data: Input image message + :type data: sensor_msgs.msg.Image + """ + # 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.face_publisher is not None: + # Publish detections in ROS message + ros_boxes = self.bridge.to_ros_boxes(boxes) # Convert to ROS boxes + self.face_publisher.publish(ros_boxes) + + if self.image_publisher is not None: + # Get an OpenCV image back + image = image.opencv() + # Annotate image with face 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 + self.image_publisher.publish(self.bridge.to_ros_image(Image(image), encoding='bgr8')) + + +def main(args=None): + rclpy.init(args=args) + + parser = argparse.ArgumentParser() + parser.add_argument("-i", "--input_rgb_image_topic", help="Topic name for input rgb image", + type=str, default="image_raw") + parser.add_argument("-o", "--output_rgb_image_topic", help="Topic name for output annotated rgb image", + type=lambda value: value if value.lower() != "none" else None, + default="/opendr/image_faces_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/faces") + 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="Retinaface backbone, options are either 'mnet' or 'resnet', where 'mnet' detects " + "masked faces as well", + type=str, default="resnet", choices=["resnet", "mnet"]) + args = parser.parse_args() + + try: + if args.device == "cuda" and mx.context.num_gpus() > 0: + device = "cuda" + elif args.device == "cuda": + print("GPU not found. Using CPU instead.") + device = "cpu" + else: + print("Using CPU.") + device = "cpu" + except: + print("Using CPU.") + device = "cpu" + + 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) + + rclpy.spin(face_detection_node) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + face_detection_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() 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 new file mode 100644 index 0000000000..b774ea0eb9 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/face_recognition_node.py @@ -0,0 +1,193 @@ +#!/usr/bin/env python +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import cv2 +import argparse +import torch + +import rclpy +from rclpy.node import Node + +from std_msgs.msg import String +from sensor_msgs.msg import Image as ROS_Image +from vision_msgs.msg import ObjectHypothesisWithPose +from opendr_bridge import ROS2Bridge + +from opendr.engine.data import Image +from opendr.perception.face_recognition import FaceRecognitionLearner +from opendr.perception.object_detection_2d import RetinaFaceLearner +from opendr.perception.object_detection_2d.datasets.transforms import BoundingBoxListToNumpyArray + + +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"): + """ + Creates a ROS2 Node for face recognition. + :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 + image is published) + :type output_rgb_image_topic: str + :param detections_topic: Topic to which we are publishing the recognized face information (if None, + no face recognition message is published) + :type detections_topic: str + :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 device: Device on which we are running inference ('cpu' or 'cuda') + :type device: str + :param backbone: Backbone network + :type backbone: str + :param database_path: Path of the directory where the images of the faces to be recognized are stored + :type database_path: str + """ + super().__init__('opendr_face_recognition_node') + + self.image_subscriber = self.create_subscription(ROS_Image, input_rgb_image_topic, self.callback, 1) + + if output_rgb_image_topic is not None: + self.image_publisher = self.create_publisher(ROS_Image, output_rgb_image_topic, 1) + else: + self.image_publisher = None + + if detections_topic is not None: + self.face_publisher = self.create_publisher(ObjectHypothesisWithPose, detections_topic, 1) + else: + self.face_publisher = None + + if detections_id_topic is not None: + + self.face_id_publisher = self.create_publisher(String, detections_id_topic, 1) + else: + self.face_id_publisher = None + + self.bridge = ROS2Bridge() + + # Initialize the face recognizer + self.recognizer = FaceRecognitionLearner(device=device, mode='backbone_only', backbone=backbone) + self.recognizer.download(path=".") + self.recognizer.load(".") + self.recognizer.fit_reference(database_path, save_path=".", create_new=True) + + # Initialize the face detector + self.face_detector = RetinaFaceLearner(backbone='mnet', device=device) + self.face_detector.download(path=".", verbose=True) + self.face_detector.load("retinaface_{}".format('mnet')) + self.class_names = ["face", "masked_face"] + + self.get_logger().info("Face recognition node initialized.") + + def callback(self, data): + """ + Callback that process the input data and publishes to the corresponding topics. + :param data: Input image message + :type data: sensor_msgs.msg.Image + """ + # Convert sensor_msgs.msg.Image into OpenDR Image + image = self.bridge.from_ros_image(data, encoding='bgr8') + # Get an OpenCV image back + image = image.opencv() + + # Run face detection and recognition + if image is not None: + bounding_boxes = self.face_detector.infer(image) + if bounding_boxes: + bounding_boxes = BoundingBoxListToNumpyArray()(bounding_boxes) + boxes = bounding_boxes[:, :4] + for idx, box in enumerate(boxes): + (startX, startY, endX, endY) = int(box[0]), int(box[1]), int(box[2]), int(box[3]) + frame = image[startY:endY, startX:endX] + result = self.recognizer.infer(frame) + + # Publish face information and ID + if self.face_publisher is not None: + self.face_publisher.publish(self.bridge.to_ros_face(result)) + + if self.face_id_publisher is not None: + self.face_id_publisher.publish(self.bridge.to_ros_face_id(result)) + + if self.image_publisher is not None: + if result.description != 'Not found': + color = (0, 255, 0) + else: + color = (0, 0, 255) + # Annotate image with face detection/recognition boxes + cv2.rectangle(image, (startX, startY), (endX, endY), color, thickness=2) + cv2.putText(image, result.description, (startX, endY - 10), cv2.FONT_HERSHEY_SIMPLEX, + 1, color, 2, cv2.LINE_AA) + + if self.image_publisher is not None: + # 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')) + + +def main(args=None): + rclpy.init(args=args) + + parser = argparse.ArgumentParser() + parser.add_argument("-i", "--input_rgb_image_topic", help="Topic name for input rgb image", + type=str, default="image_raw") + parser.add_argument("-o", "--output_rgb_image_topic", help="Topic name for output annotated rgb image", + type=lambda value: value if value.lower() != "none" else None, + default="/opendr/image_face_reco_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/face_recognition") + 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("--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", + type=str, default="mobilefacenet", choices=["mobilefacenet"]) + parser.add_argument("--dataset_path", + help="Path of the directory where the images of the faces to be recognized are stored, " + "defaults to \"./database\"", + type=str, default="./database") + args = parser.parse_args() + + try: + if args.device == "cuda" and torch.cuda.is_available(): + device = "cuda" + elif args.device == "cuda": + print("GPU not found. Using CPU instead.") + device = "cpu" + else: + print("Using CPU.") + device = "cpu" + except: + print("Using CPU.") + device = "cpu" + + face_recognition_node = FaceRecognitionNode(device=device, backbone=args.backbone, database_path=args.dataset_path, + 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) + + rclpy.spin(face_recognition_node) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + face_recognition_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() 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 new file mode 100644 index 0000000000..56b22309c0 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/facial_emotion_estimation_node.py @@ -0,0 +1,218 @@ +#!/usr/bin/env python +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import argparse +import torch +import numpy as np +import cv2 +from torchvision import transforms +import PIL + +import rclpy +from rclpy.node import Node +from std_msgs.msg import String +from vision_msgs.msg import ObjectHypothesis +from sensor_msgs.msg import Image as ROS_Image +from opendr_bridge import ROS2Bridge + +from opendr.engine.data import Image +from opendr.perception.facial_expression_recognition import FacialEmotionLearner +from opendr.perception.facial_expression_recognition import image_processing +from opendr.perception.object_detection_2d import RetinaFaceLearner +from opendr.perception.object_detection_2d.datasets.transforms import BoundingBoxListToNumpyArray + +INPUT_IMAGE_SIZE = (96, 96) +INPUT_IMAGE_NORMALIZATION_MEAN = [0.0, 0.0, 0.0] +INPUT_IMAGE_NORMALIZATION_STD = [1.0, 1.0, 1.0] + + +class FacialEmotionEstimationNode(Node): + def __init__(self, + face_detector_learner, + input_rgb_image_topic="/image_raw", + 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", + device="cuda"): + """ + Creates a ROS Node for facial emotion 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 + image is published) + :type output_rgb_image_topic: str + :param output_emotions_topic: Topic to which we are publishing the facial emotion results + (if None, we are not publishing the info) + :type output_emotions_topic: str + :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 device: device on which we are running inference ('cpu' or 'cuda') + :type device: str + """ + super().__init__('opendr_facial_emotion_estimation_node') + + self.image_subscriber = self.create_subscription(ROS_Image, input_rgb_image_topic, self.callback, 1) + self.bridge = ROS2Bridge() + + if output_rgb_image_topic is not None: + self.image_publisher = self.create_publisher(ROS_Image, output_rgb_image_topic, 1) + else: + self.image_publisher = None + + if output_emotions_topic is not None: + self.hypothesis_publisher = self.create_publisher(ObjectHypothesis, output_emotions_topic, 1) + else: + self.hypothesis_publisher = None + + if output_emotions_description_topic is not None: + self.string_publisher = self.create_publisher(String, output_emotions_description_topic, 1) + else: + self.string_publisher = None + + # Initialize the face detector + self.face_detector = face_detector_learner + + # Initialize the facial emotion estimator + self.facial_emotion_estimator = FacialEmotionLearner(device=device, batch_size=2, + ensemble_size=9, + name_experiment='esr_9') + self.facial_emotion_estimator.init_model(num_branches=9) + model_saved_path = self.facial_emotion_estimator.download(path=None, mode="pretrained") + self.facial_emotion_estimator.load(ensemble_size=9, path_to_saved_network=model_saved_path) + + self.get_logger().info("Facial emotion estimation node started.") + + def callback(self, data): + """ + Callback that processes the input data and publishes to the corresponding topics. + :param data: input message + :type data: sensor_msgs.msg.Image + """ + + # Convert sensor_msgs.msg.Image into OpenDR Image + image = self.bridge.from_ros_image(data, encoding='bgr8').opencv() + emotion = None + # Run face detection and emotion estimation + + if image is not None: + bounding_boxes = self.face_detector.infer(image) + if bounding_boxes: + bounding_boxes = BoundingBoxListToNumpyArray()(bounding_boxes) + boxes = bounding_boxes[:, :4] + for idx, box in enumerate(boxes): + (startX, startY, endX, endY) = int(box[0]), int(box[1]), int(box[2]), int(box[3]) + face_crop = image[startY:endY, startX:endX] + + # Preprocess detected face + input_face = _pre_process_input_image(face_crop) + + # Recognize facial expression + emotion, affect = self.facial_emotion_estimator.infer(input_face) + + # Converts from Tensor to ndarray + affect = np.array([a.cpu().detach().numpy() for a in affect]) + affect = affect[0] # a numpy array of valence and arousal values + emotion = emotion[0] # the emotion class with confidence tensor + + 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) + cv2.putText(image, "Arousal: %.2f" % affect[1], (startX, endY - 15), cv2.FONT_HERSHEY_SIMPLEX, + 0.5, (0, 255, 255), 1, cv2.LINE_AA) + cv2.putText(image, emotion.description, (startX, endY), cv2.FONT_HERSHEY_SIMPLEX, + 0.5, (0, 255, 255), 1, cv2.LINE_AA) + + if self.hypothesis_publisher is not None and emotion: + self.hypothesis_publisher.publish(self.bridge.to_ros_category(emotion)) + + if self.string_publisher is not None and emotion: + self.string_publisher.publish(self.bridge.to_ros_category_description(emotion)) + + if self.image_publisher is not None: + # 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')) + + +def _pre_process_input_image(image): + """ + Pre-processes an image for ESR-9. + + :param image: (ndarray) + :return: (ndarray) image + """ + + image = image_processing.resize(image, INPUT_IMAGE_SIZE) + image = PIL.Image.fromarray(image) + image = transforms.Normalize(mean=INPUT_IMAGE_NORMALIZATION_MEAN, + std=INPUT_IMAGE_NORMALIZATION_STD)(transforms.ToTensor()(image)).unsqueeze(0) + + return image + + +def main(args=None): + rclpy.init(args=args) + + parser = argparse.ArgumentParser() + parser.add_argument('-i', '--input_rgb_image_topic', type=str, help='Topic name for input rgb image', + default='/image_raw') + parser.add_argument("-o", "--output_rgb_image_topic", help="Topic name for output annotated rgb image", + type=lambda value: value if value.lower() != "none" else None, + default="/opendr/image_emotion_estimation_annotated") + parser.add_argument("-e", "--output_emotions_topic", help="Topic name for output emotion", + type=lambda value: value if value.lower() != "none" else None, + default="/opendr/facial_emotion_estimation") + parser.add_argument('-m', '--output_emotions_description_topic', + 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('-d', '--device', help='Device to use, either cpu or cuda', + type=str, default="cuda", choices=["cuda", "cpu"]) + args = parser.parse_args() + + try: + if args.device == "cuda" and torch.cuda.is_available(): + print("GPU found.") + device = 'cuda' + elif args.device == "cuda": + print("GPU not found. Using CPU instead.") + device = "cpu" + else: + print("Using CPU") + device = 'cpu' + except: + print("Using CPU") + device = 'cpu' + + # Initialize the face detector + face_detector = RetinaFaceLearner(backbone="resnet", device=device) + face_detector.download(path=".", verbose=True) + face_detector.load("retinaface_{}".format("resnet")) + + facial_emotion_estimation_node = FacialEmotionEstimationNode( + face_detector, + input_rgb_image_topic=args.input_rgb_image_topic, + 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, + device=device) + + rclpy.spin(facial_emotion_estimation_node) + facial_emotion_estimation_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() 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 new file mode 100644 index 0000000000..3057bc7f83 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/fall_detection_node.py @@ -0,0 +1,189 @@ +#!/usr/bin/env python +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import cv2 +import argparse +import torch + +import rclpy +from rclpy.node import Node + +from sensor_msgs.msg import Image as ROS_Image +from vision_msgs.msg import Detection2DArray +from opendr_bridge import ROS2Bridge + +from opendr.engine.data import Image +from opendr.engine.target import BoundingBox, BoundingBoxList +from opendr.perception.pose_estimation import get_bbox +from opendr.perception.pose_estimation import LightweightOpenPoseLearner +from opendr.perception.fall_detection import FallDetectorLearner + + +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", + num_refinement_stages=2, use_stride=False, half_precision=False): + """ + Creates a ROS2 Node for rule-based fall detection based on Lightweight OpenPose. + :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 + image is published) + :type output_rgb_image_topic: str + :param detections_topic: Topic to which we are publishing the annotations (if None, no pose detection message + is published) + :type detections_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 + model's head, including the initial stage. Can be 0, 1 or 2, with more stages meaning slower and more accurate + inference + :type num_refinement_stages: int + :param use_stride: Whether to add a stride value in the model, which reduces accuracy but increases + inference speed + :type use_stride: bool + :param half_precision: Enables inference using half (fp16) precision instead of single (fp32) precision. + Valid only for GPU-based inference + :type half_precision: bool + """ + super().__init__('opendr_fall_detection_node') + + self.image_subscriber = self.create_subscription(ROS_Image, input_rgb_image_topic, self.callback, 1) + + if output_rgb_image_topic is not None: + self.image_publisher = self.create_publisher(ROS_Image, output_rgb_image_topic, 1) + else: + self.image_publisher = None + + if detections_topic is not None: + self.fall_publisher = self.create_publisher(Detection2DArray, detections_topic, 1) + else: + self.fall_publisher = None + + self.bridge = ROS2Bridge() + + # Initialize the pose estimation learner + self.pose_estimator = LightweightOpenPoseLearner(device=device, num_refinement_stages=num_refinement_stages, + mobilenet_use_stride=use_stride, + half_precision=half_precision) + self.pose_estimator.download(path=".", verbose=True) + self.pose_estimator.load("openpose_default") + + # Initialize the fall detection learner + self.fall_detector = FallDetectorLearner(self.pose_estimator) + + self.get_logger().info("Fall detection node initialized.") + + def callback(self, data): + """ + Callback that process the input data and publishes to the corresponding topics. + :param data: Input image message + :type data: sensor_msgs.msg.Image + """ + # 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[2] + x, y, w, h = get_bbox(pose) + + if fallen == 1: + if self.image_publisher is not None: + # Paint person bounding box inferred from pose + color = (0, 0, 255) + cv2.rectangle(image, (x, y), (x + w, y + h), color, 2) + cv2.putText(image, "Fallen person", (x, y + h - 10), cv2.FONT_HERSHEY_SIMPLEX, + 1, color, 2, cv2.LINE_AA) + + if self.fall_publisher is not None: + # Convert detected boxes to ROS type and add to list + bboxes.data.append(BoundingBox(left=x, top=y, width=w, height=h, name=fallen_pose_id)) + fallen_pose_id += 1 + + if self.fall_publisher is not None: + if len(bboxes) > 0: + self.fall_publisher.publish(self.bridge.to_ros_boxes(bboxes)) + + if self.image_publisher is not None: + self.image_publisher.publish(self.bridge.to_ros_image(Image(image), encoding='bgr8')) + + +def main(args=None): + rclpy.init(args=args) + + parser = argparse.ArgumentParser() + parser.add_argument("-i", "--input_rgb_image_topic", help="Topic name for input rgb image", + type=str, default="image_raw") + parser.add_argument("-o", "--output_rgb_image_topic", help="Topic name for output annotated rgb image", + type=lambda value: value if value.lower() != "none" else None, + default="/opendr/image_fallen_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/fallen") + 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, + action="store_true") + args = parser.parse_args() + + try: + if args.device == "cuda" and torch.cuda.is_available(): + device = "cuda" + elif args.device == "cuda": + print("GPU not found. Using CPU instead.") + device = "cpu" + else: + print("Using CPU.") + device = "cpu" + except: + print("Using CPU.") + device = "cpu" + + if args.accelerate: + stride = True + stages = 0 + half_prec = True + else: + stride = False + stages = 2 + half_prec = False + + fall_detection_node = FallDetectionNode(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) + + rclpy.spin(fall_detection_node) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + fall_detection_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() 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 new file mode 100644 index 0000000000..7934c8ac19 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/heart_anomaly_detection_node.py @@ -0,0 +1,123 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*-_ +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import argparse +import torch + +import rclpy +from rclpy.node import Node +from vision_msgs.msg import Classification2D +from std_msgs.msg import Float32MultiArray + +from opendr_bridge import ROS2Bridge +from opendr.perception.heart_anomaly_detection import GatedRecurrentUnitLearner, AttentionNeuralBagOfFeatureLearner + + +class HeartAnomalyNode(Node): + + def __init__(self, input_ecg_topic="/ecg/ecg", output_heart_anomaly_topic="/opendr/heart_anomaly", + 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 device: device on which we are running inference ('cpu' or 'cuda') + :type device: str + :param model: model to use: anbof or gru + :type model: str + """ + super().__init__("opendr_heart_anomaly_detection_node") + + self.publisher = self.create_publisher(Classification2D, output_heart_anomaly_topic, 1) + + self.subscriber = self.create_subscription(Float32MultiArray, input_ecg_topic, self.callback, 1) + + self.bridge = ROS2Bridge() + + # AF dataset + self.channels = 1 + self.series_length = 9000 + + if model == 'gru': + self.learner = GatedRecurrentUnitLearner(in_channels=self.channels, series_length=self.series_length, + n_class=4, device=device) + elif model == 'anbof': + self.learner = AttentionNeuralBagOfFeatureLearner(in_channels=self.channels, series_length=self.series_length, + n_class=4, device=device, attention_type='temporal') + + self.learner.download(path='.', fold_idx=0) + self.learner.load(path='.') + + self.get_logger().info("Heart anomaly detection node initialized.") + + def callback(self, msg_data): + """ + Callback that process the input data and publishes to the corresponding topics + :param msg_data: input message + :type msg_data: std_msgs.msg.Float32MultiArray + """ + # 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) + + # Publish results + ros_class = self.bridge.from_category_to_rosclass(class_pred, self.get_clock().now().to_msg()) + self.publisher.publish(ros_class) + + +def main(args=None): + rclpy.init(args=args) + + parser = argparse.ArgumentParser() + parser.add_argument("-i", "--input_ecg_topic", type=str, default="/ecg/ecg", + 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("--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)", + choices=["cuda", "cpu"]) + args = parser.parse_args() + + try: + if args.device == "cuda" and torch.cuda.is_available(): + device = "cuda" + elif args.device == "cuda": + print("GPU not found. Using CPU instead.") + device = "cpu" + else: + print("Using CPU") + device = "cpu" + except: + print("Using CPU") + device = "cpu" + + heart_anomaly_detection_node = HeartAnomalyNode(input_ecg_topic=args.input_ecg_topic, + output_heart_anomaly_topic=args.output_heart_anomaly_topic, + model=args.model, device=device) + + rclpy.spin(heart_anomaly_detection_node) + + heart_anomaly_detection_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() 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 new file mode 100644 index 0000000000..f8c6a1e30e --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/hr_pose_estimation_node.py @@ -0,0 +1,173 @@ +#!/usr/bin/env python +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import argparse +import torch + +import rclpy +from rclpy.node import Node + +from sensor_msgs.msg import Image as ROS_Image +from opendr_bridge import ROS2Bridge +from opendr_interface.msg import OpenDRPose2D + +from opendr.engine.data import Image +from opendr.perception.pose_estimation import draw +from opendr.perception.pose_estimation import HighResolutionPoseEstimationLearner + + +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", + num_refinement_stages=2, use_stride=False, half_precision=False): + """ + Creates a ROS2 Node for pose estimation with Lightweight OpenPose. + :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 + image is published) + :type output_rgb_image_topic: str + :param detections_topic: Topic to which we are publishing the annotations (if None, no pose detection message + is published) + :type detections_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 + model's head, including the initial stage. Can be 0, 1 or 2, with more stages meaning slower and more accurate + inference + :type num_refinement_stages: int + :param use_stride: Whether to add a stride value in the model, which reduces accuracy but increases + inference speed + :type use_stride: bool + :param half_precision: Enables inference using half (fp16) precision instead of single (fp32) precision. + Valid only for GPU-based inference + :type half_precision: bool + """ + super().__init__('opendr_pose_estimation_node') + + self.image_subscriber = self.create_subscription(ROS_Image, input_rgb_image_topic, self.callback, 1) + + if output_rgb_image_topic is not None: + self.image_publisher = self.create_publisher(ROS_Image, output_rgb_image_topic, 1) + else: + self.image_publisher = None + + if detections_topic is not None: + self.pose_publisher = self.create_publisher(OpenDRPose2D, detections_topic, 1) + else: + self.pose_publisher = None + + self.bridge = ROS2Bridge() + + # Initialize the high resolution pose estimation learner + self.pose_estimator = HighResolutionPoseEstimationLearner(device=device, num_refinement_stages=num_refinement_stages, + mobilenet_use_stride=use_stride, + half_precision=half_precision) + self.pose_estimator.download(path=".", verbose=True) + self.pose_estimator.load("openpose_default") + + self.get_logger().info("Pose estimation node initialized.") + + def callback(self, data): + """ + Callback that process the input data and publishes to the corresponding topics. + :param data: Input image message + :type data: sensor_msgs.msg.Image + """ + # 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) + + # Publish detections in ROS message + if self.pose_publisher is not None: + for pose in poses: + if pose.id is None: # Temporary fix for pose not having id + pose.id = -1 + if self.pose_publisher is not None: + # Convert OpenDR pose to ROS2 pose message using bridge and publish it + self.pose_publisher.publish(self.bridge.to_ros_pose(pose)) + + if self.image_publisher is not None: + # Get an OpenCV image back + image = image.opencv() + # Annotate image with poses + for pose in poses: + draw(image, pose) + # 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')) + + +def main(args=None): + rclpy.init(args=args) + + parser = argparse.ArgumentParser() + parser.add_argument("-i", "--input_rgb_image_topic", help="Topic name for input rgb image", + type=str, default="image_raw") + parser.add_argument("-o", "--output_rgb_image_topic", help="Topic name for output annotated rgb image, if \"None\" " + "no output image is published", + type=lambda value: value if value.lower() != "none" else None, + default="/opendr/image_pose_annotated") + parser.add_argument("-d", "--detections_topic", help="Topic name for detection messages, if \"None\" " + "no detection message is published", + type=lambda value: value if value.lower() != "none" else None, + default="/opendr/poses") + 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, + action="store_true") + args = parser.parse_args() + + try: + if args.device == "cuda" and torch.cuda.is_available(): + device = "cuda" + elif args.device == "cuda": + print("GPU not found. Using CPU instead.") + device = "cpu" + else: + print("Using CPU.") + device = "cpu" + except: + print("Using CPU.") + device = "cpu" + + if args.accelerate: + stride = True + stages = 0 + half_prec = True + else: + stride = False + 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) + + rclpy.spin(pose_estimator_node) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + pose_estimator_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/image_dataset_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/image_dataset_node.py new file mode 100644 index 0000000000..3587e37aef --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/image_dataset_node.py @@ -0,0 +1,113 @@ +#!/usr/bin/env python +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import argparse +import os +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image as ROS_Image +from opendr_bridge import ROS2Bridge +from opendr.engine.datasets import DatasetIterator +from opendr.perception.object_tracking_2d import MotDataset, RawMotDatasetIterator + + +class ImageDatasetNode(Node): + def __init__( + self, + dataset: DatasetIterator, + output_rgb_image_topic="/opendr/dataset_image", + data_fps=10, + ): + """ + Creates a ROS2 Node for publishing dataset images + """ + + super().__init__('opendr_image_dataset_node') + + self.dataset = dataset + self.bridge = ROS2Bridge() + self.timer = self.create_timer(1.0 / data_fps, self.timer_callback) + self.sample_index = 0 + + self.output_image_publisher = self.create_publisher( + ROS_Image, output_rgb_image_topic, 1 + ) + self.get_logger().info("Publishing images.") + + def timer_callback(self): + image = self.dataset[self.sample_index % len(self.dataset)][0] + # Dataset should have an (Image, Target) pair as elements + + message = self.bridge.to_ros_image( + image, encoding="bgr8" + ) + self.output_image_publisher.publish(message) + + self.sample_index += 1 + + +def main(args=None): + rclpy.init(args=args) + + parser = argparse.ArgumentParser() + parser.add_argument("-d", "--dataset_path", help="Path to a dataset", + type=str, default="MOT") + parser.add_argument( + "-ks", "--mot20_subsets_path", help="Path to mot20 subsets", + type=str, default=os.path.join( + "..", "..", "src", "opendr", "perception", "object_tracking_2d", + "datasets", "splits", "nano_mot20.train" + ) + ) + parser.add_argument("-o", "--output_rgb_image_topic", help="Topic name to publish the data", + type=str, default="/opendr/dataset_image") + parser.add_argument("-f", "--fps", help="Data FPS", + type=float, default=30) + args = parser.parse_args() + + dataset_path = args.dataset_path + mot20_subsets_path = args.mot20_subsets_path + output_rgb_image_topic = args.output_rgb_image_topic + data_fps = args.fps + + if not os.path.exists(dataset_path): + dataset_path = MotDataset.download_nano_mot20( + "MOT", True + ).path + + dataset = RawMotDatasetIterator( + dataset_path, + { + "mot20": mot20_subsets_path + }, + scan_labels=False + ) + dataset_node = ImageDatasetNode( + dataset, + output_rgb_image_topic=output_rgb_image_topic, + data_fps=data_fps, + ) + + rclpy.spin(dataset_node) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + dataset_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() 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 new file mode 100644 index 0000000000..cb43293f19 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/landmark_based_facial_expression_recognition_node.py @@ -0,0 +1,184 @@ +#!/usr/bin/env python +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import argparse +import torch +import numpy as np + +import rclpy +from rclpy.node import Node +from std_msgs.msg import String +from vision_msgs.msg import ObjectHypothesis +from sensor_msgs.msg import Image as ROS_Image +from opendr_bridge import ROS2Bridge + +from opendr.perception.facial_expression_recognition import ProgressiveSpatioTemporalBLNLearner +from opendr.perception.facial_expression_recognition import landmark_extractor +from opendr.perception.facial_expression_recognition import gen_muscle_data +from opendr.perception.facial_expression_recognition import data_normalization + + +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'): + """ + Creates a ROS2 Node for landmark-based facial expression recognition. + :param input_rgb_image_topic: Topic from which we are reading the input image + :type input_rgb_image_topic: str + :param output_category_topic: Topic to which we are publishing the recognized facial expression category info + (if None, we are not publishing the info) + :type output_category_topic: str + :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 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. + (Options: 'pstbln_ck+', 'pstbln_casia', 'pstbln_afew') + :type model: str + :param shape_predictor: pretrained model to use for landmark extraction from a facial image + :type model: str + """ + super().__init__('opendr_landmark_based_facial_expression_recognition_node') + # Set up ROS topics and bridge + + self.image_subscriber = self.create_subscription(ROS_Image, input_rgb_image_topic, self.callback, 1) + + if output_category_topic is not None: + self.hypothesis_publisher = self.create_publisher(ObjectHypothesis, output_category_topic, 1) + else: + self.hypothesis_publisher = None + + if output_category_description_topic is not None: + self.string_publisher = self.create_publisher(String, output_category_description_topic, 1) + else: + self.string_publisher = None + + self.bridge = ROS2Bridge() + + # Initialize the landmark-based facial expression recognition + if model == 'pstbln_ck+': + num_point = 303 + num_class = 7 + elif model == 'pstbln_casia': + num_point = 309 + num_class = 6 + elif model == 'pstbln_afew': + num_point = 312 + num_class = 7 + self.model_name, self.dataset_name = model.split("_") + self.expression_classifier = ProgressiveSpatioTemporalBLNLearner(device=device, dataset_name=self.dataset_name, + 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 + self.expression_classifier.load(model_saved_path, model) + self.shape_predictor = shape_predictor + + self.get_logger().info("landmark-based facial expression recognition node started!") + + def callback(self, data): + """ + Callback that process the input data and publishes to the corresponding topics + :param data: input message + :type data: sensor_msgs.msg.Image + """ + + # Convert sensor_msgs.msg.Image into OpenDR Image + image = self.bridge.from_ros_image(data, encoding='bgr8') + landmarks = landmark_extractor(image, './landmarks.npy', self.shape_predictor) + + # 3: sequence numpy data generation from extracted landmarks and normalization: + + numpy_data = _landmark2numpy(landmarks) + norm_data = data_normalization(numpy_data) + muscle_data = gen_muscle_data(norm_data, './muscle_data') + + # Run expression recognition + category = self.expression_classifier.infer(muscle_data) + + if self.hypothesis_publisher is not None: + self.hypothesis_publisher.publish(self.bridge.to_ros_category(category)) + + if self.string_publisher is not None: + self.string_publisher.publish(self.bridge.to_ros_category_description(category)) + + +def _landmark2numpy(landmarks): + num_landmarks = 68 + num_dim = 2 # feature dimension for each facial landmark + num_faces = 1 # number of faces in each frame + num_frames = 15 + numpy_data = np.zeros((1, num_dim, num_frames, num_landmarks, num_faces)) + for t in range(num_frames): + numpy_data[0, 0:num_dim, t, :, 0] = landmarks + return numpy_data + + +def main(args=None): + rclpy.init(args=args) + + parser = argparse.ArgumentParser() + parser.add_argument("-i", "--input_rgb_image_topic", help="Topic name for input image", + type=str, default="image_raw") + parser.add_argument("-o", "--output_category_topic", help="Topic name for output recognized category", + type=lambda value: value if value.lower() != "none" else None, + default="/opendr/landmark_expression_recognition") + 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("--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'", + type=str, default="pstbln_afew", choices=['pstbln_ck+', 'pstbln_casia', 'pstbln_afew']) + parser.add_argument("-s", "--shape_predictor", help="Shape predictor (landmark_extractor) to use", + type=str, default='./predictor_path') + args = parser.parse_args() + + try: + if args.device == "cuda" and torch.cuda.is_available(): + device = "cuda" + elif args.device == "cuda": + print("GPU not found. Using CPU instead.") + device = "cpu" + else: + print("Using CPU.") + device = "cpu" + except: + print("Using CPU.") + device = "cpu" + + landmark_expression_estimation_node = \ + LandmarkFacialExpressionRecognitionNode( + 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, + device=device, model=args.model, + shape_predictor=args.shape_predictor) + + rclpy.spin(landmark_expression_estimation_node) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + landmark_expression_estimation_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() 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 new file mode 100644 index 0000000000..e0ba51c629 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_centernet_node.py @@ -0,0 +1,143 @@ +#!/usr/bin/env python +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import argparse +import mxnet as mx + +import rclpy +from rclpy.node import Node + +from sensor_msgs.msg import Image as ROS_Image +from vision_msgs.msg import Detection2DArray +from opendr_bridge import ROS2Bridge + +from opendr.engine.data import Image +from opendr.perception.object_detection_2d import CenterNetDetectorLearner +from opendr.perception.object_detection_2d import draw_bounding_boxes + + +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"): + """ + Creates a ROS2 Node for object detection with Centernet. + :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 + image is published) + :type output_rgb_image_topic: str + :param detections_topic: Topic to which we are publishing the annotations (if None, no object detection message + is published) + :type detections_topic: str + :param device: device on which we are running inference ('cpu' or 'cuda') + :type device: str + :param backbone: backbone network + :type backbone: str + """ + super().__init__('opendr_object_detection_2d_centernet_node') + + self.image_subscriber = self.create_subscription(ROS_Image, input_rgb_image_topic, self.callback, 1) + + if output_rgb_image_topic is not None: + self.image_publisher = self.create_publisher(ROS_Image, output_rgb_image_topic, 1) + else: + self.image_publisher = None + + if detections_topic is not None: + self.object_publisher = self.create_publisher(Detection2DArray, detections_topic, 1) + else: + self.object_publisher = None + + self.bridge = ROS2Bridge() + + self.object_detector = CenterNetDetectorLearner(backbone=backbone, device=device) + self.object_detector.download(path=".", verbose=True) + self.object_detector.load("centernet_default") + + self.get_logger().info("Object Detection 2D Centernet node initialized.") + + def callback(self, data): + """ + Callback that process the input data and publishes to the corresponding topics. + :param data: input message + :type data: sensor_msgs.msg.Image + """ + # Convert sensor_msgs.msg.Image into OpenDR Image + image = self.bridge.from_ros_image(data, encoding='bgr8') + + # Run object detection + boxes = self.object_detector.infer(image, threshold=0.45, keep_size=False) + + 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) + + 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 + self.image_publisher.publish(self.bridge.to_ros_image(Image(image), encoding='bgr8')) + + +def main(args=None): + rclpy.init(args=args) + + parser = argparse.ArgumentParser() + parser.add_argument("-i", "--input_rgb_image_topic", help="Topic name for input rgb image", + type=str, default="image_raw") + parser.add_argument("-o", "--output_rgb_image_topic", help="Topic name for output annotated rgb image", + type=lambda value: value if value.lower() != "none" else 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("--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"]) + args = parser.parse_args() + + try: + if args.device == "cuda" and mx.context.num_gpus() > 0: + device = "cuda" + elif args.device == "cuda": + print("GPU not found. Using CPU instead.") + device = "cpu" + else: + print("Using CPU.") + device = "cpu" + except: + print("Using CPU.") + device = "cpu" + + 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) + + rclpy.spin(object_detection_centernet_node) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + object_detection_centernet_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() 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 new file mode 100644 index 0000000000..154dabf79f --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_detr_node.py @@ -0,0 +1,242 @@ +#!/usr/bin/env python +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import argparse +import torch +import numpy as np + +import rclpy +from rclpy.node import Node + +from sensor_msgs.msg import Image as ROS_Image +from vision_msgs.msg import Detection2DArray +from opendr_bridge import ROS2Bridge + +from opendr.engine.data import Image +from opendr.perception.object_detection_2d import DetrLearner +from opendr.perception.object_detection_2d import draw_bounding_boxes + + +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", + ): + """ + Creates a ROS2 Node for object detection with DETR. + :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 + image is published) + :type output_rgb_image_topic: str + :param detections_topic: Topic to which we are publishing the annotations (if None, no object detection message + is published) + :type detections_topic: str + :param device: device on which we are running inference ('cpu' or 'cuda') + :type device: str + """ + super().__init__("opendr_object_detection_2d_detr_node") + + if output_rgb_image_topic is not None: + self.image_publisher = self.create_publisher(ROS_Image, output_rgb_image_topic, 1) + else: + self.image_publisher = None + + if detections_topic is not None: + self.detection_publisher = self.create_publisher(Detection2DArray, detections_topic, 1) + else: + self.detection_publisher = None + + self.image_subscriber = self.create_subscription(ROS_Image, input_rgb_image_topic, self.callback, 1) + + self.bridge = ROS2Bridge() + + self.class_names = [ + "N/A", + "person", + "bicycle", + "car", + "motorcycle", + "airplane", + "bus", + "train", + "truck", + "boat", + "traffic light", + "fire hydrant", + "N/A", + "stop sign", + "parking meter", + "bench", + "bird", + "cat", + "dog", + "horse", + "sheep", + "cow", + "elephant", + "bear", + "zebra", + "giraffe", + "N/A", + "backpack", + "umbrella", + "N/A", + "N/A", + "handbag", + "tie", + "suitcase", + "frisbee", + "skis", + "snowboard", + "sports ball", + "kite", + "baseball bat", + "baseball glove", + "skateboard", + "surfboard", + "tennis racket", + "bottle", + "N/A", + "wine glass", + "cup", + "fork", + "knife", + "spoon", + "bowl", + "banana", + "apple", + "sandwich", + "orange", + "broccoli", + "carrot", + "hot dog", + "pizza", + "donut", + "cake", + "chair", + "couch", + "potted plant", + "bed", + "N/A", + "dining table", + "N/A", + "N/A", + "toilet", + "N/A", + "tv", + "laptop", + "mouse", + "remote", + "keyboard", + "cell phone", + "microwave", + "oven", + "toaster", + "sink", + "refrigerator", + "N/A", + "book", + "clock", + "vase", + "scissors", + "teddy bear", + "hair drier", + "toothbrush", + ] + + # Initialize the detection estimation + self.object_detector = DetrLearner(device=device) + self.object_detector.download(path=".", verbose=True) + + self.get_logger().info("Object Detection 2D DETR node initialized.") + + def callback(self, data): + """ + Callback that process the input data and publishes to the corresponding topics + :param data: input message + :type data: sensor_msgs.msg.Image + """ + # Convert sensor_msgs.msg.Image into OpenDR Image + image = self.bridge.from_ros_image(data, encoding="bgr8") + + # Run detection estimation + boxes = self.object_detector.infer(image) + + # Annotate image and publish results: + if self.detection_publisher is not None: + ros_detection = self.bridge.to_ros_bounding_box_list(boxes) + self.detection_publisher.publish(ros_detection) + # We get can the data back using self.bridge.from_ros_bounding_box_list(ros_detection) + # e.g., opendr_detection = self.bridge.from_ros_bounding_box_list(ros_detection) + + if self.image_publisher is not None: + # Get an OpenCV image back + image = np.float32(image.opencv()) + image = draw_bounding_boxes(image, boxes, class_names=self.class_names) + message = self.bridge.to_ros_image(Image(image), encoding="bgr8") + self.image_publisher.publish(message) + + +def main(args=None): + rclpy.init(args=args) + + parser = argparse.ArgumentParser() + parser.add_argument("-i", "--input_rgb_image_topic", help="Topic name for input rgb image", + type=str, default="/image_raw") + parser.add_argument("-o", "--output_rgb_image_topic", help="Topic name for output annotated rgb image", + type=lambda value: value if value.lower() != "none" else 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("--device", help="Device to use, either \"cpu\" or \"cuda\", defaults to \"cuda\"", + type=str, default="cuda", choices=["cuda", "cpu"]) + args = parser.parse_args() + + try: + if args.device == "cuda" and torch.cuda.is_available(): + device = "cuda" + elif args.device == "cuda": + print("GPU not found. Using CPU instead.") + device = "cpu" + else: + print("Using CPU.") + device = "cpu" + except: + print("Using CPU.") + device = "cpu" + + 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, + ) + + rclpy.spin(object_detection_detr_node) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + object_detection_detr_node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() 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 new file mode 100644 index 0000000000..9f0b3b9760 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_gem_node.py @@ -0,0 +1,282 @@ +#!/usr/bin/env python +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + +import argparse +import cv2 +import 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 +from vision_msgs.msg import Detection2DArray + +from opendr.engine.data import Image +from opendr.perception.object_detection_2d import GemLearner +from opendr.perception.object_detection_2d import draw_bounding_boxes + + +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, + ): + """ + Creates a ROS2 Node for object detection with GEM + :param input_rgb_image_topic: Topic from which we are reading the input rgb image + :type input_rgb_image_topic: str + :param input_infra_image_topic: Topic from which we are reading the input infrared image + :type: input_infra_image_topic: str + :param output_rgb_image_topic: Topic to which we are publishing the annotated rgb image (if None, we are not + publishing annotated image) + :type output_rgb_image_topic: str + :param output_infra_image_topic: Topic to which we are publishing the annotated infrared image (if None, we are not + publishing annotated image) + :type output_infra_image_topic: str + :param detections_topic: Topic to which we are publishing the annotations (if None, we are + not publishing annotations) + :type detections_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 + specific and can be obtained using get_color_infra_alignment.py which is located in the + opendr/perception/object_detection2d/utils module. + :type pts_rgb: {list, numpy.ndarray} + :param pts_infra: Points on the infrared image that define alignment with rgb image. These are camera specific + and can be obtained using get_color_infra_alignment.py which is located in the + opendr/perception/object_detection2d/utils module. + :type pts_infra: {list, numpy.ndarray} + """ + super().__init__("opendr_object_detection_2d_gem_node") + + if output_rgb_image_topic is not None: + self.rgb_publisher = self.create_publisher(msg_type=ROS_Image, topic=output_rgb_image_topic, qos_profile=10) + else: + self.rgb_publisher = None + if output_infra_image_topic is not None: + self.ir_publisher = self.create_publisher(msg_type=ROS_Image, topic=output_infra_image_topic, qos_profile=10) + else: + self.ir_publisher = None + + if detections_topic is not None: + self.detection_publisher = self.create_publisher(msg_type=Detection2DArray, topic=detections_topic, qos_profile=10) + else: + self.detection_publisher = None + if pts_infra is None: + pts_infra = np.array( + [ + [478, 248], + [465, 338], + [458, 325], + [468, 256], + [341, 240], + [335, 310], + [324, 321], + [311, 383], + [434, 365], + [135, 384], + [67, 257], + [167, 206], + [124, 131], + [364, 276], + [424, 269], + [277, 131], + [41, 310], + [202, 320], + [188, 318], + [188, 308], + [196, 241], + [499, 317], + [311, 164], + [220, 216], + [435, 352], + [213, 363], + [390, 364], + [212, 368], + [390, 370], + [467, 324], + [415, 364], + ] + ) + self.get_logger().warn( + "\nUsing default calibration values for pts_infra!" + + "\nThese are probably incorrect." + + "\nThe correct values for pts_infra can be found by running get_rgb_infra_alignment.py." + + "\nThis file is located in the opendr/perception/object_detection2d/utils module." + ) + if pts_rgb is None: + pts_rgb = np.array( + [ + [910, 397], + [889, 572], + [874, 552], + [891, 411], + [635, 385], + [619, 525], + [603, 544], + [576, 682], + [810, 619], + [216, 688], + [90, 423], + [281, 310], + [193, 163], + [684, 449], + [806, 431], + [504, 170], + [24, 538], + [353, 552], + [323, 550], + [323, 529], + [344, 387], + [961, 533], + [570, 233], + [392, 336], + [831, 610], + [378, 638], + [742, 630], + [378, 648], + [742, 640], + [895, 550], + [787, 630], + ] + ) + self.get_logger().warn( + "\nUsing default calibration values for pts_rgb!" + + "\nThese are probably incorrect." + + "\nThe correct values for pts_rgb can be found by running get_color_infra_alignment.py." + + "\nThis file is located in the opendr/perception/object_detection2d/utils module." + ) + # Object classes + self.classes = ["N/A", "chair", "cycle", "bin", "laptop", "drill", "rocker"] + + # Estimating Homography matrix for aligning infra with rgb + self.h, status = cv2.findHomography(pts_infra, pts_rgb) + + self.bridge = ROS2Bridge() + + # Initialize the detection estimation + model_backbone = "resnet50" + + self.gem_learner = GemLearner( + backbone=model_backbone, + num_classes=7, + device=device, + ) + self.gem_learner.fusion_method = "sc_avg" + self.gem_learner.download(path=".", verbose=True) + + # Subscribers + msg_rgb = message_filters.Subscriber(self, ROS_Image, input_rgb_image_topic, 1) + msg_ir = message_filters.Subscriber(self, ROS_Image, input_infra_image_topic, 1) + + sync = message_filters.TimeSynchronizer([msg_rgb, msg_ir], 1) + sync.registerCallback(self.callback) + + def callback(self, msg_rgb, msg_ir): + """ + Callback that process the input data and publishes to the corresponding topics + :param msg_rgb: input rgb image message + :type msg_rgb: sensor_msgs.msg.Image + :param msg_ir: input infrared image message + :type msg_ir: sensor_msgs.msg.Image + """ + # 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() + image_ir = cv2.warpPerspective(image_ir_raw, self.h, (image_rgb.shape[1], image_rgb.shape[0])) + + # Perform inference on images + boxes, w_sensor1, _ = self.gem_learner.infer(image_rgb, image_ir) + + # Annotate image and publish results: + if self.detection_publisher is not None: + ros_detection = self.bridge.to_ros_bounding_box_list(boxes) + self.detection_publisher.publish(ros_detection) + # We can get the data back using self.bridge.from_ros_bounding_box_list(ros_detection) + # e.g., opendr_detection = self.bridge.from_ros_bounding_box_list(ros_detection) + + if self.rgb_publisher is not None: + plot_rgb = draw_bounding_boxes(image_rgb, boxes, class_names=self.classes) + message = self.bridge.to_ros_image(Image(np.uint8(plot_rgb))) + self.rgb_publisher.publish(message) + if self.ir_publisher is not None: + plot_ir = draw_bounding_boxes(image_ir, boxes, class_names=self.classes) + message = self.bridge.to_ros_image(Image(np.uint8(plot_ir))) + self.ir_publisher.publish(message) + + +def main(args=None): + rclpy.init(args=args) + + parser = argparse.ArgumentParser() + parser.add_argument("-ic", "--input_rgb_image_topic", help="Topic name for input rgb image", + type=str, default="/camera/color/image_raw") + parser.add_argument("-ii", "--input_infra_image_topic", help="Topic name for input infrared image", + type=str, default="/camera/infra/image_raw") + parser.add_argument("-oc", "--output_rgb_image_topic", help="Topic name for output annotated rgb image", + type=lambda value: value if value.lower() != "none" else None, + default="/opendr/rgb_image_objects_annotated") + parser.add_argument("-oi", "--output_infra_image_topic", help="Topic name for output annotated infrared image", + type=lambda value: value if value.lower() != "none" else None, + default="/opendr/infra_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("--device", help='Device to use, either "cpu" or "cuda", defaults to "cuda"', + type=str, default="cuda", choices=["cuda", "cpu"]) + args = parser.parse_args() + + try: + if args.device == "cuda" and torch.cuda.is_available(): + device = "cuda" + elif args.device == "cuda": + print("GPU not found. Using CPU instead.") + device = "cpu" + else: + print("Using CPU.") + device = "cpu" + except: + print("Using CPU.") + device = "cpu" + + gem_node = ObjectDetectionGemNode( + device=device, + input_rgb_image_topic=args.input_rgb_image_topic, + output_rgb_image_topic=args.output_rgb_image_topic, + input_infra_image_topic=args.input_infra_image_topic, + output_infra_image_topic=args.output_infra_image_topic, + detections_topic=args.detections_topic, + ) + + rclpy.spin(gem_node) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + gem_node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() 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 new file mode 100755 index 0000000000..31902c032e --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_nanodet_node.py @@ -0,0 +1,144 @@ +#!/usr/bin/env python +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import argparse +import torch + +import rclpy +from rclpy.node import Node + +from sensor_msgs.msg import Image as ROS_Image +from vision_msgs.msg import Detection2DArray +from opendr_bridge import ROS2Bridge + +from opendr.engine.data import Image +from opendr.perception.object_detection_2d import NanodetLearner +from opendr.perception.object_detection_2d import draw_bounding_boxes + + +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"): + """ + Creates a ROS2 Node for object detection with Nanodet. + :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 + image is published) + :type output_rgb_image_topic: str + :param detections_topic: Topic to which we are publishing the annotations (if None, no object detection message + is published) + :type detections_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 + :type model: str + """ + super().__init__('object_detection_2d_nanodet_node') + + self.image_subscriber = self.create_subscription(ROS_Image, input_rgb_image_topic, self.callback, 1) + + if output_rgb_image_topic is not None: + self.image_publisher = self.create_publisher(ROS_Image, output_rgb_image_topic, 1) + else: + self.image_publisher = None + + if detections_topic is not None: + self.object_publisher = self.create_publisher(Detection2DArray, detections_topic, 1) + else: + self.object_publisher = None + + self.bridge = ROS2Bridge() + + # Initialize the object detector + self.object_detector = NanodetLearner(model_to_use=model, device=device) + self.object_detector.download(path=".", mode="pretrained", verbose=True) + self.object_detector.load("./nanodet_{}".format(model)) + + self.get_logger().info("Object Detection 2D Nanodet node initialized.") + + def callback(self, data): + """ + Callback that processes the input data and publishes to the corresponding topics. + :param data: input message + :type data: sensor_msgs.msg.Image + """ + # Convert sensor_msgs.msg.Image into OpenDR Image + image = self.bridge.from_ros_image(data, encoding='bgr8') + + # Run object detection + boxes = self.object_detector.infer(image, threshold=0.35) + + # Get an OpenCV image back + image = image.opencv() + + # 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) + + if self.image_publisher is not None: + # 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 + self.image_publisher.publish(self.bridge.to_ros_image(Image(image), encoding='bgr8')) + + +def main(args=None): + rclpy.init(args=args) + + parser = argparse.ArgumentParser() + parser.add_argument("-i", "--input_rgb_image_topic", help="Topic name for input rgb image", + type=str, default="image_raw") + parser.add_argument("-o", "--output_rgb_image_topic", help="Topic name for output annotated rgb image", + type=lambda value: value if value.lower() != "none" else 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("--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() + + try: + if args.device == "cuda" and torch.cuda.is_available(): + device = "cuda" + elif args.device == "cuda": + print("GPU not found. Using CPU instead.") + device = "cpu" + else: + print("Using CPU.") + device = "cpu" + except: + print("Using CPU.") + device = "cpu" + + 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) + + rclpy.spin(object_detection_nanodet_node) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + object_detection_nanodet_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() 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 new file mode 100644 index 0000000000..acf4bce4a4 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_ssd_node.py @@ -0,0 +1,170 @@ +#!/usr/bin/env python +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import argparse +import mxnet as mx + +import rclpy +from rclpy.node import Node + +from sensor_msgs.msg import Image as ROS_Image +from vision_msgs.msg import Detection2DArray +from opendr_bridge import ROS2Bridge + +from opendr.engine.data import Image +from opendr.perception.object_detection_2d import SingleShotDetectorLearner +from opendr.perception.object_detection_2d import draw_bounding_boxes +from opendr.perception.object_detection_2d import Seq2SeqNMSLearner, SoftNMS, FastNMS, ClusterNMS + + +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'): + """ + Creates a ROS2 Node for object detection with SSD. + :param input_rgb_image_topic: Topic from which we are reading the input image + :type input_rgb_image_topic: str + :param output_rgb_image_topic: Topic to which we are publishing the annotated image (if None, we are not publishing + annotated image) + :type output_rgb_image_topic: str + :param detections_topic: Topic to which we are publishing the annotations (if None, no object detection message + is published) + :type detections_topic: str + :param device: device on which we are running inference ('cpu' or 'cuda') + :type device: str + :param backbone: backbone network + :type backbone: str + :param nms_type: type of NMS method, can be one + of 'default', 'seq2seq-nms', 'soft-nms', 'fast-nms', 'cluster-nms' + :type nms_type: str + """ + super().__init__('opendr_object_detection_2d_ssd_node') + + self.image_subscriber = self.create_subscription(ROS_Image, input_rgb_image_topic, self.callback, 1) + + if output_rgb_image_topic is not None: + self.image_publisher = self.create_publisher(ROS_Image, output_rgb_image_topic, 1) + else: + self.image_publisher = None + + if detections_topic is not None: + self.object_publisher = self.create_publisher(Detection2DArray, detections_topic, 1) + else: + self.object_publisher = None + + self.bridge = ROS2Bridge() + + self.object_detector = SingleShotDetectorLearner(backbone=backbone, device=device) + self.object_detector.download(path=".", verbose=True) + self.object_detector.load("ssd_default_person") + self.custom_nms = None + + # Initialize NMS if selected + if nms_type == 'seq2seq-nms': + self.custom_nms = Seq2SeqNMSLearner(fmod_map_type='EDGEMAP', iou_filtering=0.8, + app_feats='fmod', device=device) + self.custom_nms.download(model_name='seq2seq_pets_jpd_fmod', path='.') + self.custom_nms.load('./seq2seq_pets_jpd_fmod/', verbose=True) + self.get_logger().info("Object Detection 2D SSD node seq2seq-nms initialized.") + elif nms_type == 'soft-nms': + self.custom_nms = SoftNMS(nms_thres=0.45, device=device) + self.get_logger().info("Object Detection 2D SSD node soft-nms initialized.") + elif nms_type == 'fast-nms': + self.custom_nms = FastNMS(device=device) + self.get_logger().info("Object Detection 2D SSD node fast-nms initialized.") + elif nms_type == 'cluster-nms': + self.custom_nms = ClusterNMS(device=device) + self.get_logger().info("Object Detection 2D SSD node cluster-nms initialized.") + else: + self.get_logger().info("Object Detection 2D SSD node using default NMS.") + + self.get_logger().info("Object Detection 2D SSD node initialized.") + + def callback(self, data): + """ + Callback that process the input data and publishes to the corresponding topics. + :param data: input message + :type data: sensor_msgs.msg.Image + """ + # Convert sensor_msgs.msg.Image into OpenDR Image + image = self.bridge.from_ros_image(data, encoding='bgr8') + + # Run object detection + boxes = self.object_detector.infer(image, threshold=0.45, keep_size=False, custom_nms=self.custom_nms) + + 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) + + 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 + self.image_publisher.publish(self.bridge.to_ros_image(Image(image), encoding='bgr8')) + + +def main(args=None): + rclpy.init(args=args) + + parser = argparse.ArgumentParser() + parser.add_argument("-i", "--input_rgb_image_topic", help="Topic name for input rgb image", + type=str, default="image_raw") + parser.add_argument("-o", "--output_rgb_image_topic", help="Topic name for output annotated rgb image", + type=lambda value: value if value.lower() != "none" else 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("--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"]) + parser.add_argument("--nms_type", help="Non-Maximum Suppression type, defaults to \"default\", options are " + "\"seq2seq-nms\", \"soft-nms\", \"fast-nms\", \"cluster-nms\"", + type=str, default="default", + choices=["default", "seq2seq-nms", "soft-nms", "fast-nms", "cluster-nms"]) + args = parser.parse_args() + + try: + if args.device == "cuda" and mx.context.num_gpus() > 0: + device = "cuda" + elif args.device == "cuda": + print("GPU not found. Using CPU instead.") + device = "cpu" + else: + print("Using CPU.") + device = "cpu" + except: + print("Using CPU.") + device = "cpu" + + 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) + + rclpy.spin(object_detection_ssd_node) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + object_detection_ssd_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() 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 new file mode 100644 index 0000000000..43bd7aab03 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_yolov3_node.py @@ -0,0 +1,143 @@ +#!/usr/bin/env python +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import argparse +import mxnet as mx + +import rclpy +from rclpy.node import Node + +from sensor_msgs.msg import Image as ROS_Image +from vision_msgs.msg import Detection2DArray +from opendr_bridge import ROS2Bridge + +from opendr.engine.data import Image +from opendr.perception.object_detection_2d import YOLOv3DetectorLearner +from opendr.perception.object_detection_2d import draw_bounding_boxes + + +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"): + """ + Creates a ROS2 Node for object detection with YOLOV3 + :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 + image is published) + :type output_rgb_image_topic: str + :param detections_topic: Topic to which we are publishing the annotations (if None, no object detection message + is published) + :type detections_topic: str + :param device: device on which we are running inference ('cpu' or 'cuda') + :type device: str + :param backbone: backbone network + :type backbone: str + """ + super().__init__('object_detection_2d_yolov3_node') + + self.image_subscriber = self.create_subscription(ROS_Image, input_rgb_image_topic, self.callback, 1) + + if output_rgb_image_topic is not None: + self.image_publisher = self.create_publisher(ROS_Image, output_rgb_image_topic, 1) + else: + self.image_publisher = None + + if detections_topic is not None: + self.object_publisher = self.create_publisher(Detection2DArray, detections_topic, 1) + else: + self.object_publisher = None + + self.bridge = ROS2Bridge() + + self.object_detector = YOLOv3DetectorLearner(backbone=backbone, device=device) + self.object_detector.download(path=".", verbose=True) + self.object_detector.load("yolo_default") + + self.get_logger().info("Object Detection 2D YOLOV3 node initialized.") + + def callback(self, data): + """ + Callback that processes the input data and publishes to the corresponding topics. + :param data: input message + :type data: sensor_msgs.msg.Image + """ + # Convert sensor_msgs.msg.Image into OpenDR Image + image = self.bridge.from_ros_image(data, encoding='bgr8') + + # Run object detection + boxes = self.object_detector.infer(image, threshold=0.1, keep_size=False) + + 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) + + 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 + self.image_publisher.publish(self.bridge.to_ros_image(Image(image), encoding='bgr8')) + + +def main(args=None): + rclpy.init(args=args) + + parser = argparse.ArgumentParser() + parser.add_argument("-i", "--input_rgb_image_topic", help="Topic name for input rgb image", + type=str, default="image_raw") + parser.add_argument("-o", "--output_rgb_image_topic", help="Topic name for output annotated rgb image", + type=lambda value: value if value.lower() != "none" else 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("--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\"", + type=str, default="darknet53", choices=["darknet53"]) + args = parser.parse_args() + + try: + if args.device == "cuda" and mx.context.num_gpus() > 0: + device = "cuda" + elif args.device == "cuda": + print("GPU not found. Using CPU instead.") + device = "cpu" + else: + print("Using CPU.") + device = "cpu" + except: + print("Using CPU.") + device = "cpu" + + 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) + + rclpy.spin(object_detection_yolov3_node) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + object_detection_yolov3_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() 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 new file mode 100644 index 0000000000..e80d0e34a4 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_2d_yolov5_node.py @@ -0,0 +1,142 @@ +#!/usr/bin/env python +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import argparse +import torch + +import rclpy +from rclpy.node import Node + +from sensor_msgs.msg import Image as ROS_Image +from vision_msgs.msg import Detection2DArray +from opendr_bridge import ROS2Bridge + +from opendr.engine.data import Image +from opendr.perception.object_detection_2d import YOLOv5DetectorLearner +from opendr.perception.object_detection_2d import draw_bounding_boxes + + +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"): + """ + Creates a ROS2 Node for object detection with YOLOV5. + :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 + image is published) + :type output_rgb_image_topic: str + :param detections_topic: Topic to which we are publishing the annotations (if None, no object detection message + is published) + :type detections_topic: str + :param device: device on which we are running inference ('cpu' or 'cuda') + :type device: str + :param model: model to use + :type model: str + """ + super().__init__('object_detection_2d_yolov5_node') + + self.image_subscriber = self.create_subscription(ROS_Image, input_rgb_image_topic, self.callback, 1) + + if output_rgb_image_topic is not None: + self.image_publisher = self.create_publisher(ROS_Image, output_rgb_image_topic, 1) + else: + self.image_publisher = None + + if detections_topic is not None: + self.object_publisher = self.create_publisher(Detection2DArray, detections_topic, 1) + else: + self.object_publisher = None + + self.bridge = ROS2Bridge() + + self.object_detector = YOLOv5DetectorLearner(model_name=model, device=device) + + self.get_logger().info("Object Detection 2D YOLOV5 node initialized.") + + def callback(self, data): + """ + Callback that processes the input data and publishes to the corresponding topics. + :param data: input message + :type data: sensor_msgs.msg.Image + """ + # Convert sensor_msgs.msg.Image into OpenDR Image + image = self.bridge.from_ros_image(data, encoding='bgr8') + + # Run object detection + boxes = self.object_detector.infer(image) + + 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) + + 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, line_thickness=3) + # 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')) + + +def main(args=None): + rclpy.init(args=args) + + parser = argparse.ArgumentParser() + parser.add_argument("-i", "--input_rgb_image_topic", help="Topic name for input rgb image", + type=str, default="image_raw") + parser.add_argument("-o", "--output_rgb_image_topic", help="Topic name for output annotated rgb image", + type=lambda value: value if value.lower() != "none" else 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("--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", + choices=['yolov5s', 'yolov5n', 'yolov5m', 'yolov5l', 'yolov5x', + 'yolov5n6', 'yolov5s6', 'yolov5m6', 'yolov5l6', 'custom']) + args = parser.parse_args() + + try: + if args.device == "cuda" and torch.cuda.is_available(): + device = "cuda" + elif args.device == "cuda": + print("GPU not found. Using CPU instead.") + device = "cpu" + else: + print("Using CPU.") + device = "cpu" + except: + print("Using CPU.") + device = "cpu" + + 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) + + rclpy.spin(object_detection_yolov5_node) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + object_detection_yolov5_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py new file mode 100644 index 0000000000..4c3b883905 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_detection_3d_voxel_node.py @@ -0,0 +1,151 @@ +#!/usr/bin/env python +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import torch +import argparse +import os +import rclpy +from rclpy.node import Node +from vision_msgs.msg import Detection3DArray +from sensor_msgs.msg import PointCloud as ROS_PointCloud +from opendr_bridge import ROS2Bridge +from opendr.perception.object_detection_3d import VoxelObjectDetection3DLearner + + +class ObjectDetection3DVoxelNode(Node): + def __init__( + self, + input_point_cloud_topic="/opendr/dataset_point_cloud", + detections_topic="/opendr/objects3d", + device="cuda:0", + model_name="tanet_car_xyres_16", + model_config_path=os.path.join( + "$OPENDR_HOME", "src", "opendr", "perception", "object_detection_3d", + "voxel_object_detection_3d", "second_detector", "configs", "tanet", + "ped_cycle", "test_short.proto" + ), + temp_dir="temp", + ): + """ + Creates a ROS2 Node for 3D object detection + :param input_point_cloud_topic: Topic from which we are reading the input point cloud + :type input_point_cloud_topic: str + :param detections_topic: Topic to which we are publishing the annotations + :type detections_topic: str + :param device: device on which we are running inference ('cpu' or 'cuda') + :type device: str + :param model_name: the pretrained model to download or a trained model in temp_dir + :type model_name: str + :param temp_dir: where to store models + :type temp_dir: str + """ + + super().__init__('opendr_object_detection_3d_voxel_node') + + self.get_logger().info("Using model_name: {}".format(model_name)) + + self.learner = VoxelObjectDetection3DLearner( + device=device, temp_path=temp_dir, model_config_path=model_config_path + ) + if not os.path.exists(os.path.join(temp_dir, model_name)): + VoxelObjectDetection3DLearner.download(model_name, temp_dir) + + self.learner.load(os.path.join(temp_dir, model_name), verbose=True) + + # Initialize OpenDR ROSBridge object + self.bridge = ROS2Bridge() + + self.detection_publisher = self.create_publisher( + Detection3DArray, detections_topic, 1 + ) + + self.create_subscription(ROS_PointCloud, input_point_cloud_topic, self.callback, 1) + + self.get_logger().info("Object Detection 3D Voxel Node initialized.") + + def callback(self, data): + """ + Callback that processes the input data and publishes to the corresponding topics. + :param data: input message + :type data: sensor_msgs.msg.Image + """ + + # Convert sensor_msgs.msg.Image into OpenDR Image + point_cloud = self.bridge.from_ros_point_cloud(data) + detection_boxes = self.learner.infer(point_cloud) + + # Convert detected boxes to ROS type and publish + ros_boxes = self.bridge.to_ros_boxes_3d(detection_boxes) + self.detection_publisher.publish(ros_boxes) + + +def main(args=None): + rclpy.init(args=args) + + parser = argparse.ArgumentParser() + parser.add_argument("-i", "--input_point_cloud_topic", + help="Point Cloud topic provided by either a point_cloud_dataset_node or any other 3D Point Cloud Node", + type=str, default="/opendr/dataset_point_cloud") + parser.add_argument("-d", "--detections_topic", + help="Output detections topic", + type=str, default="/opendr/objects3d") + parser.add_argument("--device", help="Device to use, either \"cpu\" or \"cuda\", defaults to \"cuda\"", + type=str, default="cuda", choices=["cuda", "cpu"]) + parser.add_argument("-n", "--model_name", help="Name of the trained model", + type=str, default="tanet_car_xyres_16") + parser.add_argument( + "-c", "--model_config_path", help="Path to a model .proto config", + type=str, default=os.path.join( + "$OPENDR_HOME", "src", "opendr", "perception", "object_detection_3d", + "voxel_object_detection_3d", "second_detector", "configs", "tanet", + "car", "xyres_16.proto" + ) + ) + parser.add_argument("-t", "--temp_dir", help="Path to a temp dir with models", + type=str, default="temp") + args = parser.parse_args() + + try: + if args.device == "cuda" and torch.cuda.is_available(): + device = "cuda" + elif args.device == "cuda": + print("GPU not found. Using CPU instead.") + device = "cpu" + else: + print("Using CPU.") + device = "cpu" + except: + print("Using CPU.") + device = "cpu" + + voxel_node = ObjectDetection3DVoxelNode( + device=device, + model_name=args.model_name, + model_config_path=args.model_config_path, + input_point_cloud_topic=args.input_point_cloud_topic, + temp_dir=args.temp_dir, + detections_topic=args.detections_topic, + ) + + rclpy.spin(voxel_node) + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + voxel_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py new file mode 100644 index 0000000000..30b83a8b75 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_deep_sort_node.py @@ -0,0 +1,247 @@ +#!/usr/bin/env python +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import torch +import argparse +import cv2 +import os +from opendr.engine.target import TrackingAnnotationList +import rclpy +from rclpy.node import Node +from vision_msgs.msg import Detection2DArray +from std_msgs.msg import Int32MultiArray +from sensor_msgs.msg import Image as ROS_Image +from opendr_bridge import ROS2Bridge +from opendr.perception.object_tracking_2d import ( + ObjectTracking2DDeepSortLearner, + ObjectTracking2DFairMotLearner +) +from opendr.engine.data import Image, ImageWithDetections + + +class ObjectTracking2DDeepSortNode(Node): + def __init__( + self, + detector=None, + input_rgb_image_topic="image_raw", + output_detection_topic="/opendr/objects", + output_tracking_id_topic="/opendr/objects_tracking_id", + output_rgb_image_topic="/opendr/image_objects_annotated", + device="cuda:0", + model_name="deep_sort", + temp_dir="temp", + ): + """ + Creates a ROS2 Node for 2D object tracking + :param detector: Learner to generate object detections + :type detector: Learner + :param input_rgb_image_topic: Topic from which we are reading the input image + :type input_rgb_image_topic: str + :param output_rgb_image_topic: Topic to which we are publishing the annotated image (if None, we are not publishing + annotated image) + :type output_rgb_image_topic: str + :param output_detection_topic: Topic to which we are publishing the detections + :type output_detection_topic: str + :param output_tracking_id_topic: Topic to which we are publishing the tracking ids + :type output_tracking_id_topic: str + :param device: device on which we are running inference ('cpu' or 'cuda') + :type device: str + :param model_name: the pretrained model to download or a saved model in temp_dir folder to use + :type model_name: str + :param temp_dir: the folder to download models + :type temp_dir: str + """ + + super().__init__('opendr_object_tracking_2d_deep_sort_node') + + self.get_logger().info("Using model_name: {}".format(model_name)) + + self.detector = detector + self.learner = ObjectTracking2DDeepSortLearner( + device=device, temp_path=temp_dir, + ) + if not os.path.exists(os.path.join(temp_dir, model_name)): + ObjectTracking2DDeepSortLearner.download(model_name, temp_dir) + + self.learner.load(os.path.join(temp_dir, model_name), verbose=True) + + self.bridge = ROS2Bridge() + + if output_tracking_id_topic is not None: + self.tracking_id_publisher = self.create_publisher( + Int32MultiArray, output_tracking_id_topic, 1 + ) + + if output_rgb_image_topic is not None: + self.output_image_publisher = self.create_publisher( + ROS_Image, output_rgb_image_topic, 1 + ) + + if output_detection_topic is not None: + self.detection_publisher = self.create_publisher( + Detection2DArray, output_detection_topic, 1 + ) + + self.create_subscription(ROS_Image, input_rgb_image_topic, self.callback, 1) + + def callback(self, data): + """ + Callback that processes the input data and publishes to the corresponding topics. + :param data: input message + :type data: sensor_msgs.msg.Image + """ + + # Convert sensor_msgs.msg.Image into OpenDR Image + image = self.bridge.from_ros_image(data, encoding="bgr8") + detection_boxes = self.detector.infer(image) + image_with_detections = ImageWithDetections(image.numpy(), detection_boxes) + tracking_boxes = self.learner.infer(image_with_detections, swap_left_top=False) + + if self.output_image_publisher is not None: + frame = image.opencv() + draw_predictions(frame, tracking_boxes) + message = self.bridge.to_ros_image( + Image(frame), encoding="bgr8" + ) + self.output_image_publisher.publish(message) + self.get_logger().info("Published annotated image") + + if self.detection_publisher is not None: + detection_boxes = tracking_boxes.bounding_box_list() + ros_boxes = self.bridge.to_ros_boxes(detection_boxes) + self.detection_publisher.publish(ros_boxes) + self.get_logger().info("Published " + str(len(detection_boxes)) + " detection boxes") + + if self.tracking_id_publisher is not None: + ids = [int(tracking_box.id) for tracking_box in tracking_boxes] + ros_ids = Int32MultiArray() + ros_ids.data = ids + self.tracking_id_publisher.publish(ros_ids) + self.get_logger().info("Published " + str(len(ids)) + " tracking ids") + + +colors = [ + (255, 0, 255), + (0, 0, 255), + (0, 255, 0), + (255, 0, 0), + (35, 69, 55), + (43, 63, 54), +] + + +def draw_predictions(frame, predictions: TrackingAnnotationList, is_centered=False, is_flipped_xy=True): + global colors + w, h, _ = frame.shape + + for prediction in predictions.boxes: + prediction = prediction + + if not hasattr(prediction, "id"): + prediction.id = 0 + + color = colors[int(prediction.id) * 7 % len(colors)] + + x = prediction.left + y = prediction.top + + if is_flipped_xy: + x = prediction.top + y = prediction.left + + if is_centered: + x -= prediction.width + y -= prediction.height + + cv2.rectangle( + frame, + (int(x), int(y)), + ( + int(x + prediction.width), + int(y + prediction.height), + ), + color, + 2, + ) + + +def main(args=None): + rclpy.init(args=args) + + parser = argparse.ArgumentParser() + parser.add_argument("-i", "--input_rgb_image_topic", + help="Input Image topic provided by either an image_dataset_node, webcam or any other image node", + type=str, default="/image_raw") + parser.add_argument("-o", "--output_rgb_image_topic", + help="Output annotated image topic with a visualization of detections and their ids", + type=lambda value: value if value.lower() != "none" else None, + default="/opendr/image_objects_annotated") + parser.add_argument("-d", "--detections_topic", + help="Output detections topic", + type=lambda value: value if value.lower() != "none" else None, + default="/opendr/objects") + parser.add_argument("-t", "--tracking_id_topic", + help="Output tracking ids topic with the same element count as in output_detection_topic", + type=lambda value: value if value.lower() != "none" else None, + default="/opendr/objects_tracking_id") + parser.add_argument("--device", help="Device to use, either \"cpu\" or \"cuda\", defaults to \"cuda\"", + type=str, default="cuda", choices=["cuda", "cpu"]) + parser.add_argument("-n", "--model_name", help="Name of the trained model", + type=str, default="deep_sort", choices=["deep_sort"]) + parser.add_argument("-td", "--temp_dir", help="Path to a temporary directory with models", + type=str, default="temp") + args = parser.parse_args() + + try: + if args.device == "cuda" and torch.cuda.is_available(): + device = "cuda" + elif args.device == "cuda": + print("GPU not found. Using CPU instead.") + device = "cpu" + else: + print("Using CPU.") + device = "cpu" + except: + print("Using CPU.") + device = "cpu" + + detection_learner = ObjectTracking2DFairMotLearner( + device=device, temp_path=args.temp_dir, + ) + if not os.path.exists(os.path.join(args.temp_dir, "fairmot_dla34")): + ObjectTracking2DFairMotLearner.download("fairmot_dla34", args.temp_dir) + + detection_learner.load(os.path.join(args.temp_dir, "fairmot_dla34"), verbose=True) + + deep_sort_node = ObjectTracking2DDeepSortNode( + detector=detection_learner, + device=device, + model_name=args.model_name, + input_rgb_image_topic=args.input_rgb_image_topic, + temp_dir=args.temp_dir, + output_detection_topic=args.detections_topic, + output_tracking_id_topic=args.tracking_id_topic, + output_rgb_image_topic=args.output_rgb_image_topic, + ) + rclpy.spin(deep_sort_node) + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + deep_sort_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py new file mode 100755 index 0000000000..bcd30f68ac --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_fair_mot_node.py @@ -0,0 +1,231 @@ +#!/usr/bin/env python +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import torch +import argparse +import cv2 +import os +from opendr.engine.target import TrackingAnnotationList +import rclpy +from rclpy.node import Node +from vision_msgs.msg import Detection2DArray +from std_msgs.msg import Int32MultiArray +from sensor_msgs.msg import Image as ROS_Image +from opendr_bridge import ROS2Bridge +from opendr.perception.object_tracking_2d import ( + ObjectTracking2DFairMotLearner, +) +from opendr.engine.data import Image + + +class ObjectTracking2DFairMotNode(Node): + def __init__( + self, + input_rgb_image_topic="image_raw", + output_rgb_image_topic="/opendr/image_objects_annotated", + output_detection_topic="/opendr/objects", + output_tracking_id_topic="/opendr/objects_tracking_id", + device="cuda:0", + model_name="fairmot_dla34", + temp_dir="temp", + ): + """ + Creates a ROS2 Node for 2D object tracking + :param input_rgb_image_topic: Topic from which we are reading the input image + :type input_rgb_image_topic: str + :param output_rgb_image_topic: Topic to which we are publishing the annotated image (if None, we are not publishing + annotated image) + :type output_rgb_image_topic: str + :param output_detection_topic: Topic to which we are publishing the detections + :type output_detection_topic: str + :param output_tracking_id_topic: Topic to which we are publishing the tracking ids + :type output_tracking_id_topic: str + :param device: device on which we are running inference ('cpu' or 'cuda') + :type device: str + :param model_name: the pretrained model to download or a saved model in temp_dir folder to use + :type model_name: str + :param temp_dir: the folder to download models + :type temp_dir: str + """ + + super().__init__('opendr_object_tracking_2d_fair_mot_node') + + self.learner = ObjectTracking2DFairMotLearner( + device=device, temp_path=temp_dir, + ) + if not os.path.exists(os.path.join(temp_dir, model_name)): + ObjectTracking2DFairMotLearner.download(model_name, temp_dir) + + self.learner.load(os.path.join(temp_dir, model_name), verbose=True) + + # Initialize OpenDR ROSBridge object + self.bridge = ROS2Bridge() + + if output_detection_topic is not None: + self.detection_publisher = self.create_publisher( + Detection2DArray, output_detection_topic, 1 + ) + + if output_tracking_id_topic is not None: + self.tracking_id_publisher = self.create_publisher( + Int32MultiArray, output_tracking_id_topic, 1 + ) + + if output_rgb_image_topic is not None: + self.output_image_publisher = self.create_publisher( + ROS_Image, output_rgb_image_topic, 1 + ) + + self.create_subscription(ROS_Image, input_rgb_image_topic, self.callback, 1) + + def callback(self, data): + """ + Callback that processes the input data and publishes to the corresponding topics. + :param data: input message + :type data: sensor_msgs.msg.Image + """ + + # Convert sensor_msgs.msg.Image into OpenDR Image + image = self.bridge.from_ros_image(data, encoding="bgr8") + tracking_boxes = self.learner.infer(image) + + if self.output_image_publisher is not None: + frame = image.opencv() + draw_predictions(frame, tracking_boxes) + message = self.bridge.to_ros_image( + Image(frame), encoding="bgr8" + ) + self.output_image_publisher.publish(message) + self.get_logger().info("Published annotated image") + + if self.detection_publisher is not None: + detection_boxes = tracking_boxes.bounding_box_list() + ros_boxes = self.bridge.to_ros_boxes(detection_boxes) + self.detection_publisher.publish(ros_boxes) + self.get_logger().info("Published " + str(len(detection_boxes)) + " detection boxes") + + if self.tracking_id_publisher is not None: + ids = [tracking_box.id for tracking_box in tracking_boxes] + ros_ids = Int32MultiArray() + ros_ids.data = ids + self.tracking_id_publisher.publish(ros_ids) + self.get_logger().info("Published " + str(len(ids)) + " tracking ids") + + +colors = [ + (255, 0, 255), + (0, 0, 255), + (0, 255, 0), + (255, 0, 0), + (35, 69, 55), + (43, 63, 54), +] + + +def draw_predictions(frame, predictions: TrackingAnnotationList, is_centered=False, is_flipped_xy=True): + global colors + w, h, _ = frame.shape + + for prediction in predictions.boxes: + prediction = prediction + + if not hasattr(prediction, "id"): + prediction.id = 0 + + color = colors[int(prediction.id) * 7 % len(colors)] + + x = prediction.left + y = prediction.top + + if is_flipped_xy: + x = prediction.top + y = prediction.left + + if is_centered: + x -= prediction.width + y -= prediction.height + + cv2.rectangle( + frame, + (int(x), int(y)), + ( + int(x + prediction.width), + int(y + prediction.height), + ), + color, + 2, + ) + + +def main(args=None): + rclpy.init(args=args) + + parser = argparse.ArgumentParser() + parser.add_argument("-i", "--input_rgb_image_topic", + help="Input Image topic provided by either an image_dataset_node, webcam or any other image node", + type=str, default="/image_raw") + parser.add_argument("-o", "--output_rgb_image_topic", + help="Output annotated image topic with a visualization of detections and their ids", + type=lambda value: value if value.lower() != "none" else None, + default="/opendr/image_objects_annotated") + parser.add_argument("-d", "--detections_topic", + help="Output detections topic", + type=lambda value: value if value.lower() != "none" else None, + default="/opendr/objects") + parser.add_argument("-t", "--tracking_id_topic", + help="Output tracking ids topic with the same element count as in output_detection_topic", + type=lambda value: value if value.lower() != "none" else None, + default="/opendr/objects_tracking_id") + parser.add_argument("--device", help="Device to use, either \"cpu\" or \"cuda\", defaults to \"cuda\"", + type=str, default="cuda", choices=["cuda", "cpu"]) + parser.add_argument("-n", "--model_name", help="Name of the trained model", + type=str, default="fairmot_dla34", choices=["fairmot_dla34"]) + parser.add_argument("-td", "--temp_dir", help="Path to a temporary directory with models", + type=str, default="temp") + args = parser.parse_args() + + try: + if args.device == "cuda" and torch.cuda.is_available(): + device = "cuda" + elif args.device == "cuda": + print("GPU not found. Using CPU instead.") + device = "cpu" + else: + print("Using CPU.") + device = "cpu" + except: + print("Using CPU.") + device = "cpu" + + fair_mot_node = ObjectTracking2DFairMotNode( + device=device, + model_name=args.model_name, + input_rgb_image_topic=args.input_rgb_image_topic, + temp_dir=args.temp_dir, + output_detection_topic=args.detections_topic, + output_tracking_id_topic=args.tracking_id_topic, + output_rgb_image_topic=args.output_rgb_image_topic, + ) + + rclpy.spin(fair_mot_node) + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + fair_mot_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_siamrpn_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_siamrpn_node.py new file mode 100644 index 0000000000..f2d49919ad --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_2d_siamrpn_node.py @@ -0,0 +1,179 @@ +#!/usr/bin/env python +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import argparse +import mxnet as mx + +import cv2 +from math import dist +import rclpy +from rclpy.node import Node + +from sensor_msgs.msg import Image as ROS_Image +from vision_msgs.msg import Detection2D +from opendr_bridge import ROS2Bridge + +from opendr.engine.data import Image +from opendr.engine.target import TrackingAnnotation, BoundingBox +from opendr.perception.object_tracking_2d import SiamRPNLearner +from opendr.perception.object_detection_2d import YOLOv3DetectorLearner + + +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", + device="cuda"): + """ + Creates a ROS2 Node for object tracking with SiamRPN. + :param object_detector: An object detector learner to use for initialization + :type object_detector: opendr.engine.learners.Learner + :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 + image is published) + :type output_rgb_image_topic: str + :param tracker_topic: Topic to which we are publishing the annotation + :type tracker_topic: str + :param device: device on which we are running inference ('cpu' or 'cuda') + :type device: str + """ + super().__init__('opendr_object_tracking_2d_siamrpn_node') + + self.image_subscriber = self.create_subscription(ROS_Image, input_rgb_image_topic, self.callback, 1) + + if output_rgb_image_topic is not None: + self.image_publisher = self.create_publisher(ROS_Image, output_rgb_image_topic, 1) + else: + self.image_publisher = None + + if tracker_topic is not None: + self.object_publisher = self.create_publisher(Detection2D, tracker_topic, 1) + else: + self.object_publisher = None + + self.bridge = ROS2Bridge() + + self.object_detector = object_detector + # Initialize object tracker + self.tracker = SiamRPNLearner(device=device) + self.image = None + self.initialized = False + + self.get_logger().info("Object Tracking 2D SiamRPN node started.") + + def callback(self, data): + """ + Callback that processes the input data and publishes to the corresponding topics. + :param data: input message + :type data: sensor_msgs.msg.Image + """ + # Convert sensor_msgs.msg.Image into OpenDR Image + image = self.bridge.from_ros_image(data, encoding='bgr8') + self.image = image + + if not self.initialized: + # Run object detector to initialize the tracker + image = self.bridge.from_ros_image(data, encoding='bgr8') + boxes = self.object_detector.infer(image) + + img_center = [int(image.data.shape[2] // 2), int(image.data.shape[1] // 2)] # width, height + # Find the box that is closest to the center of the image + center_box = BoundingBox("", left=0, top=0, width=0, height=0) + min_distance = dist([center_box.left, center_box.top], img_center) + for box in boxes: + new_distance = dist([int(box.left + box.width // 2), int(box.top + box.height // 2)], img_center) + if new_distance < min_distance and box.width > 32 and box.height > 32: # Ignore very small boxes + center_box = box + min_distance = dist([center_box.left, center_box.top], img_center) + + # Initialize tracker with the most central box found + init_box = TrackingAnnotation(center_box.name, + center_box.left, center_box.top, center_box.width, center_box.height, + id=0, score=center_box.confidence) + + self.tracker.infer(self.image, init_box) + self.initialized = True + self.get_logger().info("Object Tracking 2D SiamRPN node initialized with the most central bounding box.") + + if self.initialized: + # Run object tracking + box = self.tracker.infer(image) + + if self.object_publisher is not None: + # Publish detections in ROS message + ros_boxes = self.bridge.to_ros_single_tracking_annotation(box) + self.object_publisher.publish(ros_boxes) + + if self.image_publisher is not None: + # Get an OpenCV image back + image = image.opencv() + cv2.rectangle(image, (box.left, box.top), + (box.left + box.width, box.top + box.height), + (0, 255, 255), 3) + # 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')) + + +def main(args=None): + rclpy.init(args=args) + + parser = argparse.ArgumentParser() + parser.add_argument("-i", "--input_rgb_image_topic", help="Topic name for input rgb image", + type=str, default="/image_raw") + parser.add_argument("-o", "--output_rgb_image_topic", help="Topic name for output annotated rgb image", + type=lambda value: value if value.lower() != "none" else None, + default="/opendr/image_tracking_annotated") + 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("--device", help="Device to use, either \"cpu\" or \"cuda\", defaults to \"cuda\"", + type=str, default="cuda", choices=["cuda", "cpu"]) + args = parser.parse_args() + + try: + if args.device == "cuda" and mx.context.num_gpus() > 0: + device = "cuda" + elif args.device == "cuda": + print("GPU not found. Using CPU instead.") + device = "cpu" + else: + print("Using CPU.") + device = "cpu" + except: + print("Using CPU.") + device = "cpu" + + object_detector = YOLOv3DetectorLearner(backbone="darknet53", device=device) + object_detector.download(path=".", verbose=True) + object_detector.load("yolo_default") + + 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) + + rclpy.spin(object_tracker_2d_siamrpn_node) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + object_tracker_2d_siamrpn_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py new file mode 100644 index 0000000000..c0cfb95124 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/object_tracking_3d_ab3dmot_node.py @@ -0,0 +1,177 @@ +#!/usr/bin/env python +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import torch +import argparse +import os +import rclpy +from rclpy.node import Node +from vision_msgs.msg import Detection3DArray +from std_msgs.msg import Int32MultiArray +from sensor_msgs.msg import PointCloud as ROS_PointCloud +from opendr_bridge import ROS2Bridge +from opendr.perception.object_tracking_3d import ObjectTracking3DAb3dmotLearner +from opendr.perception.object_detection_3d import VoxelObjectDetection3DLearner + + +class ObjectTracking3DAb3dmotNode(Node): + def __init__( + self, + detector=None, + input_point_cloud_topic="/opendr/dataset_point_cloud", + output_detection3d_topic="/opendr/detection3d", + output_tracking3d_id_topic="/opendr/tracking3d_id", + device="cuda:0", + ): + """ + Creates a ROS2 Node for 3D object tracking + :param detector: Learner that provides 3D object detections + :type detector: Learner + :param input_point_cloud_topic: Topic from which we are reading the input point cloud + :type input_point_cloud_topic: str + :param output_detection3d_topic: Topic to which we are publishing the annotations + :type output_detection3d_topic: str + :param output_tracking3d_id_topic: Topic to which we are publishing the tracking ids + :type output_tracking3d_id_topic: str + :param device: device on which we are running inference ('cpu' or 'cuda') + :type device: str + """ + super().__init__('opendr_object_tracking_3d_ab3dmot_node') + + self.detector = detector + self.learner = ObjectTracking3DAb3dmotLearner( + device=device + ) + + # Initialize OpenDR ROSBridge object + self.bridge = ROS2Bridge() + + if output_detection3d_topic is not None: + self.detection_publisher = self.create_publisher( + Detection3DArray, output_detection3d_topic, 1 + ) + + if output_tracking3d_id_topic is not None: + self.tracking_id_publisher = self.create_publisher( + Int32MultiArray, output_tracking3d_id_topic, 1 + ) + + self.create_subscription(ROS_PointCloud, input_point_cloud_topic, self.callback, 1) + + self.get_logger().info("Object Tracking 3D Ab3dmot Node initialized.") + + def callback(self, data): + """ + Callback that processes the input data and publishes to the corresponding topics. + :param data: input message + :type data: sensor_msgs.msg.Image + """ + + # Convert sensor_msgs.msg.Image into OpenDR Image + point_cloud = self.bridge.from_ros_point_cloud(data) + detection_boxes = self.detector.infer(point_cloud) + + # 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) + 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 + self.tracking_id_publisher.publish(ros_ids) + self.get_logger().info("Published " + str(len(ids)) + " tracking ids") + + +def main(args=None): + rclpy.init(args=args) + + parser = argparse.ArgumentParser() + parser.add_argument("-i", "--input_point_cloud_topic", + help="Point Cloud topic provided by either a point_cloud_dataset_node or any other 3D Point Cloud Node", + type=str, default="/opendr/dataset_point_cloud") + parser.add_argument("-d", "--detections_topic", + help="Output detections topic", + type=lambda value: value if value.lower() != "none" else None, default="/opendr/objects3d") + parser.add_argument("-t", "--tracking3d_id_topic", + help="Output tracking ids topic with the same element count as in output_detection_topic", + type=lambda value: value if value.lower() != "none" else None, default="/opendr/objects_tracking_id") + parser.add_argument("--device", help="Device to use, either \"cpu\" or \"cuda\", defaults to \"cuda\"", + type=str, default="cuda", choices=["cuda", "cpu"]) + parser.add_argument("-dn", "--detector_model_name", help="Name of the trained model", + type=str, default="tanet_car_xyres_16", choices=["tanet_car_xyres_16"]) + parser.add_argument( + "-dc", "--detector_model_config_path", help="Path to a model .proto config", + type=str, default=os.path.join( + "$OPENDR_HOME", "src", "opendr", "perception", "object_detection_3d", + "voxel_object_detection_3d", "second_detector", "configs", "tanet", + "car", "xyres_16.proto" + ) + ) + parser.add_argument("-td", "--temp_dir", help="Path to a temporary directory with models", + type=str, default="temp") + args = parser.parse_args() + + input_point_cloud_topic = args.input_point_cloud_topic + detector_model_name = args.detector_model_name + temp_dir = args.temp_dir + detector_model_config_path = args.detector_model_config_path + output_detection3d_topic = args.detections_topic + output_tracking3d_id_topic = args.tracking3d_id_topic + + try: + if args.device == "cuda" and torch.cuda.is_available(): + device = "cuda" + elif args.device == "cuda": + print("GPU not found. Using CPU instead.") + device = "cpu" + else: + print("Using CPU.") + device = "cpu" + except: + print("Using CPU.") + device = "cpu" + + detector = VoxelObjectDetection3DLearner( + device=device, + temp_path=temp_dir, + model_config_path=detector_model_config_path + ) + if not os.path.exists(os.path.join(temp_dir, detector_model_name)): + VoxelObjectDetection3DLearner.download(detector_model_name, temp_dir) + + detector.load(os.path.join(temp_dir, detector_model_name), verbose=True) + + ab3dmot_node = ObjectTracking3DAb3dmotNode( + detector=detector, + device=device, + input_point_cloud_topic=input_point_cloud_topic, + output_detection3d_topic=output_detection3d_topic, + output_tracking3d_id_topic=output_tracking3d_id_topic, + ) + + rclpy.spin(ab3dmot_node) + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + ab3dmot_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/panoptic_segmentation_efficient_ps_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/panoptic_segmentation_efficient_ps_node.py new file mode 100644 index 0000000000..e9459f6480 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/panoptic_segmentation_efficient_ps_node.py @@ -0,0 +1,198 @@ +#!/usr/bin/env python +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import sys +from pathlib import Path +import argparse +from typing import Optional + +import rclpy +from rclpy.node import Node +import matplotlib +from sensor_msgs.msg import Image as ROS_Image + +from opendr_bridge import ROS2Bridge +from opendr.perception.panoptic_segmentation import EfficientPsLearner + +# Avoid having a matplotlib GUI in a separate thread in the visualize() function +matplotlib.use('Agg') + + +class EfficientPsNode(Node): + def __init__(self, + input_rgb_image_topic: str, + checkpoint: str, + output_heatmap_topic: Optional[str] = None, + output_rgb_visualization_topic: Optional[str] = None, + detailed_visualization: bool = False + ): + """ + Initialize the EfficientPS ROS2 node and create an instance of the respective learner class. + :param checkpoint: This is either a path to a saved model or one of [Cityscapes, KITTI] to download + pre-trained model weights. + :type checkpoint: str + :param input_rgb_image_topic: ROS topic for the input image stream + :type input_rgb_image_topic: str + :param output_heatmap_topic: ROS topic for the predicted semantic and instance maps + :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 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 + """ + super().__init__('opendr_efficient_panoptic_segmentation_node') + + self.input_rgb_image_topic = input_rgb_image_topic + self.checkpoint = checkpoint + self.output_heatmap_topic = output_heatmap_topic + self.output_rgb_visualization_topic = output_rgb_visualization_topic + self.detailed_visualization = detailed_visualization + + # Initialize all ROS2 related things + self._bridge = ROS2Bridge() + self._instance_heatmap_publisher = None + self._semantic_heatmap_publisher = None + self._visualization_publisher = None + + # Initialize the panoptic segmentation network + config_file = Path(sys.modules[ + EfficientPsLearner.__module__].__file__).parent / 'configs' / 'singlegpu_cityscapes.py' + self._learner = EfficientPsLearner(str(config_file)) + + # Other + self._tmp_folder = Path(__file__).parent.parent / 'tmp' / 'efficientps' + self._tmp_folder.mkdir(exist_ok=True, parents=True) + + def _init_learner(self) -> bool: + """ + The model can be initialized via + 1. downloading pre-trained weights for Cityscapes or KITTI. + 2. passing a path to an existing checkpoint file. + + This has not been done in the __init__() function since logging is available only once the node is registered. + """ + if self.checkpoint in ['cityscapes', 'kitti']: + file_path = EfficientPsLearner.download(str(self._tmp_folder), + trained_on=self.checkpoint) + self.checkpoint = file_path + + if self._learner.load(self.checkpoint): + self.get_logger().info('Successfully loaded the checkpoint.') + return True + else: + self.get_logger().error('Failed to load the checkpoint.') + return False + + def _init_subscriber(self): + """ + Subscribe to all relevant topics. + """ + self.image_subscriber = self.create_subscription(ROS_Image, self.input_rgb_image_topic, + self.callback, 1) + + def _init_publisher(self): + """ + Set up the publishers as requested by the user. + """ + if self.output_heatmap_topic is not None: + self._instance_heatmap_publisher = self.create_publisher(ROS_Image, + f'{self.output_heatmap_topic}/instance', + 10) + self._semantic_heatmap_publisher = self.create_publisher(ROS_Image, + f'{self.output_heatmap_topic}/semantic', + 10) + if self.output_rgb_visualization_topic is not None: + self._visualization_publisher = self.create_publisher(ROS_Image, + self.output_rgb_visualization_topic, + 10) + + def listen(self): + """ + Start the node and begin processing input data. The order of the function calls ensures that the node does not + try to process input images without being in a trained state. + """ + if self._init_learner(): + self._init_publisher() + self._init_subscriber() + self.get_logger().info('EfficientPS node started!') + rclpy.spin(self) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + self.destroy_node() + rclpy.shutdown() + + def callback(self, data: ROS_Image): + """ + Predict the panoptic segmentation map from the input image and publish the results. + :param data: Input image message + :type data: sensor_msgs.msg.Image + """ + # Convert sensor_msgs.msg.Image to OpenDR Image + image = self._bridge.from_ros_image(data) + + try: + # Retrieve a list of two OpenDR heatmaps: [instance map, semantic map] + prediction = self._learner.infer(image) + + # 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, + detailed=self.detailed_visualization) + self._visualization_publisher.publish(self._bridge.to_ros_image(panoptic_image, encoding="rgb8")) + + if self._instance_heatmap_publisher is not None and self._instance_heatmap_publisher.get_subscription_count() > 0: + self._instance_heatmap_publisher.publish(self._bridge.to_ros_image(prediction[0])) + if self._semantic_heatmap_publisher is not None and self._semantic_heatmap_publisher.get_subscription_count() > 0: + self._semantic_heatmap_publisher.publish(self._bridge.to_ros_image(prediction[1])) + + except Exception as e: + self.get_logger().error(f'Failed to generate prediction: {e}') + + +def main(args=None): + rclpy.init(args=args) + parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter) + parser.add_argument('-i', '--input_rgb_image_topic', type=str, default='/image_raw', + help='listen to RGB images on this topic') + parser.add_argument('-oh', '--output_heatmap_topic', + type=lambda value: value if value.lower() != "none" else None, + default='/opendr/panoptic', + help='publish the semantic and instance maps on this topic as "OUTPUT_HEATMAP_TOPIC/semantic" \ + and "OUTPUT_HEATMAP_TOPIC/instance"') + parser.add_argument('-ov', '--output_rgb_image_topic', + type=lambda value: value if value.lower() != "none" else 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('--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') + parser.add_argument('--checkpoint', type=str, default='cityscapes', + help='download pretrained models [cityscapes, kitti] or load from the provided path') + args = parser.parse_args() + + efficient_ps_node = EfficientPsNode(args.input_rgb_image_topic, + args.checkpoint, + args.output_heatmap_topic, + args.output_rgb_image_topic, + args.detailed_visualization) + efficient_ps_node.listen() + + +if __name__ == '__main__': + main() diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py new file mode 100644 index 0000000000..5ea7f129ff --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/point_cloud_dataset_node.py @@ -0,0 +1,110 @@ +#!/usr/bin/env python +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import argparse +import os +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import PointCloud as ROS_PointCloud +from opendr_bridge import ROS2Bridge +from opendr.engine.datasets import DatasetIterator +from opendr.perception.object_detection_3d import KittiDataset, LabeledPointCloudsDatasetIterator + + +class PointCloudDatasetNode(Node): + def __init__( + self, + dataset: DatasetIterator, + output_point_cloud_topic="/opendr/dataset_point_cloud", + data_fps=10, + ): + """ + Creates a ROS Node for publishing dataset point clouds + """ + + super().__init__('opendr_point_cloud_dataset_node') + + self.dataset = dataset + self.bridge = ROS2Bridge() + self.timer = self.create_timer(1.0 / data_fps, self.timer_callback) + self.sample_index = 0 + + self.output_point_cloud_publisher = self.create_publisher( + ROS_PointCloud, output_point_cloud_topic, 1 + ) + self.get_logger().info("Publishing point_cloud images.") + + def timer_callback(self): + + point_cloud = self.dataset[self.sample_index % len(self.dataset)][0] + # Dataset should have a (PointCloud, Target) pair as elements + + message = self.bridge.to_ros_point_cloud( + point_cloud, self.get_clock().now().to_msg() + ) + self.output_point_cloud_publisher.publish(message) + + self.sample_index += 1 + + +def main(args=None): + rclpy.init(args=args) + + parser = argparse.ArgumentParser() + parser.add_argument("-d", "--dataset_path", + help="Path to a dataset. If does not exist, nano KITTI dataset will be downloaded there.", + type=str, default="KITTI/opendr_nano_kitti") + parser.add_argument("-ks", "--kitti_subsets_path", + help="Path to kitti subsets. Used only if a KITTI dataset is downloaded", + type=str, + default="../../src/opendr/perception/object_detection_3d/datasets/nano_kitti_subsets") + parser.add_argument("-o", "--output_point_cloud_topic", help="Topic name to publish the data", + type=str, default="/opendr/dataset_point_cloud") + parser.add_argument("-f", "--fps", help="Data FPS", + type=float, default=10) + args = parser.parse_args() + + dataset_path = args.dataset_path + kitti_subsets_path = args.kitti_subsets_path + output_point_cloud_topic = args.output_point_cloud_topic + data_fps = args.fps + + if not os.path.exists(dataset_path): + dataset_path = KittiDataset.download_nano_kitti( + "KITTI", kitti_subsets_path=kitti_subsets_path, + create_dir=True, + ).path + + dataset = LabeledPointCloudsDatasetIterator( + dataset_path + "/training/velodyne_reduced", + dataset_path + "/training/label_2", + dataset_path + "/training/calib", + ) + + dataset_node = PointCloudDatasetNode( + dataset, output_point_cloud_topic=output_point_cloud_topic, data_fps=data_fps + ) + + rclpy.spin(dataset_node) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + dataset_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/projects/opendr_ws_2/src/opendr_perception/opendr_perception/pose_estimation_node.py b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/pose_estimation_node.py new file mode 100644 index 0000000000..9193517314 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/pose_estimation_node.py @@ -0,0 +1,169 @@ +#!/usr/bin/env python +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import argparse +import torch + +import rclpy +from rclpy.node import Node + +from sensor_msgs.msg import Image as ROS_Image +from opendr_bridge import ROS2Bridge +from opendr_interface.msg import OpenDRPose2D + +from opendr.engine.data import Image +from opendr.perception.pose_estimation import draw +from opendr.perception.pose_estimation import LightweightOpenPoseLearner + + +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", + num_refinement_stages=2, use_stride=False, half_precision=False): + """ + Creates a ROS2 Node for pose estimation with Lightweight OpenPose. + :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 + image is published) + :type output_rgb_image_topic: str + :param detections_topic: Topic to which we are publishing the annotations (if None, no pose detection message + is published) + :type detections_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 + model's head, including the initial stage. Can be 0, 1 or 2, with more stages meaning slower and more accurate + inference + :type num_refinement_stages: int + :param use_stride: Whether to add a stride value in the model, which reduces accuracy but increases + inference speed + :type use_stride: bool + :param half_precision: Enables inference using half (fp16) precision instead of single (fp32) precision. + Valid only for GPU-based inference + :type half_precision: bool + """ + super().__init__('opendr_pose_estimation_node') + + self.image_subscriber = self.create_subscription(ROS_Image, input_rgb_image_topic, self.callback, 1) + + if output_rgb_image_topic is not None: + self.image_publisher = self.create_publisher(ROS_Image, output_rgb_image_topic, 1) + else: + self.image_publisher = None + + if detections_topic is not None: + self.pose_publisher = self.create_publisher(OpenDRPose2D, detections_topic, 1) + else: + self.pose_publisher = None + + self.bridge = ROS2Bridge() + + self.pose_estimator = LightweightOpenPoseLearner(device=device, num_refinement_stages=num_refinement_stages, + mobilenet_use_stride=use_stride, + half_precision=half_precision) + self.pose_estimator.download(path=".", verbose=True) + self.pose_estimator.load("openpose_default") + + self.get_logger().info("Pose estimation node initialized.") + + def callback(self, data): + """ + Callback that process the input data and publishes to the corresponding topics. + :param data: Input image message + :type data: sensor_msgs.msg.Image + """ + # 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) + + # Publish detections in ROS message + for pose in poses: + if self.pose_publisher is not None: + # Convert OpenDR pose to ROS2 pose message using bridge and publish it + self.pose_publisher.publish(self.bridge.to_ros_pose(pose)) + + if self.image_publisher is not None: + # Get an OpenCV image back + image = image.opencv() + # Annotate image with poses + for pose in poses: + draw(image, pose) + # 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')) + + +def main(args=None): + rclpy.init(args=args) + + parser = argparse.ArgumentParser() + parser.add_argument("-i", "--input_rgb_image_topic", help="Topic name for input rgb image", + type=str, default="image_raw") + parser.add_argument("-o", "--output_rgb_image_topic", help="Topic name for output annotated rgb image, if \"None\" " + "no output image is published", + type=lambda value: value if value.lower() != "none" else None, + default="/opendr/image_pose_annotated") + parser.add_argument("-d", "--detections_topic", help="Topic name for detection messages, if \"None\" " + "no detection message is published", + type=lambda value: value if value.lower() != "none" else None, + default="/opendr/poses") + 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, + action="store_true") + args = parser.parse_args() + + try: + if args.device == "cuda" and torch.cuda.is_available(): + device = "cuda" + elif args.device == "cuda": + print("GPU not found. Using CPU instead.") + device = "cpu" + else: + print("Using CPU.") + device = "cpu" + except: + print("Using CPU.") + device = "cpu" + + if args.accelerate: + stride = True + stages = 0 + half_prec = True + else: + stride = False + 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) + + rclpy.spin(pose_estimator_node) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + pose_estimator_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() 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 new file mode 100755 index 0000000000..8b73944192 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/rgbd_hand_gesture_recognition_node.py @@ -0,0 +1,166 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import argparse +import os +import cv2 +import numpy as np +import torch + +import rclpy +from rclpy.node import Node +import message_filters +from sensor_msgs.msg import Image as ROS_Image +from vision_msgs.msg import Classification2D + +from opendr_bridge import ROS2Bridge +from opendr.engine.data import Image +from opendr.perception.multimodal_human_centric import RgbdHandGestureLearner + + +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): + """ + 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. + :param input_rgb_image_topic: Topic from which we are reading the input image + :type input_rgb_image_topic: str + :param input_depth_image_topic: Topic from which we are reading the input depth image + :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 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 + :type delay: float + """ + super().__init__("opendr_rgbd_hand_gesture_recognition_node") + + self.gesture_publisher = self.create_publisher(Classification2D, output_gestures_topic, 1) + + 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 + ts = message_filters.ApproximateTimeSynchronizer([image_sub, depth_sub], queue_size=10, slop=delay) + ts.registerCallback(self.callback) + + self.bridge = ROS2Bridge() + + # Initialize the gesture recognition + self.gesture_learner = RgbdHandGestureLearner(n_class=16, architecture="mobilenet_v2", device=device) + model_path = './mobilenet_v2' + if not os.path.exists(model_path): + self.gesture_learner.download(path=model_path) + self.gesture_learner.load(path=model_path) + + # mean and std for preprocessing, based on HANDS dataset + self.mean = np.asarray([0.485, 0.456, 0.406, 0.0303]).reshape(1, 1, 4) + self.std = np.asarray([0.229, 0.224, 0.225, 0.0353]).reshape(1, 1, 4) + + self.get_logger().info("RGBD gesture recognition node started!") + + def callback(self, rgb_data, depth_data): + """ + Callback that process the input data and publishes to the corresponding topics + :param rgb_data: input image message + :type rgb_data: sensor_msgs.msg.Image + :param depth_data: input depth image message + :type depth_data: sensor_msgs.msg.Image + """ + + # 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' + depth_image = self.bridge.from_ros_image_to_depth(depth_data, encoding='mono16') + img = self.preprocess(rgb_image, depth_image) + + # Run gesture recognition + gesture_class = self.gesture_learner.infer(img) + + # Publish results + ros_gesture = self.bridge.from_category_to_rosclass(gesture_class, self.get_clock().now().to_msg()) + self.gesture_publisher.publish(ros_gesture) + + def preprocess(self, rgb_image, depth_image): + """ + Preprocess rgb_image, depth_image and concatenate them + :param rgb_image: input RGB image + :type rgb_image: engine.data.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) + + # resize the images to 224x224 + rgb_image = cv2.resize(rgb_image, (224, 224)) + depth_image = cv2.resize(depth_image, (224, 224)) + + # concatenate and standardize + img = np.concatenate([rgb_image, np.expand_dims(depth_image, axis=-1)], axis=-1) + img = (img - self.mean) / self.std + img = Image(img, dtype=np.float32) + return img + + +def main(args=None): + rclpy.init(args=args) + + # Default topics are according to kinectv2 drivers at https://github.com/OpenKinect/libfreenect2 + # and https://github.com/code-iai-iai_kinect2 + parser = argparse.ArgumentParser() + parser.add_argument("-ic", "--input_rgb_image_topic", help="Topic name for input rgb image", + type=str, default="/kinect2/qhd/image_color_rect") + parser.add_argument("-id", "--input_depth_image_topic", help="Topic name for input 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("--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) + + args = parser.parse_args() + + try: + if args.device == "cuda" and torch.cuda.is_available(): + device = "cuda" + elif args.device == "cuda": + print("GPU not found. Using CPU instead.") + device = "cpu" + else: + print("Using CPU") + device = "cpu" + except: + print("Using CPU") + device = "cpu" + + 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) + + rclpy.spin(gesture_node) + + gesture_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() 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 new file mode 100644 index 0000000000..91f860bbd1 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/semantic_segmentation_bisenet_node.py @@ -0,0 +1,197 @@ +#!/usr/bin/env python +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import argparse +import numpy as np +import torch +import cv2 +import colorsys + +import rclpy +from rclpy.node import Node + +from sensor_msgs.msg import Image as ROS_Image +from opendr_bridge import ROS2Bridge + +from opendr.engine.data import Image +from opendr.engine.target import Heatmap +from opendr.perception.semantic_segmentation import BisenetLearner + + +class BisenetNode(Node): + + 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"): + """ + Creates a ROS2 Node for semantic segmentation with Bisenet. + :param input_rgb_image_topic: Topic from which we are reading the input image + :type input_rgb_image_topic: str + :param output_heatmap_topic: Topic to which we are publishing the heatmap in the form of a ROS image containing + class ids + :type output_heatmap_topic: str + :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 device: device on which we are running inference ('cpu' or 'cuda') + :type device: str + """ + super().__init__('opendr_semantic_segmentation_bisenet_node') + + self.image_subscriber = self.create_subscription(ROS_Image, input_rgb_image_topic, self.callback, 1) + + if output_heatmap_topic is not None: + self.heatmap_publisher = self.create_publisher(ROS_Image, output_heatmap_topic, 1) + else: + self.heatmap_publisher = None + + if output_rgb_image_topic is not None: + self.visualization_publisher = self.create_publisher(ROS_Image, output_rgb_image_topic, 1) + else: + self.visualization_publisher = None + + self.bridge = ROS2Bridge() + + # Initialize the semantic segmentation model + self.learner = BisenetLearner(device=device) + self.learner.download(path="bisenet_camvid") + self.learner.load("bisenet_camvid") + + self.class_names = ["Bicyclist", "Building", "Car", "Column Pole", "Fence", "Pedestrian", "Road", "Sidewalk", + "Sign Symbol", "Sky", "Tree", "Unknown"] + self.colors = self.getDistinctColors(len(self.class_names)) # Generate n distinct colors + + self.get_logger().info("Semantic segmentation bisenet node initialized.") + + def callback(self, data): + """ + Callback that process the input data and publishes to the corresponding topics. + :param data: Input image message + :type data: sensor_msgs.msg.Image + """ + # Convert sensor_msgs.msg.Image into OpenDR Image + image = self.bridge.from_ros_image(data, encoding='bgr8') + + try: + # Run semantic segmentation to retrieve the OpenDR heatmap + heatmap = self.learner.infer(image) + + # 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 + self.heatmap_publisher.publish(self.bridge.to_ros_image(heatmap)) + + # Publish heatmap color visualization blended with the input image and a class color legend + if self.visualization_publisher is not None: + heatmap_colors = Image(self.colors[heatmap.numpy()]) + image = Image(cv2.resize(image.convert("channels_last", "bgr"), (960, 720))) + alpha = 0.4 # 1.0 means full input image, 0.0 means full heatmap + beta = (1.0 - alpha) + image_blended = cv2.addWeighted(image.opencv(), alpha, heatmap_colors.opencv(), beta, 0.0) + # Add a legend + image_blended = self.addLegend(image_blended, np.unique(heatmap.data)) + + self.visualization_publisher.publish(self.bridge.to_ros_image(Image(image_blended), + encoding='bgr8')) + except Exception: + self.get_logger().warn('Failed to generate prediction.') + + def addLegend(self, image, unique_class_ints): + # Text setup + origin_x, origin_y = 5, 5 # Text origin x, y + color_rectangle_size = 25 + font_size = 1.0 + font_thickness = 2 + w_max = 0 + for i in range(len(unique_class_ints)): + text = self.class_names[unique_class_ints[i]] # Class name + x, y = origin_x, origin_y + i * color_rectangle_size # Text position + # Determine class color and convert to regular integers + color = (int(self.colors[unique_class_ints[i]][0]), + int(self.colors[unique_class_ints[i]][1]), + int(self.colors[unique_class_ints[i]][2])) + # Get text width and height + (w, h), _ = cv2.getTextSize(text, cv2.FONT_HERSHEY_SIMPLEX, font_size, font_thickness) + if w >= w_max: + w_max = w + # Draw partial background rectangle + image = cv2.rectangle(image, (x - origin_x, y), + (x + origin_x + color_rectangle_size + w_max, + y + color_rectangle_size), + (255, 255, 255, 0.5), -1) + # Draw color rectangle + image = cv2.rectangle(image, (x, y), + (x + color_rectangle_size, y + color_rectangle_size), color, -1) + # Draw class name text + image = cv2.putText(image, text, (x + color_rectangle_size + 2, y + h), + cv2.FONT_HERSHEY_SIMPLEX, font_size, (0, 0, 0), font_thickness) + return image + + @staticmethod + def HSVToRGB(h, s, v): + (r, g, b) = colorsys.hsv_to_rgb(h, s, v) + return np.array([int(255 * r), int(255 * g), int(255 * b)]) + + def getDistinctColors(self, n): + huePartition = 1.0 / (n + 1) + return np.array([self.HSVToRGB(huePartition * value, 1.0, 1.0) for value in range(0, n)]).astype(np.uint8) + + +def main(args=None): + rclpy.init(args=args) + parser = argparse.ArgumentParser() + parser.add_argument("-i", "--input_rgb_image_topic", help="Topic name for input rgb image", + type=str, default="image_raw") + parser.add_argument("-o", "--output_heatmap_topic", help="Topic to which we are publishing the heatmap in the form " + "of a ROS image containing class ids", + type=lambda value: value if value.lower() != "none" else None, + default="/opendr/heatmap") + parser.add_argument("-ov", "--output_rgb_image_topic", help="Topic to which we are publishing the heatmap image " + "blended with the input image and a class legend for " + "visualization purposes", + type=lambda value: value if value.lower() != "none" else None, + default="/opendr/heatmap_visualization") + parser.add_argument("--device", help="Device to use, either \"cpu\" or \"cuda\", defaults to \"cuda\"", + type=str, default="cuda", choices=["cuda", "cpu"]) + args = parser.parse_args() + + try: + if args.device == "cuda" and torch.cuda.is_available(): + device = "cuda" + elif args.device == "cuda": + print("GPU not found. Using CPU instead.") + device = "cpu" + else: + print("Using CPU.") + device = "cpu" + except: + print("Using CPU.") + device = "cpu" + + 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) + + rclpy.spin(bisenet_node) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + bisenet_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() 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 new file mode 100644 index 0000000000..dce55a5630 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/skeleton_based_action_recognition_node.py @@ -0,0 +1,249 @@ +#!/usr/bin/env python +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import argparse +import torch +import numpy as np + +import rclpy +from rclpy.node import Node +from std_msgs.msg import String +from vision_msgs.msg import ObjectHypothesis +from sensor_msgs.msg import Image as ROS_Image +from opendr_bridge import ROS2Bridge +from opendr_interface.msg import OpenDRPose2D + +from opendr.engine.data import Image +from opendr.perception.pose_estimation import draw +from opendr.perception.pose_estimation import LightweightOpenPoseLearner +from opendr.perception.skeleton_based_action_recognition import SpatioTemporalGCNLearner +from opendr.perception.skeleton_based_action_recognition import ProgressiveSpatioTemporalGCNLearner + + +class SkeletonActionRecognitionNode(Node): + + def __init__(self, input_rgb_image_topic="image_raw", + output_rgb_image_topic="/opendr/image_pose_annotated", + pose_annotations_topic="/opendr/poses", + output_category_topic="/opendr/skeleton_recognized_action", + output_category_description_topic="/opendr/skeleton_recognized_action_description", + device="cuda", model='stgcn'): + """ + Creates a ROS2 Node for skeleton-based action recognition + :param input_rgb_image_topic: Topic from which we are reading the input image + :type input_rgb_image_topic: str + :param output_rgb_image_topic: Topic to which we are publishing the annotated image (if None, we are not publishing + annotated image) + :type output_rgb_image_topic: str + :param pose_annotations_topic: Topic to which we are publishing the annotations (if None, we are not publishing + annotated pose annotations) + :type pose_annotations_topic: str + :param output_category_topic: Topic to which we are publishing the recognized action category info + (if None, we are not publishing the info) + :type output_category_topic: str + :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 device: device on which we are running inference ('cpu' or 'cuda') + :type device: str + :param model: model to use for skeleton-based action recognition. + (Options: 'stgcn', 'pstgcn') + :type model: str + """ + super().__init__('opendr_skeleton_based_action_recognition_node') + # Set up ROS topics and bridge + + self.image_subscriber = self.create_subscription(ROS_Image, input_rgb_image_topic, self.callback, 1) + + if output_rgb_image_topic is not None: + self.image_publisher = self.create_publisher(ROS_Image, output_rgb_image_topic, 1) + else: + self.image_publisher = None + + if pose_annotations_topic is not None: + self.pose_publisher = self.create_publisher(OpenDRPose2D, pose_annotations_topic, 1) + else: + self.pose_publisher = None + + if output_category_topic is not None: + self.hypothesis_publisher = self.create_publisher(ObjectHypothesis, output_category_topic, 1) + else: + self.hypothesis_publisher = None + + if output_category_description_topic is not None: + self.string_publisher = self.create_publisher(String, output_category_description_topic, 1) + else: + self.string_publisher = None + + self.bridge = ROS2Bridge() + + # Initialize the pose estimation + self.pose_estimator = LightweightOpenPoseLearner(device=device, num_refinement_stages=2, + mobilenet_use_stride=False, + half_precision=False + ) + self.pose_estimator.download(path=".", verbose=True) + self.pose_estimator.load("openpose_default") + + # Initialize the skeleton_based action recognition + if model == 'stgcn': + self.action_classifier = SpatioTemporalGCNLearner(device=device, dataset_name='nturgbd_cv', + method_name=model, in_channels=2, num_point=18, + graph_type='openpose') + elif model == 'pstgcn': + self.action_classifier = ProgressiveSpatioTemporalGCNLearner(device=device, dataset_name='nturgbd_cv', + topology=[5, 4, 5, 2, 3, 4, 3, 4], + in_channels=2, num_point=18, + graph_type='openpose') + + 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') + + self.get_logger().info("Skeleton-based action recognition node started!") + + def callback(self, data): + """ + Callback that process the input data and publishes to the corresponding topics + :param data: input message + :type data: sensor_msgs.msg.Image + """ + + # Convert sensor_msgs.msg.Image into OpenDR Image + image = self.bridge.from_ros_image(data, encoding='bgr8') + + # Run pose estimation + poses = self.pose_estimator.infer(image) + if len(poses) > 2: + # 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): + poses_list.append(poses) + skeleton_seq = _pose2numpy(num_frames, poses_list) + + # Run action recognition + category = self.action_classifier.infer(skeleton_seq) + category.confidence = float(category.confidence.max()) + + if self.hypothesis_publisher is not None: + self.hypothesis_publisher.publish(self.bridge.to_ros_category(category)) + + if self.string_publisher is not None: + self.string_publisher.publish(self.bridge.to_ros_category_description(category)) + + +def _select_2_poses(poses): + selected_poses = [] + energy = [] + for i in range(len(poses)): + s = poses[i].data[:, 0].std() + poses[i].data[:, 1].std() + energy.append(s) + energy = np.array(energy) + index = energy.argsort()[::-1][0:2] + for i in range(len(index)): + selected_poses.append(poses[index[i]]) + return selected_poses + + +def _pose2numpy(num_current_frames, poses_list): + C = 2 + T = 300 + V = 18 + M = 2 # num_person_in + skeleton_seq = np.zeros((1, C, T, V, M)) + for t in range(num_current_frames): + for m in range(len(poses_list[t])): + skeleton_seq[0, 0:2, t, :, m] = np.transpose(poses_list[t][m].data) + return skeleton_seq + + +def main(args=None): + rclpy.init(args=args) + + parser = argparse.ArgumentParser() + parser.add_argument("-i", "--input_rgb_image_topic", help="Topic name for input image", + type=str, default="image_raw") + parser.add_argument("-o", "--output_rgb_image_topic", help="Topic name for output annotated image", + type=lambda value: value if value.lower() != "none" else None, + default="/opendr/image_pose_annotated") + parser.add_argument("-p", "--pose_annotations_topic", help="Topic name for pose annotations", + type=lambda value: value if value.lower() != "none" else None, + default="/opendr/poses") + parser.add_argument("-c", "--output_category_topic", help="Topic name for recognized action category", + type=lambda value: value if value.lower() != "none" else None, + default="/opendr/skeleton_recognized_action") + parser.add_argument("-d", "--output_category_description_topic", 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("--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\"", + type=str, default="stgcn", choices=["stgcn", "pstgcn"]) + + args = parser.parse_args() + + try: + if args.device == "cuda" and torch.cuda.is_available(): + device = "cuda" + elif args.device == "cuda": + print("GPU not found. Using CPU instead.") + device = "cpu" + else: + print("Using CPU.") + device = "cpu" + except: + print("Using CPU.") + device = "cpu" + + skeleton_action_recognition_node = \ + SkeletonActionRecognitionNode(input_rgb_image_topic=args.input_rgb_image_topic, + output_rgb_image_topic=args.output_rgb_image_topic, + pose_annotations_topic=args.pose_annotations_topic, + output_category_topic=args.output_category_topic, + output_category_description_topic=args.output_category_description_topic, + device=device, + model=args.model) + + rclpy.spin(skeleton_action_recognition_node) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + skeleton_action_recognition_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() 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 new file mode 100755 index 0000000000..d15f26433a --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/speech_command_recognition_node.py @@ -0,0 +1,147 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import argparse +import torch +import numpy as np + +import rclpy +from rclpy.node import Node +from audio_common_msgs.msg import AudioData +from vision_msgs.msg import Classification2D + +from opendr_bridge import ROS2Bridge +from opendr.engine.data import Timeseries +from opendr.perception.speech_recognition import MatchboxNetLearner, EdgeSpeechNetsLearner, QuadraticSelfOnnLearner + + +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"): + """ + 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 buffer_size: Length of the audio buffer in seconds + :type buffer_size: float + :param model: base speech command recognition model: matchboxnet or quad_selfonn + :type model: str + :param device: device for inference ("cpu" or "cuda") + :type device: str + + """ + super().__init__("opendr_speech_command_recognition_node") + + self.publisher = self.create_publisher(Classification2D, output_speech_command_topic, 1) + + self.create_subscription(AudioData, input_audio_topic, self.callback, 1) + + self.bridge = ROS2Bridge() + + # Initialize the internal audio buffer + self.buffer_size = buffer_size + self.data_buffer = np.zeros((1, 1)) + + # Initialize the recognition model + if model == "matchboxnet": + self.learner = MatchboxNetLearner(output_classes_n=20, device=device) + load_path = "./MatchboxNet" + elif model == "edgespeechnets": + self.learner = EdgeSpeechNetsLearner(output_classes_n=20, device=device) + assert model_path is not None, "No pretrained EdgeSpeechNets model available for download" + elif model == "quad_selfonn": + self.learner = QuadraticSelfOnnLearner(output_classes_n=20, device=device) + load_path = "./QuadraticSelfOnn" + + # Download the recognition model + if model_path is None: + self.learner.download_pretrained(path=".") + self.learner.load(load_path) + else: + self.learner.load(model_path) + + self.get_logger().info("Speech command recognition node started!") + + def callback(self, msg_data): + """ + Callback that processes the input data and publishes predictions to the output topic + :param msg_data: incoming message + :type msg_data: audio_common_msgs.msg.AudioData + """ + # Accumulate data until the buffer is full + 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: + + # Convert sample to OpenDR Timeseries and perform classification + input_sample = Timeseries(self.data_buffer) + class_pred = self.learner.infer(input_sample) + + # Publish output + ros_class = self.bridge.from_category_to_rosclass(class_pred, self.get_clock().now().to_msg()) + self.publisher.publish(ros_class) + + # Reset the audio buffer + self.data_buffer = np.zeros((1, 1)) + + +def main(args=None): + rclpy.init(args=args) + + parser = argparse.ArgumentParser() + parser.add_argument("-i", "--input_audio_topic", type=str, default="/audio", + 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("--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") + parser.add_argument("--model", default="matchboxnet", choices=["matchboxnet", "edgespeechnets", "quad_selfonn"], + help="Model to be used for prediction: matchboxnet, edgespeechnets or quad_selfonn") + parser.add_argument("--model_path", type=str, + help="Path to the model files, if not given, the pretrained model will be downloaded") + args = parser.parse_args() + + try: + if args.device == "cuda" and torch.cuda.is_available(): + device = "cuda" + elif args.device == "cuda": + print("GPU not found. Using CPU instead.") + device = "cpu" + else: + print("Using CPU") + device = "cpu" + except: + print("Using CPU") + device = "cpu" + + speech_node = SpeechRecognitionNode(input_audio_topic=args.input_audio_topic, + output_speech_command_topic=args.output_speech_command_topic, + buffer_size=args.buffer_size, model=args.model, model_path=args.model_path, + device=device) + + rclpy.spin(speech_node) + + speech_node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() 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 new file mode 100644 index 0000000000..9e137036b8 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/opendr_perception/video_activity_recognition_node.py @@ -0,0 +1,250 @@ +#!/usr/bin/env python +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import argparse +import torch +import torchvision +import cv2 +import rclpy +from rclpy.node import Node +from pathlib import Path + +from std_msgs.msg import String +from vision_msgs.msg import ObjectHypothesis +from sensor_msgs.msg import Image as ROS_Image +from opendr_bridge import ROS2Bridge + +from opendr.engine.data import Video, Image +from opendr.perception.activity_recognition import CLASSES as KINETICS400_CLASSES +from opendr.perception.activity_recognition import CoX3DLearner +from opendr.perception.activity_recognition import X3DLearner + + +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", + ): + """ + Creates a ROS2 Node for video-based human activity recognition. + :param input_rgb_image_topic: Topic from which we are reading the input image + :type input_rgb_image_topic: str + :param output_category_topic: Topic to which we are publishing the recognized activity + (if None, we are not publishing the info) + :type output_category_topic: str + :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 device: device on which we are running inference ('cpu' or 'cuda') + :type device: str + :param model: Architecture to use for human activity recognition. + (Options: 'cox3d-s', 'cox3d-m', 'cox3d-l', 'x3d-xs', 'x3d-s', 'x3d-m', 'x3d-l') + :type model: str + """ + super().__init__("opendr_video_human_activity_recognition_node") + assert model in { + "cox3d-s", + "cox3d-m", + "cox3d-l", + "x3d-xs", + "x3d-s", + "x3d-m", + "x3d-l", + } + model_name, model_size = model.split("-") + Learner = {"cox3d": CoX3DLearner, "x3d": X3DLearner}[model_name] + + # Initialize the human activity recognition + self.learner = Learner(device=device, backbone=model_size) + self.learner.download(path="model_weights", model_names={model_size}) + self.learner.load(Path("model_weights") / f"x3d_{model_size}.pyth") + + # Set up preprocessing + if model_name == "cox3d": + self.preprocess = _image_preprocess( + image_size=self.learner.model_hparams["image_size"] + ) + else: # == x3d + self.preprocess = _video_preprocess( + image_size=self.learner.model_hparams["image_size"], + window_size=self.learner.model_hparams["frames_per_clip"], + ) + + # Set up ROS topics and bridge + self.image_subscriber = self.create_subscription( + ROS_Image, input_rgb_image_topic, self.callback, 1 + ) + self.hypothesis_publisher = ( + self.create_publisher(ObjectHypothesis, output_category_topic, 1) + if output_category_topic + else None + ) + self.string_publisher = ( + self.create_publisher(String, output_category_description_topic, 1) + if output_category_description_topic + else None + ) + self.bridge = ROS2Bridge() + self.get_logger().info("Video Human Activity Recognition node initialized.") + + 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 + """ + image = self.bridge.from_ros_image(data, encoding="rgb8") + if image is None: + return + + x = self.preprocess(image.convert("channels_first", "rgb")) + + result = self.learner.infer(x) + assert len(result) == 1 + category = result[0] + # Confidence for predicted class + category.confidence = float(category.confidence.max()) + category.description = KINETICS400_CLASSES[category.data] # Class name + + if self.hypothesis_publisher is not None: + self.hypothesis_publisher.publish(self.bridge.to_ros_category(category)) + + if self.string_publisher is not None: + self.string_publisher.publish( + self.bridge.to_ros_category_description(category) + ) + + +def _resize(image, size=None, inter=cv2.INTER_AREA): + # initialize the dimensions of the image to be resized and + # grab the image size + dim = None + (h, w) = image.shape[:2] + + if h > w: + # calculate the ratio of the width and construct the + # dimensions + r = size / float(w) + dim = (size, int(h * r)) + else: + # calculate the ratio of the height and construct the + # dimensions + r = size / float(h) + dim = (int(w * r), size) + + # resize the image + resized = cv2.resize(image, dim, interpolation=inter) + + # return the resized image + return resized + + +def _image_preprocess(image_size: int): + standardize = torchvision.transforms.Normalize( + mean=(0.45, 0.45, 0.45), std=(0.225, 0.225, 0.225) + ) + + def wrapped(frame): + nonlocal standardize + frame = frame.transpose((1, 2, 0)) # C, H, W -> H, W, C + frame = _resize(frame, size=image_size) + frame = torch.tensor(frame).permute((2, 0, 1)) # H, W, C -> C, H, W + frame = frame / 255.0 # [0, 255] -> [0.0, 1.0] + frame = standardize(frame) + return Image(frame, dtype=float) + + return wrapped + + +def _video_preprocess(image_size: int, window_size: int): + frames = [] + + standardize = torchvision.transforms.Normalize( + mean=(0.45, 0.45, 0.45), std=(0.225, 0.225, 0.225) + ) + + def wrapped(frame): + nonlocal frames, standardize + frame = frame.transpose((1, 2, 0)) # C, H, W -> H, W, C + frame = _resize(frame, size=image_size) + frame = torch.tensor(frame).permute((2, 0, 1)) # H, W, C -> C, H, W + frame = frame / 255.0 # [0, 255] -> [0.0, 1.0] + frame = standardize(frame) + if not frames: + frames = [frame for _ in range(window_size)] + else: + frames.pop(0) + frames.append(frame) + vid = Video(torch.stack(frames, dim=1)) + return vid + + return wrapped + + +def main(args=None): + rclpy.init(args=args) + + parser = argparse.ArgumentParser() + parser.add_argument("-i", "--input_rgb_image_topic", help="Topic name for input rgb image", + type=str, default="/image_raw") + parser.add_argument("-o", "--output_category_topic", help="Topic to which we are publishing the recognized activity", + type=lambda value: value if value.lower() != "none" else None, + default="/opendr/human_activity_recognition") + parser.add_argument("-od", "--output_category_description_topic", + 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"', + type=str, default="cuda", choices=["cuda", "cpu"]) + parser.add_argument("--model", help="Architecture to use for human activity recognition.", + type=str, default="cox3d-m", + choices=["cox3d-s", "cox3d-m", "cox3d-l", "x3d-xs", "x3d-s", "x3d-m", "x3d-l"]) + args = parser.parse_args() + + try: + if args.device == "cuda" and torch.cuda.is_available(): + device = "cuda" + elif args.device == "cuda": + print("GPU not found. Using CPU instead.") + device = "cpu" + else: + print("Using CPU.") + device = "cpu" + except Exception: + print("Using CPU.") + device = "cpu" + + human_activity_recognition_node = HumanActivityRecognitionNode( + 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, + device=device, + model=args.model, + ) + rclpy.spin(human_activity_recognition_node) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + human_activity_recognition_node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/projects/opendr_ws_2/src/opendr_perception/package.xml b/projects/opendr_ws_2/src/opendr_perception/package.xml new file mode 100644 index 0000000000..21199f4985 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/package.xml @@ -0,0 +1,26 @@ + + + + opendr_perception + 1.0.0 + OpenDR ROS2 nodes for the perception package + OpenDR Project Coordinator + Apache License v2.0 + + rclpy + + std_msgs + vision_msgs + geometry_msgs + + opendr_bridge + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/projects/opendr_ws_2/src/opendr_perception/resource/opendr_perception b/projects/opendr_ws_2/src/opendr_perception/resource/opendr_perception new file mode 100644 index 0000000000..e69de29bb2 diff --git a/projects/opendr_ws_2/src/opendr_perception/setup.cfg b/projects/opendr_ws_2/src/opendr_perception/setup.cfg new file mode 100644 index 0000000000..45e65634f1 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/setup.cfg @@ -0,0 +1,6 @@ +[develop] +script_dir=$base/lib/opendr_perception +[install] +install_scripts=$base/lib/opendr_perception +[build_scripts] +executable = /usr/bin/env python3 diff --git a/projects/opendr_ws_2/src/opendr_perception/setup.py b/projects/opendr_ws_2/src/opendr_perception/setup.py new file mode 100644 index 0000000000..3e9cfeebfb --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/setup.py @@ -0,0 +1,55 @@ +from setuptools import setup + +package_name = 'opendr_perception' + +setup( + name=package_name, + version='1.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='OpenDR Project Coordinator', + maintainer_email='tefas@csd.auth.gr', + description='OpenDR ROS2 nodes for the perception package', + license='Apache License v2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'pose_estimation = opendr_perception.pose_estimation_node:main', + 'hr_pose_estimation = opendr_perception.hr_pose_estimation_node:main', + 'object_detection_2d_centernet = opendr_perception.object_detection_2d_centernet_node:main', + 'object_detection_2d_detr = opendr_perception.object_detection_2d_detr_node:main', + 'object_detection_2d_yolov3 = opendr_perception.object_detection_2d_yolov3_node:main', + 'object_detection_2d_yolov5 = opendr_perception.object_detection_2d_yolov5_node:main', + 'object_detection_2d_ssd = opendr_perception.object_detection_2d_ssd_node:main', + 'object_detection_2d_nanodet = opendr_perception.object_detection_2d_nanodet_node:main', + 'object_detection_2d_gem = opendr_perception.object_detection_2d_gem_node:main', + 'object_tracking_2d_siamrpn = opendr_perception.object_tracking_2d_siamrpn_node:main', + 'face_detection_retinaface = opendr_perception.face_detection_retinaface_node:main', + 'semantic_segmentation_bisenet = opendr_perception.semantic_segmentation_bisenet_node:main', + 'panoptic_segmentation = opendr_perception.panoptic_segmentation_efficient_ps_node:main', + 'face_recognition = opendr_perception.face_recognition_node:main', + 'fall_detection = opendr_perception.fall_detection_node:main', + 'point_cloud_dataset = opendr_perception.point_cloud_dataset_node:main', + 'image_dataset = opendr_perception.image_dataset_node:main', + 'object_detection_3d_voxel = opendr_perception.object_detection_3d_voxel_node:main', + 'object_tracking_3d_ab3dmot = opendr_perception.object_tracking_3d_ab3dmot_node:main', + 'object_tracking_2d_fair_mot = opendr_perception.object_tracking_2d_fair_mot_node:main', + 'object_tracking_2d_deep_sort = opendr_perception.object_tracking_2d_deep_sort_node:main', + 'video_activity_recognition = opendr_perception.video_activity_recognition_node:main', + 'audiovisual_emotion_recognition = opendr_perception.audiovisual_emotion_recognition_node:main', + 'speech_command_recognition = opendr_perception.speech_command_recognition_node:main', + 'heart_anomaly_detection = opendr_perception.heart_anomaly_detection_node:main', + 'rgbd_hand_gestures_recognition = opendr_perception.rgbd_hand_gesture_recognition_node:main', + 'landmark_based_facial_expression_recognition = \ + opendr_perception.landmark_based_facial_expression_recognition_node:main', + 'facial_emotion_estimation = opendr_perception.facial_emotion_estimation_node:main', + 'skeleton_based_action_recognition = opendr_perception.skeleton_based_action_recognition_node:main', + ], + }, +) diff --git a/projects/opendr_ws_2/src/opendr_perception/test/test_copyright.py b/projects/opendr_ws_2/src/opendr_perception/test/test_copyright.py new file mode 100644 index 0000000000..cc8ff03f79 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/projects/opendr_ws_2/src/opendr_perception/test/test_flake8.py b/projects/opendr_ws_2/src/opendr_perception/test/test_flake8.py new file mode 100644 index 0000000000..27ee1078ff --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/projects/opendr_ws_2/src/opendr_perception/test/test_pep257.py b/projects/opendr_ws_2/src/opendr_perception/test/test_pep257.py new file mode 100644 index 0000000000..b234a3840f --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_perception/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/projects/opendr_ws_2/src/opendr_planning/launch/end_to_end_planning_robot_launch.py b/projects/opendr_ws_2/src/opendr_planning/launch/end_to_end_planning_robot_launch.py new file mode 100644 index 0000000000..ae61c2c1f7 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_planning/launch/end_to_end_planning_robot_launch.py @@ -0,0 +1,56 @@ +#!/usr/bin/env python +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import pathlib +import launch +from launch_ros.actions import Node +from launch import LaunchDescription +from ament_index_python.packages import get_package_share_directory +from webots_ros2_driver.webots_launcher import WebotsLauncher, Ros2SupervisorLauncher +from webots_ros2_driver.utils import controller_url_prefix + + +def generate_launch_description(): + package_dir = get_package_share_directory('opendr_planning') + robot_description = pathlib.Path(os.path.join(package_dir, 'resource', 'uav_robot.urdf')).read_text() + + webots = WebotsLauncher( + world=os.path.join(package_dir, 'worlds', 'train-no-dynamic-random-obstacles.wbt') + ) + + ros2_supervisor = Ros2SupervisorLauncher() + + e2e_UAV_robot_driver = Node( + package='webots_ros2_driver', + executable='driver', + output='screen', + additional_env={'WEBOTS_CONTROLLER_URL': controller_url_prefix() + 'quad_plus_sitl'}, + parameters=[ + {'robot_description': robot_description}, + ] + ) + + return LaunchDescription([ + webots, + e2e_UAV_robot_driver, + ros2_supervisor, + launch.actions.RegisterEventHandler( + event_handler=launch.event_handlers.OnProcessExit( + target_action=webots, + on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())], + ) + ) + ]) diff --git a/projects/opendr_ws_2/src/opendr_planning/opendr_planning/__init__.py b/projects/opendr_ws_2/src/opendr_planning/opendr_planning/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/projects/opendr_ws_2/src/opendr_planning/opendr_planning/end_to_end_planner_node.py b/projects/opendr_ws_2/src/opendr_planning/opendr_planning/end_to_end_planner_node.py new file mode 100755 index 0000000000..9cd8fa2c83 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_planning/opendr_planning/end_to_end_planner_node.py @@ -0,0 +1,98 @@ +#!/usr/bin/env python +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import rclpy +from rclpy.node import Node +import numpy as np +from cv_bridge import CvBridge +from sensor_msgs.msg import Imu, Image +from geometry_msgs.msg import PoseStamped, PointStamped +from opendr.planning.end_to_end_planning import EndToEndPlanningRLLearner +from opendr.planning.end_to_end_planning.utils.euler_quaternion_transformations import euler_from_quaternion +from opendr.planning.end_to_end_planning.utils.euler_quaternion_transformations import euler_to_quaternion + + +class EndToEndPlannerNode(Node): + + def __init__(self): + """ + Creates a ROS Node for end-to-end planner + """ + super().__init__("opendr_end_to_end_planner_node") + self.model_name = "" + self.current_pose = PoseStamped() + self.target_pose = PoseStamped() + self.current_pose.header.frame_id = "map" + self.target_pose.header.frame_id = "map" + self.bridge = CvBridge() + self.input_depth_image_topic = "/quad_plus_sitl/range_finder" + self.position_topic = "/quad_plus_sitl/gps" + self.orientation_topic = "/imu" + self.end_to_end_planner = EndToEndPlanningRLLearner(env=None) + + self.ros2_pub_current_pose = self.create_publisher(PoseStamped, 'current_uav_pose', 10) + self.ros2_pub_target_pose = self.create_publisher(PoseStamped, 'target_uav_pose', 10) + self.create_subscription(Imu, self.orientation_topic, self.imu_callback, 1) + self.create_subscription(PointStamped, self.position_topic, self.gps_callback, 1) + self.create_subscription(Image, self.input_depth_image_topic, self.range_callback, 1) + self.get_logger().info("End-to-end planning node initialized.") + + def range_callback(self, data): + image_arr = self.bridge.imgmsg_to_cv2(data) + self.range_image = ((np.clip(image_arr.reshape((64, 64, 1)), 0, 15) / 15.) * 255).astype(np.uint8) + observation = {'depth_cam': np.copy(self.range_image), 'moving_target': np.array([5, 0, 0])} + action = self.end_to_end_planner.infer(observation, deterministic=True)[0] + self.publish_poses(action) + + def gps_callback(self, data): + self.current_pose.pose.position.x = -data.point.x + self.current_pose.pose.position.y = -data.point.y + self.current_pose.pose.position.z = data.point.z + + def imu_callback(self, data): + self.current_orientation = data.orientation + self.current_yaw = euler_from_quaternion(data.orientation)["yaw"] + self.current_pose.pose.orientation = euler_to_quaternion(0, 0, yaw=self.current_yaw) + + def model_name_callback(self, data): + if data.data[:5] == "robot": + self.model_name = data.data + if data.data[:4] == "quad": + self.model_name = data.data + + def publish_poses(self, action): + self.ros2_pub_current_pose.publish(self.current_pose) + forward_step = np.cos(action[0] * 22.5 / 180 * np.pi) + side_step = np.sin(action[0] * 22.5 / 180 * np.pi) + yaw_step = action[1] * 22.5 / 180 * np.pi + self.target_pose.pose.position.x = self.current_pose.pose.position.x + forward_step * np.cos( + self.current_yaw) - side_step * np.sin(self.current_yaw) + self.target_pose.pose.position.y = self.current_pose.pose.position.y + forward_step * np.sin( + self.current_yaw) + side_step * np.cos(self.current_yaw) + self.target_pose.pose.position.z = self.current_pose.pose.position.z + self.target_pose.pose.orientation = euler_to_quaternion(0, 0, yaw=self.current_yaw + yaw_step) + self.ros2_pub_target_pose.publish(self.target_pose) + + +def main(args=None): + rclpy.init(args=args) + end_to_end_planner_node = EndToEndPlannerNode() + rclpy.spin(end_to_end_planner_node) + end_to_end_planner_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/projects/opendr_ws_2/src/opendr_planning/opendr_planning/end_to_end_planning_robot_driver.py b/projects/opendr_ws_2/src/opendr_planning/opendr_planning/end_to_end_planning_robot_driver.py new file mode 100644 index 0000000000..39394af5c7 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_planning/opendr_planning/end_to_end_planning_robot_driver.py @@ -0,0 +1,25 @@ +#!/usr/bin/env python +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import rclpy + + +class EndToEndPlanningUAVRobotDriver: + def init(self, webots_node, properties): + rclpy.init(args=None) + self.__node = rclpy.create_node('end_to_end_planning_uav_robot_driver') + + def step(self): + rclpy.spin_once(self.__node, timeout_sec=0) diff --git a/projects/opendr_ws_2/src/opendr_planning/package.xml b/projects/opendr_ws_2/src/opendr_planning/package.xml new file mode 100644 index 0000000000..8bf5e07640 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_planning/package.xml @@ -0,0 +1,28 @@ + + + + opendr_planning + 1.0.0 + OpenDR ROS2 nodes for the planning package + OpenDR Project Coordinator + Apache License v2.0 + + webots_ros2_driver + + rclpy + + std_msgs + vision_msgs + geometry_msgs + + opendr_bridge + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/projects/opendr_ws_2/src/opendr_planning/protos/box.proto b/projects/opendr_ws_2/src/opendr_planning/protos/box.proto new file mode 100644 index 0000000000..9c34af8955 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_planning/protos/box.proto @@ -0,0 +1,88 @@ +#VRML_SIM R2022b utf8 +# license: Copyright Cyberbotics Ltd. Licensed for use only with Webots. +# license url: https://cyberbotics.com/webots_assets_license +# This bounding object with a pipe shape is formed by a group of boxes. +PROTO box [ + field SFFloat height 0.2 # Defines the height of the pipe. + field SFFloat radius 0.5 # Defines the radius of the pipe. + field SFFloat thickness 0.05 # Defines the thickness of the pipe. + field SFInt32 subdivision 8 # Defines the number of polygons used to represent the pipe and so its resolution. + field SFFloat accuracy 0.0001 # Defines how much boxes position can differ on y axis: a 0 value represents an error-free model but it will slow down the simulation. +] +{ + %{ + local wbrandom = require('wbrandom') + + -- parameter checking + local subdivision = fields.subdivision.value + if subdivision > 200 then + io.stderr:write("High value for 'subdivision'. This can slow down the simulation\n") + elseif subdivision < 8 then + io.stderr:write("'subdivision' must be greater than or equal to 8\n") + subdivision = 8 + end + + local height = fields.height.value + if height <= 0 then + io.stderr:write("'height' must be greater than 0\n") + height = fields.height.defaultValue + end + + local radius = fields.radius.value + if radius <= 0 then + io.stderr:write("'radius' must be greater than 0\n") + radius = fields.radius.defaultValue + end + + local thickness = fields.thickness.value + if thickness <= 0 then + io.stderr:write("'thickness' must be greater than 0\n") + thickness = radius / 2 + elseif thickness >= fields.radius.value then + io.stderr:write("'thickness' must be smaller than 'radius'\n") + thickness = radius / 2 + end + + -- global stuff before entering in the main loop + local beta = 2.0 * math.pi / subdivision + local alpha = beta / 2.0 + local innerRadius = radius - thickness + local su = radius * math.cos(alpha) - innerRadius + if su < 0 then + -- fixed edge case: + -- There are 2 inner radius, depending if we measure it along the center or along the edge of the boxes. + -- If the thickness is below the difference of these two radius, then the algorithm can not achieve. + io.stderr:write("Either 'thickness' or 'subdivision' are too small for the box subdivision algorithm.\n") + su = math.abs(su) + end + local sv = height + local sw = radius * math.sin(alpha) * 2.0 + local boxRadius = innerRadius + su / 2.0 + }% + Group { # set of boxes + children [ + %{ for i = 0, (subdivision - 1) do }% + %{ + -- position of an internal box + local gamma = beta * i + beta / 2 + local ax = boxRadius * math.sin(gamma) + local ay = 0 + local az = boxRadius * math.cos(gamma) + local angle = gamma + 0.5 * math.pi + -- add small offset to boxes y translation to reduce constraints + -- on the top and bottom face due to co-planarity + local offset = wbrandom.real(-1.0, 1.0) * fields.accuracy.value; + }% + Transform { + translation %{= ax}% %{= ay + offset }% %{= az}% + rotation 0 1 0 %{= angle }% + children [ + Box { + size %{= su}% %{= sv}% %{= sw}% + } + ] + } + %{ end }% + ] + } +} diff --git a/projects/opendr_ws_2/src/opendr_planning/resource/opendr_planning b/projects/opendr_ws_2/src/opendr_planning/resource/opendr_planning new file mode 100644 index 0000000000..e69de29bb2 diff --git a/projects/opendr_ws_2/src/opendr_planning/resource/uav_robot.urdf b/projects/opendr_ws_2/src/opendr_planning/resource/uav_robot.urdf new file mode 100644 index 0000000000..7b99a8080c --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_planning/resource/uav_robot.urdf @@ -0,0 +1,34 @@ + + + + + + true + true + + + + + + true + true + + + + + + true + true + + + + + true + /imu + true + inertial_unit + + + + + diff --git a/projects/opendr_ws_2/src/opendr_planning/setup.cfg b/projects/opendr_ws_2/src/opendr_planning/setup.cfg new file mode 100644 index 0000000000..35a3135d7e --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_planning/setup.cfg @@ -0,0 +1,6 @@ +[develop] +script_dir=$base/lib/opendr_planning +[install] +install_scripts=$base/lib/opendr_planning +[build_scripts] +executable = /usr/bin/env python3 diff --git a/projects/opendr_ws_2/src/opendr_planning/setup.py b/projects/opendr_ws_2/src/opendr_planning/setup.py new file mode 100644 index 0000000000..84612024fc --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_planning/setup.py @@ -0,0 +1,31 @@ +from setuptools import setup + +package_name = 'opendr_planning' +data_files = [] +data_files.append(('share/ament_index/resource_index/packages', ['resource/' + package_name])) +data_files.append(('share/' + package_name + '/launch', ['launch/end_to_end_planning_robot_launch.py'])) +data_files.append(('share/' + package_name + '/worlds', ['worlds/train-no-dynamic-random-obstacles.wbt'])) +data_files.append(('share/' + package_name + '/protos', ['protos/box.proto'])) +data_files.append(('share/' + package_name + '/resource', ['resource/uav_robot.urdf'])) +data_files.append(('share/' + package_name, ['package.xml'])) + + +setup( + name=package_name, + version='1.0.0', + packages=[package_name], + data_files=data_files, + install_requires=['setuptools'], + zip_safe=True, + maintainer='OpenDR Project Coordinator', + maintainer_email='tefas@csd.auth.gr', + description='OpenDR ROS2 nodes for the planning package', + license='Apache License v2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'end_to_end_planner = opendr_planning.end_to_end_planner_node:main', + 'end_to_end_planning_robot_driver = opendr_planning.end_to_end_planning_robot_driver:main', + ], + }, +) diff --git a/projects/opendr_ws_2/src/opendr_planning/test/test_copyright.py b/projects/opendr_ws_2/src/opendr_planning/test/test_copyright.py new file mode 100644 index 0000000000..cc8ff03f79 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_planning/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/projects/opendr_ws_2/src/opendr_planning/test/test_flake8.py b/projects/opendr_ws_2/src/opendr_planning/test/test_flake8.py new file mode 100644 index 0000000000..27ee1078ff --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_planning/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/projects/opendr_ws_2/src/opendr_planning/test/test_pep257.py b/projects/opendr_ws_2/src/opendr_planning/test/test_pep257.py new file mode 100644 index 0000000000..b234a3840f --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_planning/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/projects/opendr_ws_2/src/opendr_planning/worlds/train-no-dynamic-random-obstacles.wbt b/projects/opendr_ws_2/src/opendr_planning/worlds/train-no-dynamic-random-obstacles.wbt new file mode 100644 index 0000000000..aff3322fe9 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_planning/worlds/train-no-dynamic-random-obstacles.wbt @@ -0,0 +1,503 @@ +#VRML_SIM R2022b utf8 + +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/appearances/protos/Grass.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/appearances/protos/Parquetry.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/floors/protos/Floor.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/apartment_structure/protos/Wall.proto" +EXTERNPROTO "../protos/box.proto" + +WorldInfo { + gravity 9.80665 + basicTimeStep 1 + FPS 15 + optimalThreadCount 4 + randomSeed 52 +} +Viewpoint { + orientation 0.2493542513111129 -0.0015806740935321666 -0.9684110484822468 3.0320770615235597 + position 31.77129355822201 3.9289180767659815 21.40152949153122 + followType "Mounted Shot" +} +DEF DEF_VEHICLE Robot { + translation -3.20133 -0.667551 2.5 + rotation 0.5387460067434838 -0.5957150074565648 -0.5957150074565648 2.15327 + children [ + Lidar { + translation 0 0.07 0 + rotation 3.4621799999783786e-06 -0.999999999993755 -7.095049999955691e-07 3.14159 + horizontalResolution 32 + fieldOfView 1.57 + verticalFieldOfView 0.1 + numberOfLayers 1 + minRange 0.3 + maxRange 5 + } + RangeFinder { + translation 0 0.1 0 + rotation -0.5773502691896258 -0.5773502691896258 -0.5773502691896258 2.0943951023931957 + maxRange 15 + } + TouchSensor { + translation 0 0.03 0 + rotation 0 1 0 1.5708 + name "touch sensor-collision" + boundingObject box { + } + } + TouchSensor { + translation 0 0.03 0.5 + rotation 0 1 0 1.5708 + name "touch sensor-safety1" + boundingObject box { + radius 1 + subdivision 12 + } + } + TouchSensor { + translation 0 0.03 1 + rotation 0 1 0 1.5708 + name "touch sensor-safety2" + boundingObject box { + radius 1.5 + subdivision 16 + } + } + Receiver { + name "receiver_main" + type "serial" + channel 1 + bufferSize 32 + } + Emitter { + name "emitter_plugin" + description "commuicates with physics plugin" + } + Shape { + appearance Appearance { + material Material { + } + } + geometry Box { + size 0.1 0.1 0.1 + } + } + Camera { + translation 0 0.12 0 + rotation 0.1294279597735375 0.9831056944488314 0.1294279597735375 -1.58783 + name "camera1" + width 128 + height 128 + } + Compass { + name "compass1" + } + GPS { + name "gps" + } + Accelerometer { + name "accelerometer1" + } + InertialUnit { + rotation 0 1 0 1.5707947122222805 + name "inertial_unit" + } + Gyro { + name "gyro1" + } + Transform { + translation 0 0 0.1 + children [ + Shape { + appearance Appearance { + material Material { + } + } + geometry DEF DEF_ARM Cylinder { + height 0.1 + radius 0.01 + } + } + ] + } + Transform { + translation -0.09999999999999999 0 0 + rotation -0.7071067811865476 0 0.7071067811865476 -3.1415923071795864 + children [ + Shape { + appearance Appearance { + material Material { + } + } + geometry USE DEF_ARM + } + ] + } + Transform { + translation 0.09999999999999999 0 0 + rotation 0 -1 0 -1.5707963071795863 + children [ + Shape { + appearance Appearance { + material Material { + diffuseColor 1 0.09999999999999999 0 + } + } + geometry USE DEF_ARM + } + ] + } + Transform { + translation 0 0 -0.1 + children [ + Shape { + appearance Appearance { + material Material { + diffuseColor 0.7999999999999999 0.7999999999999999 0.7999999999999999 + } + } + geometry USE DEF_ARM + } + ] + } + ] + name "quad_plus_sitl" + boundingObject Box { + size 0.1 0.1 0.1 + } + rotationStep 0.261799 + controller "" + customData "1" + supervisor TRUE +} +Background { + skyColor [ + 0.15 0.5 1 + ] +} +DirectionalLight { +} +Floor { + translation 0 0 -1 + rotation 0 0 1 1.5707963267948966 + size 500 750 + appearance Grass { + } +} +Floor { + translation -4 0 -0.96 + rotation 0 0 1 1.5707963267948966 + name "floor(13)" + size 0.5 30 + appearance Parquetry { + type "dark strip" + } +} +Floor { + translation -8 -14 -0.98 + rotation 0 0 1 1.5707963267948966 + name "floor(5)" + size 100 50 + appearance PBRAppearance { + baseColor 0.6 0.8 0.6 + roughness 1 + } +} +DEF cyl1 Solid { + translation -13.30571834554473 -1.447574483178714 2.7665126217916747 + rotation 0.7046199859242116 -0.2718054272768975 -0.6554635650735948 1.3264162624880482 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.6 0.3 0.0235294 + roughness 1 + metalness 0 + } + geometry DEF cyl_geo1 Cylinder { + height 1.6358972201698152 + radius 0.8305567381873773 + } + castShadows FALSE + } + ] + name "solid(6)" + boundingObject USE cyl_geo1 +} +DEF cyl2 Solid { + translation -11.573784058504305 -0.5709706439613236 2.7898036661292727 + rotation 0.80041453284557 -0.23379069518091386 -0.5519768894224041 3.004019614452083 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.6 0.3 0.0235294 + roughness 1 + metalness 0 + } + geometry DEF cyl_geo2 Cylinder { + height 1.5666220746502095 + radius 1.4073464879682038 + } + castShadows FALSE + } + ] + name "solid(16)" + boundingObject USE cyl_geo2 +} +DEF cyl3 Solid { + translation 6.495757807871515 -1.6144414097525925 2.055833951531991 + rotation 0.9501520694787192 0.1803287878394691 -0.254347347424059 1.1144016628344635 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.6 0.3 0.0235294 + roughness 1 + metalness 0 + } + geometry DEF cyl_geo3 Cylinder { + height 2.9932008423005847 + radius 1.3817552987759123 + } + castShadows FALSE + } + ] + name "solid(17)" + boundingObject USE cyl_geo3 +} +DEF cyl4 Solid { + translation 0 0 -10 + rotation 0.8826129905240483 -0.436261871860521 0.17512820480707927 -3.0124718491193443 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.6 0.3 0.0235294 + roughness 1 + metalness 0 + } + geometry DEF cyl_geo4 Cylinder { + height 2.040387292247227 + radius 1.7321406926258653 + } + castShadows FALSE + } + ] + name "solid(18)" + boundingObject USE cyl_geo4 +} +DEF cyl5 Solid { + translation 0 0 -10 + rotation -0.3917242543263733 0.07876246896092191 -0.9167052863683216 0.9303512269603899 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.6 0.3 0.0235294 + roughness 1 + metalness 0 + } + geometry DEF cyl_geo5 Cylinder { + height 2.4768414116000366 + radius 0.5824817005442169 + } + castShadows FALSE + } + ] + name "solid(19)" + boundingObject USE cyl_geo5 +} +DEF box1 Solid { + translation 4.4381089093275685 0.5548170365208641 2.05131692563986 + rotation 0.2448556165007751 0.9367176515026089 0.2502114474428831 -2.914945226248721 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.6 0.3 0.0235294 + roughness 1 + metalness 0 + } + geometry DEF box_geo1 Box { + size 0.8334023756695101 0.6127140086440774 2.1756103342302913 + } + castShadows FALSE + } + ] + name "solid(20)" + boundingObject USE box_geo1 +} +DEF box2 Solid { + translation 0 0 -10 + rotation -0.7163183367896099 0.6204835974021974 0.31919922577254956 2.929261604379051 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.6 0.3 0.0235294 + roughness 1 + metalness 0 + } + geometry DEF box_geo2 Box { + size 1.6555731912544518 0.8528384366701209 1.5923867066800264 + } + castShadows FALSE + } + ] + name "solid(21)" + boundingObject USE box_geo2 +} +DEF box3 Solid { + translation 0 0 -10 + rotation 0.492702975086357 0.008495842259129496 0.8701560773823055 -3.124774550627343 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.6 0.3 0.0235294 + roughness 1 + metalness 0 + } + geometry DEF box_geo3 Box { + size 1.114861834585034 1.9899789593315744 1.665194050916234 + } + castShadows FALSE + } + ] + name "solid(22)" + boundingObject USE box_geo3 +} +DEF box4 Solid { + translation 0 0 -10 + rotation -0.47381905460959706 -0.5794103506313973 0.6631584645241805 -2.2430503148315895 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.6 0.3 0.0235294 + roughness 1 + metalness 0 + } + geometry DEF box_geo4 Box { + size 1.6228519285122363 1.1501776483206156 2.2316284316140305 + } + castShadows FALSE + } + ] + name "solid(23)" + boundingObject USE box_geo4 +} +DEF box5 Solid { + translation 0 0 -10 + rotation 0.1849655628048051 0.930668272300889 0.3156648658130647 3.098971634530017 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.6 0.3 0.0235294 + roughness 1 + metalness 0 + } + geometry DEF box_geo5 Box { + size 2.198602344698272 0.9299983006419481 1.8591651370902504 + } + castShadows FALSE + } + ] + name "solid(24)" + boundingObject USE box_geo5 +} +DEF sph1 Solid { + translation -19.257198265348357 -3.1661159326488217 2.225830049481242 + rotation 0.46953082387497425 0.2604920627631049 0.8436140650017107 -2.2344190120762484 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.6 0.3 0.0235294 + roughness 1 + metalness 0 + } + geometry DEF sph_geo1 Sphere { + radius 1.35574388768385 + } + castShadows FALSE + } + ] + name "solid(25)" + boundingObject USE sph_geo1 +} +DEF sph2 Solid { + translation 0.2181211849140201 -0.5886797657584887 2.5285623758667715 + rotation 0.46953082387497425 0.2604920627631049 0.8436140650017107 -2.2344190120762484 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.6 0.3 0.0235294 + roughness 1 + metalness 0 + } + geometry DEF sph_geo2 Sphere { + radius 1.365103979645272 + } + castShadows FALSE + } + ] + name "solid(26)" + boundingObject USE sph_geo2 +} +DEF sph3 Solid { + translation 0 0 -10 + rotation 0.46953082387497425 0.2604920627631049 0.8436140650017107 -2.2344190120762484 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.6 0.3 0.0235294 + roughness 1 + metalness 0 + } + geometry DEF sph_geo3 Sphere { + radius 1.5576301083903183 + } + castShadows FALSE + } + ] + name "solid(27)" + boundingObject USE sph_geo3 +} +DEF sph4 Solid { + translation 0 0 -10 + rotation 0.46953082387497425 0.2604920627631049 0.8436140650017107 -2.2344190120762484 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.6 0.3 0.0235294 + roughness 1 + metalness 0 + } + geometry DEF sph_geo4 Sphere { + radius 1.8204413448018755 + } + castShadows FALSE + } + ] + name "solid(28)" + boundingObject USE sph_geo4 +} +DEF sph5 Solid { + translation 0 0 -10 + rotation 0.46953082387497425 0.2604920627631049 0.8436140650017107 -2.2344190120762484 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.6 0.3 0.0235294 + roughness 1 + metalness 0 + } + geometry DEF sph_geo5 Sphere { + radius 2.2713871330568587 + } + castShadows FALSE + } + ] + name "solid(29)" + boundingObject USE sph_geo5 +} +DEF wall1 Wall { + translation -4 -4.602323054921962 -9 + size 30 0.1 7 +} +DEF wall2 Wall { + translation -4 4.602323054921962 -9 + name "wall(2)" + size 30 0.1 7 +} diff --git a/projects/opendr_ws_2/src/opendr_simulation/opendr_simulation/__init__.py b/projects/opendr_ws_2/src/opendr_simulation/opendr_simulation/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/projects/opendr_ws_2/src/opendr_simulation/opendr_simulation/human_model_generation_client.py b/projects/opendr_ws_2/src/opendr_simulation/opendr_simulation/human_model_generation_client.py new file mode 100644 index 0000000000..c4ca320acc --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_simulation/opendr_simulation/human_model_generation_client.py @@ -0,0 +1,108 @@ +#!/usr/bin/env python +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import rclpy +from rclpy.node import Node + +import cv2 +import os +import argparse +from cv_bridge import CvBridge +from opendr_bridge import ROS2Bridge +from std_msgs.msg import Bool +from opendr_interface.srv import ImgToMesh +from opendr.simulation.human_model_generation.utilities.model_3D import Model_3D + + +class HumanModelGenerationClient(Node): + + def __init__(self, service_name="human_model_generation"): + """ + Creates a ROS Client for human model generation + :param service_name: The name of the service + :type service_name: str + """ + super().__init__('human_model_generation_client') + self.bridge_cv = CvBridge() + self.bridge_ros = ROS2Bridge() + self.cli = self.create_client(ImgToMesh, service_name) + while not self.cli.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') + self.req = ImgToMesh.Request() + + def send_request(self, img_rgb, img_msk, extract_pose): + """ + Send request to service assigned with the task to generate a human model from an image + :param img_rgb: The RGB image depicting a human + :type img_rgb: engine.data.Image + :param img_msk: The image, used as mask, for depicting a human's silhouette + :type img_msk: engine.data.Image + :param extract_pose: Defines whether to extract the pose of the depicted human or not + :type extract_pose: bool + :return: A tuple containing the generated human model and the extracted 3D pose + :rtype: tuple, (opendr.simulation.human_model_generation.utilities.model_3D.Model_3D, engine.target.Pose) + """ + extract_pose_ros = Bool() + extract_pose_ros.data = extract_pose + self.req.img_rgb = self.bridge_cv.cv2_to_imgmsg(img_rgb, encoding="bgr8") + self.req.img_msk = self.bridge_cv.cv2_to_imgmsg(img_msk, encoding="bgr8") + self.req.extract_pose = extract_pose_ros + self.future = self.cli.call_async(self.req) + rclpy.spin_until_future_complete(self, self.future) + resp = self.future.result() + pose = self.bridge_ros.from_ros_pose_3D(resp.pose) + vertices, triangles = self.bridge_ros.from_ros_mesh(resp.mesh) + vertex_colors = self.bridge_ros.from_ros_colors(resp.vertex_colors) + human_model = Model_3D(vertices, triangles, vertex_colors) + return human_model, pose + + +def main(): + + parser = argparse.ArgumentParser() + parser.add_argument("--srv_name", help="The name of the service", + type=str, default="human_model_generation") + parser.add_argument("--img_rgb", help="Path for RGB image", type=str, + default=os.path.join(os.environ['OPENDR_HOME'], 'projects/python/simulation/' + 'human_model_generation/demos/' + 'imgs_input/rgb/result_0004.jpg')) + parser.add_argument("--img_msk", help="Path for mask image", type=str, + default=os.path.join(os.environ['OPENDR_HOME'], 'projects/python/simulation/' + 'human_model_generation/demos/' + 'imgs_input/msk/result_0004.jpg')) + parser.add_argument("--rot_angles", help="Yaw angles for rotating the generated model", + nargs="+", default=['30', '120']) + parser.add_argument("--extract_pose", help="Whether to extract pose or not", action='store_true') + parser.add_argument("--plot_kps", help="Whether to plot the keypoints of the extracted pose", + action='store_true') + parser.add_argument("--out_path", help="Path for outputting the renderings/models", type=str, + default=os.path.join(os.environ['OPENDR_HOME'], 'projects/opendr_ws_2')) + args = parser.parse_args() + rot_angles = [int(x) for x in args.rot_angles] + img_rgb = cv2.imread(args.img_rgb) + img_msk = cv2.imread(args.img_msk) + rclpy.init() + client = HumanModelGenerationClient(service_name=args.srv_name) + [human_model, pose] = client.send_request(img_rgb, img_msk, extract_pose=args.extract_pose) + human_model.save_obj_mesh(os.path.join(args.out_path, 'human_model.obj')) + [out_imgs, _] = human_model.get_img_views(rot_angles, human_pose_3D=pose, plot_kps=args.plot_kps) + for i, out_img in enumerate(out_imgs): + cv2.imwrite(os.path.join(args.out_path, 'rendering' + str(rot_angles[i]) + '.jpg'), out_imgs[i].opencv()) + client.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/projects/opendr_ws_2/src/opendr_simulation/opendr_simulation/human_model_generation_service.py b/projects/opendr_ws_2/src/opendr_simulation/opendr_simulation/human_model_generation_service.py new file mode 100644 index 0000000000..39d1a97fa6 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_simulation/opendr_simulation/human_model_generation_service.py @@ -0,0 +1,109 @@ +#!/usr/bin/env python +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import rclpy +from rclpy.node import Node +import argparse +import os +import torch +from opendr_bridge import ROS2Bridge +from opendr.simulation.human_model_generation.pifu_generator_learner import PIFuGeneratorLearner +from opendr_interface.srv import ImgToMesh +from opendr.engine.target import Pose +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup + + +class PifuService(Node): + + def __init__(self, service_name='human_model_generation', device="cuda", checkpoint_dir='.'): + """ + Creates a ROS Service for human model generation + :param service_name: The name of the service + :type service_name: str + :param device: device on which we are running inference ('cpu' or 'cuda') + :type device: str + :param checkpoint_dir: the directory where the PIFu weights will be downloaded/loaded + :type checkpoint_dir: str + """ + super().__init__('human_model_generation_service') + self.bridge = ROS2Bridge() + self.service_name = service_name + # Initialize the pose estimation + self.model_generator = PIFuGeneratorLearner(device=device, checkpoint_dir=checkpoint_dir) + my_callback_group = MutuallyExclusiveCallbackGroup() + + self.srv = self.create_service(ImgToMesh, 'human_model_generation', self.gen_callback, callback_group=my_callback_group) + + def gen_callback(self, request, response): + """ + Callback that process the input data and publishes to the corresponding topics + :param request: The service request + :type request: SrvTypeRequest + :param response: SrvTypeResponse + :type response: The service response + :return response: SrvTypeResponse + :type response: The service response + """ + img_rgb = self.bridge.from_ros_image(request.img_rgb) + img_msk = self.bridge.from_ros_image(request.img_msk) + extract_pose = request.extract_pose.data + output = self.model_generator.infer([img_rgb], [img_msk], extract_pose=extract_pose) + if extract_pose is True: + model_3D = output[0] + pose = output[1] + else: + model_3D = output + pose = Pose([], 0.0) + verts = model_3D.get_vertices() + faces = model_3D.get_faces() + vert_colors = model_3D.vert_colors + response.mesh = self.bridge.to_ros_mesh(verts, faces) + response.vertex_colors = self.bridge.to_ros_colors(vert_colors) + response.pose = self.bridge.to_ros_pose_3D(pose) + return response + + +def main(): + + parser = argparse.ArgumentParser() + 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("--srv_name", help="The name of the service", + type=str, default="human_model_generation") + parser.add_argument("--checkpoint_dir", help="Path to directory for the checkpoints of the method's network", + type=str, default=os.path.join(os.environ['OPENDR_HOME'], 'projects/opendr_ws_2')) + args = parser.parse_args() + + try: + if args.device == "cuda" and torch.cuda.is_available(): + device = "cuda" + elif args.device == "cuda": + print("GPU not found. Using CPU instead.") + device = "cpu" + else: + print("Using CPU.") + device = "cpu" + except: + print("Using CPU.") + device = "cpu" + + rclpy.init() + pifu_service = PifuService(service_name=args.srv_name, device=device, checkpoint_dir=args.checkpoint_dir) + rclpy.spin(pifu_service) + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/projects/opendr_ws_2/src/opendr_simulation/package.xml b/projects/opendr_ws_2/src/opendr_simulation/package.xml new file mode 100644 index 0000000000..662032a372 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_simulation/package.xml @@ -0,0 +1,36 @@ + + + + opendr_simulation + 1.0.0 + OpenDR ROS2 nodes for the simulation package + OpenDR Project Coordinator + Apache License v2.0 + std_msgs + shape_msgs + sensor_msgs + vision_msgs + ament_cmake + rosidl_default_generators + rosidl_default_runtime + opendr_interface + rclpy + opendr_bridge + rosidl_interface_packages + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + ament_lint_auto + ament_lint_common + + + ament_python + + + + + + + diff --git a/projects/opendr_ws_2/src/opendr_simulation/resource/opendr_simulation b/projects/opendr_ws_2/src/opendr_simulation/resource/opendr_simulation new file mode 100644 index 0000000000..e69de29bb2 diff --git a/projects/opendr_ws_2/src/opendr_simulation/setup.cfg b/projects/opendr_ws_2/src/opendr_simulation/setup.cfg new file mode 100644 index 0000000000..58800215e6 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_simulation/setup.cfg @@ -0,0 +1,6 @@ +[develop] +script_dir=$base/lib/opendr_simulation +[install] +install_scripts=$base/lib/opendr_simulation +[build_scripts] +executable = /usr/bin/env python3 diff --git a/projects/opendr_ws_2/src/opendr_simulation/setup.py b/projects/opendr_ws_2/src/opendr_simulation/setup.py new file mode 100644 index 0000000000..935e1e7247 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_simulation/setup.py @@ -0,0 +1,27 @@ +from setuptools import setup + +package_name = 'opendr_simulation' + +setup( + name=package_name, + version='1.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='OpenDR Project Coordinator', + maintainer_email='tefas@csd.auth.gr', + description='OpenDR ROS2 nodes for the simulation package', + license='Apache License v2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'human_model_generation_service = opendr_simulation.human_model_generation_service:main', + 'human_model_generation_client = opendr_simulation.human_model_generation_client:main' + ], + }, +) diff --git a/projects/opendr_ws_2/src/opendr_simulation/test/test_copyright.py b/projects/opendr_ws_2/src/opendr_simulation/test/test_copyright.py new file mode 100644 index 0000000000..cc8ff03f79 --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_simulation/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/projects/opendr_ws_2/src/opendr_simulation/test/test_flake8.py b/projects/opendr_ws_2/src/opendr_simulation/test/test_flake8.py new file mode 100644 index 0000000000..27ee1078ff --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_simulation/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/projects/opendr_ws_2/src/opendr_simulation/test/test_pep257.py b/projects/opendr_ws_2/src/opendr_simulation/test/test_pep257.py new file mode 100644 index 0000000000..b234a3840f --- /dev/null +++ b/projects/opendr_ws_2/src/opendr_simulation/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/projects/python/simulation/synthetic_multi_view_facial_image_generation/SyntheticDataGeneration.py b/projects/python/simulation/synthetic_multi_view_facial_image_generation/SyntheticDataGeneration.py index 4df8e55fff..59e114a64c 100644 --- a/projects/python/simulation/synthetic_multi_view_facial_image_generation/SyntheticDataGeneration.py +++ b/projects/python/simulation/synthetic_multi_view_facial_image_generation/SyntheticDataGeneration.py @@ -40,9 +40,9 @@ from shutil import copyfile import cv2 import os -from algorithm.DDFA import preprocessing_1 -from algorithm.DDFA import preprocessing_2 -from algorithm.Rotate_and_Render import test_multipose +from .algorithm.DDFA import preprocessing_1 +from .algorithm.DDFA import preprocessing_2 +from .algorithm.Rotate_and_Render import test_multipose class MultiviewDataGeneration(): diff --git a/projects/python/simulation/synthetic_multi_view_facial_image_generation/algorithm/Rotate_and_Render/models/networks/__init__.py b/projects/python/simulation/synthetic_multi_view_facial_image_generation/algorithm/Rotate_and_Render/models/networks/__init__.py index 91e0febc81..36314edf6e 100644 --- a/projects/python/simulation/synthetic_multi_view_facial_image_generation/algorithm/Rotate_and_Render/models/networks/__init__.py +++ b/projects/python/simulation/synthetic_multi_view_facial_image_generation/algorithm/Rotate_and_Render/models/networks/__init__.py @@ -1,18 +1,18 @@ import torch -from algorithm.Rotate_and_Render.models.networks.base_network import BaseNetwork -from algorithm.Rotate_and_Render.models.networks import loss -from algorithm.Rotate_and_Render.models.networks import discriminator -from algorithm.Rotate_and_Render.models.networks import generator -from algorithm.Rotate_and_Render.models.networks import encoder -from algorithm.Rotate_and_Render.models.networks.render import Render -import algorithm.Rotate_and_Render.util.util as util +from .base_network import BaseNetwork +from . import loss +from . import discriminator +from . import generator +from . import encoder +from .render import Render +from ...util.util import find_class_in_module __all__ = ['loss', 'discriminator', 'generator', 'encoder', 'Render'] def find_network_using_name(target_network_name, filename): target_class_name = target_network_name + filename module_name = 'algorithm.Rotate_and_Render.models.networks.' + filename - network = util.find_class_in_module(target_class_name, module_name) + network = find_class_in_module(target_class_name, module_name) assert issubclass(network, BaseNetwork), \ "Class %s should be a subclass of BaseNetwork" % network diff --git a/projects/python/simulation/synthetic_multi_view_facial_image_generation/algorithm/Rotate_and_Render/options/base_options.py b/projects/python/simulation/synthetic_multi_view_facial_image_generation/algorithm/Rotate_and_Render/options/base_options.py index 8528a58620..8bd14494e7 100644 --- a/projects/python/simulation/synthetic_multi_view_facial_image_generation/algorithm/Rotate_and_Render/options/base_options.py +++ b/projects/python/simulation/synthetic_multi_view_facial_image_generation/algorithm/Rotate_and_Render/options/base_options.py @@ -4,8 +4,8 @@ import os from ..util import util import torch -from algorithm.Rotate_and_Render import models -from algorithm.Rotate_and_Render import data +from .. import models +from .. import data import pickle __all__ = ['math'] diff --git a/projects/python/simulation/synthetic_multi_view_facial_image_generation/algorithm/Rotate_and_Render/test_multipose.py b/projects/python/simulation/synthetic_multi_view_facial_image_generation/algorithm/Rotate_and_Render/test_multipose.py index 8e8bf09b58..d18a1c48ba 100644 --- a/projects/python/simulation/synthetic_multi_view_facial_image_generation/algorithm/Rotate_and_Render/test_multipose.py +++ b/projects/python/simulation/synthetic_multi_view_facial_image_generation/algorithm/Rotate_and_Render/test_multipose.py @@ -14,7 +14,7 @@ import torch import math from .models.networks.rotate_render import TestRender -from algorithm.Rotate_and_Render.data import dataset_info +from .data import dataset_info multiprocessing.set_start_method('spawn', force=True) __all__ = ['dataset_info'] diff --git a/src/opendr/perception/facial_expression_recognition/__init__.py b/src/opendr/perception/facial_expression_recognition/__init__.py index 90c171aa8c..cad70122f1 100644 --- a/src/opendr/perception/facial_expression_recognition/__init__.py +++ b/src/opendr/perception/facial_expression_recognition/__init__.py @@ -1,30 +1,22 @@ -from opendr.perception.facial_expression_recognition.\ +from opendr.perception.facial_expression_recognition. \ landmark_based_facial_expression_recognition.progressive_spatio_temporal_bln_learner \ import ProgressiveSpatioTemporalBLNLearner -from opendr.perception.facial_expression_recognition.\ +from opendr.perception.facial_expression_recognition. \ landmark_based_facial_expression_recognition.algorithm.datasets.CASIA_CK_data_gen \ import CK_CLASSES, CASIA_CLASSES -from opendr.perception.facial_expression_recognition.\ +from opendr.perception.facial_expression_recognition. \ landmark_based_facial_expression_recognition.algorithm.datasets.landmark_extractor import landmark_extractor -from opendr.perception.facial_expression_recognition.\ +from opendr.perception.facial_expression_recognition. \ landmark_based_facial_expression_recognition.algorithm.datasets.gen_facial_muscles_data import gen_muscle_data -from opendr.perception.facial_expression_recognition.\ +from opendr.perception.facial_expression_recognition. \ landmark_based_facial_expression_recognition.algorithm.datasets.AFEW_data_gen import data_normalization from opendr.perception.facial_expression_recognition.image_based_facial_emotion_estimation.facial_emotion_learner \ import FacialEmotionLearner -from opendr.perception.facial_expression_recognition.image_based_facial_emotion_estimation.algorithm.model \ - import cbam -from opendr.perception.facial_expression_recognition.image_based_facial_emotion_estimation.algorithm.model.\ - diversified_esr import DiversifiedESR -from opendr.perception.facial_expression_recognition.image_based_facial_emotion_estimation.algorithm.model.esr_9 \ - import ESR from opendr.perception.facial_expression_recognition.image_based_facial_emotion_estimation.algorithm.utils \ import datasets from opendr.perception.facial_expression_recognition.image_based_facial_emotion_estimation.algorithm.utils \ import image_processing - __all__ = ['ProgressiveSpatioTemporalBLNLearner', 'CK_CLASSES', 'CASIA_CLASSES', 'landmark_extractor', - 'gen_muscle_data', 'data_normalization', 'FacialEmotionLearner', 'cbam', 'datasets', - 'image_processing', 'ESR', 'DiversifiedESR'] + 'gen_muscle_data', 'data_normalization', 'FacialEmotionLearner', 'image_processing', 'datasets'] diff --git a/src/opendr/perception/facial_expression_recognition/image_based_facial_emotion_estimation/algorithm/__init__.py b/src/opendr/perception/facial_expression_recognition/image_based_facial_emotion_estimation/algorithm/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/opendr/perception/facial_expression_recognition/image_based_facial_emotion_estimation/algorithm/model/__init__.py b/src/opendr/perception/facial_expression_recognition/image_based_facial_emotion_estimation/algorithm/model/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/opendr/perception/facial_expression_recognition/image_based_facial_emotion_estimation/algorithm/utils/__init__.py b/src/opendr/perception/facial_expression_recognition/image_based_facial_emotion_estimation/algorithm/utils/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/opendr/perception/object_detection_2d/centernet/centernet_learner.py b/src/opendr/perception/object_detection_2d/centernet/centernet_learner.py index fd986af456..4df4575c00 100644 --- a/src/opendr/perception/object_detection_2d/centernet/centernet_learner.py +++ b/src/opendr/perception/object_detection_2d/centernet/centernet_learner.py @@ -531,21 +531,25 @@ def download(self, path=None, mode="pretrained", verbose=False, "centernet_voc.json") if verbose: print("Downloading metadata...") - urlretrieve(file_url, os.path.join(path, "centernet_default.json")) + file_path = os.path.join(path, "centernet_default.json") + if not os.path.exists(file_path): + urlretrieve(file_url, file_path) if verbose: print("Downloading params...") file_url = os.path.join(url, "pretrained", "centernet_voc", "centernet_voc.params") - - urlretrieve(file_url, - os.path.join(path, "centernet_voc.params")) + file_path = os.path.join(path, "centernet_voc.params") + if not os.path.exists(file_path): + urlretrieve(file_url, file_path) elif mode == "images": file_url = os.path.join(url, "images", "bicycles.jpg") if verbose: print("Downloading example image...") - urlretrieve(file_url, os.path.join(path, "bicycles.jpg")) + file_path = os.path.join(path, "bicycles.jpg") + if not os.path.exists(file_path): + urlretrieve(file_url, file_path) elif mode == "test_data": os.makedirs(os.path.join(path, "test_data"), exist_ok=True) @@ -555,17 +559,23 @@ def download(self, path=None, mode="pretrained", verbose=False, file_url = os.path.join(url, "test_data", "train.txt") if verbose: print("Downloading filelist...") - urlretrieve(file_url, os.path.join(path, "test_data", "train.txt")) + file_path = os.path.join(path, "test_data", "train.txt") + if not os.path.exists(file_path): + urlretrieve(file_url, file_path) # download image file_url = os.path.join(url, "test_data", "Images", "000040.jpg") if verbose: print("Downloading image...") - urlretrieve(file_url, os.path.join(path, "test_data", "Images", "000040.jpg")) + file_path = os.path.join(path, "test_data", "Images", "000040.jpg") + if not os.path.exists(file_path): + urlretrieve(file_url, file_path) # download annotations file_url = os.path.join(url, "test_data", "Annotations", "000040.jpg.txt") if verbose: print("Downloading annotations...") - urlretrieve(file_url, os.path.join(path, "test_data", "Annotations", "000040.jpg.txt")) + file_path = os.path.join(path, "test_data", "Annotations", "000040.jpg.txt") + if not os.path.exists(file_path): + urlretrieve(file_url, file_path) def optimize(self, target_device): """This method is not used in this implementation.""" diff --git a/src/opendr/perception/object_detection_2d/ssd/ssd_learner.py b/src/opendr/perception/object_detection_2d/ssd/ssd_learner.py index f3f2a7d379..e495a8daca 100644 --- a/src/opendr/perception/object_detection_2d/ssd/ssd_learner.py +++ b/src/opendr/perception/object_detection_2d/ssd/ssd_learner.py @@ -1,739 +1,750 @@ -# Copyright 2020-2022 OpenDR European Project -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -# general imports -import os -import time -import json -import numpy as np -import warnings -from tqdm import tqdm -from urllib.request import urlretrieve - -# gluoncv ssd imports -from gluoncv.data.transforms import presets -from gluoncv.data.batchify import Tuple, Stack, Pad -import mxnet as mx -from mxnet import gluon -from mxnet import autograd -from gluoncv import model_zoo -from gluoncv import utils as gutils -from gluoncv.loss import SSDMultiBoxLoss -from gluoncv.utils.metrics.voc_detection import VOC07MApMetric -from gluoncv.utils.metrics.coco_detection import COCODetectionMetric - -# OpenDR engine imports -from opendr.engine.learners import Learner -from opendr.engine.data import Image -from opendr.engine.target import BoundingBox, BoundingBoxList -from opendr.engine.datasets import ExternalDataset -from opendr.engine.constants import OPENDR_SERVER_URL - -# algorithm imports -from opendr.perception.object_detection_2d.utils.eval_utils import DetectionDatasetCOCOEval -from opendr.perception.object_detection_2d.datasets import DetectionDataset -from opendr.perception.object_detection_2d.datasets.transforms import ImageToNDArrayTransform, \ - BoundingBoxListToNumpyArray, \ - transform_test, pad_test -from opendr.perception.object_detection_2d.nms.utils import NMSCustom - -gutils.random.seed(0) - - -class SingleShotDetectorLearner(Learner): - supported_backbones = {"vgg16_atrous": [512, 300], - "resnet50_v1": [512], - "mobilenet1.0": [512], - "mobilenet0.25": [300], - "resnet34_v1b": [300]} - - def __init__(self, lr=1e-3, epochs=120, batch_size=8, - device='cuda', backbone='vgg16_atrous', - img_size=512, lr_schedule='', temp_path='temp', - checkpoint_after_iter=5, checkpoint_load_iter=0, - val_after=5, log_after=100, num_workers=8, - weight_decay=5e-4, momentum=0.9): - super(SingleShotDetectorLearner, self).__init__(lr=lr, batch_size=batch_size, lr_schedule=lr_schedule, - checkpoint_after_iter=checkpoint_after_iter, - checkpoint_load_iter=checkpoint_load_iter, - temp_path=temp_path, device=device, backbone=backbone) - self.epochs = epochs - self.log_after = log_after - self.val_after = val_after - self.num_workers = num_workers - self.checkpoint_str_format = "checkpoint_epoch_{}.params" - self.backbone = backbone.lower() - - if self.backbone not in self.supported_backbones: - raise ValueError(self.backbone + " backbone is not supported. Call .info() function for a complete list of " - "available backbones.") - else: - if img_size not in self.supported_backbones[self.backbone]: - raise ValueError("Image size {} is not supported for backbone {}." - "Supported image sizes: {}".format(img_size, self.backbone, - self.supported_backbones[self.backbone])) - - if 'cuda' in self.device: - if mx.context.num_gpus() > 0: - if self.device == 'cuda': - self.ctx = mx.gpu(0) - else: - self.ctx = mx.gpu(int(self.device.split(':')[1])) - else: - self.ctx = mx.cpu() - else: - self.ctx = mx.cpu() - - self.img_size = img_size - self.weight_decay = weight_decay - self.momentum = momentum - - model_name = 'ssd_{}_{}_voc'.format(self.img_size, self.backbone) - net = model_zoo.get_model(model_name, pretrained=False, pretrained_base=True, root=self.temp_path) - self._model = net - with warnings.catch_warnings(record=True): - warnings.simplefilter("always") - self._model.initialize() - self._model.collect_params().reset_ctx(self.ctx) - _, _, _ = self._model(mx.nd.zeros((1, 3, self.img_size, self.img_size), self.ctx)) - self.classes = ['None'] - - # Initialize temp path - if not os.path.exists(self.temp_path): - os.makedirs(self.temp_path) - - def info(self): - print("The following backbone and image sizes are supported:") - for k, v in self.supported_backbones.items(): - print('{}: {}'.format(k, v)) - - def save(self, path, verbose=False): - """ - Method for saving the current model in the path provided. - :param path: path to folder where model will be saved - :type path: str - :param verbose: whether to print a success message or not, defaults to False - :type verbose: bool, optional - """ - os.makedirs(path, exist_ok=True) - - # model_name = 'ssd_' + self.backbone - model_name = os.path.basename(path) - if verbose: - print(model_name) - metadata = {"model_paths": [], "framework": "mxnet", "format": "params", - "has_data": False, "inference_params": {}, "optimized": False, - "optimizer_info": {}, "backbone": self.backbone, "classes": self.classes} - param_filepath = model_name + ".params" - metadata["model_paths"].append(param_filepath) - - self._model.save_parameters(os.path.join(path, metadata["model_paths"][0])) - if verbose: - print("Model parameters saved.") - - with open(os.path.join(path, model_name + '.json'), 'w', encoding='utf-8') as f: - json.dump(metadata, f, ensure_ascii=False, indent=4) - if verbose: - print("Model metadata saved.") - return True - - def load(self, path, verbose=False): - """ - Loads the model from the path provided, based on the metadata .json file included. - :param path: path of the directory where the model was saved - :type path: str - :param verbose: whether to print a success message or not, defaults to False - :type verbose: bool, optional - """ - - model_name = os.path.basename(os.path.normpath(path)) - if verbose: - print("Model name:", model_name, "-->", os.path.join(path, model_name + ".json")) - with open(os.path.join(path, model_name + ".json")) as f: - metadata = json.load(f) - - self.backbone = metadata["backbone"] - self.__create_model(metadata["classes"]) - - self._model.load_parameters(os.path.join(path, metadata["model_paths"][0])) - self._model.collect_params().reset_ctx(self.ctx) - self._model.hybridize(static_alloc=True, static_shape=True) - if verbose: - print("Loaded parameters and metadata.") - return True - - def download(self, path=None, mode="pretrained", verbose=False, - url=OPENDR_SERVER_URL + "/perception/object_detection_2d/ssd/"): - """ - Downloads all files necessary for inference, evaluation and training. Valid mode options are: ["pretrained", - "images", "test_data"]. - :param path: folder to which files will be downloaded, if None self.temp_path will be used - :type path: str, optional - :param mode: one of: ["pretrained", "images", "test_data"], where "pretrained" downloads a pretrained - network depending on the self.backbone type, "images" downloads example inference data, "backbone" downloads a - pretrained resnet backbone for training, and "annotations" downloads additional annotation files for training - :type mode: str, optional - :param verbose: if True, additional information is printed on stdout - :type verbose: bool, optional - :param url: URL to file location on FTP server - :type url: str, optional - """ - valid_modes = ["pretrained", "images", "test_data"] - if mode not in valid_modes: - raise UserWarning("mode parameter not valid:", mode, ", file should be one of:", valid_modes) - - if path is None: - path = self.temp_path - - if not os.path.exists(path): - os.makedirs(path) - - if mode == "pretrained": - path = os.path.join(path, "ssd_default_person") - if not os.path.exists(path): - os.makedirs(path) - - if verbose: - print("Downloading pretrained model...") - - file_url = os.path.join(url, "pretrained", - "ssd_512_vgg16_atrous_wider_person", - "ssd_512_vgg16_atrous_wider_person.json") - if verbose: - print("Downloading metadata...") - urlretrieve(file_url, os.path.join(path, "ssd_default_person.json")) - - if verbose: - print("Downloading params...") - file_url = os.path.join(url, "pretrained", "ssd_512_vgg16_atrous_wider_person", - "ssd_512_vgg16_atrous_wider_person.params") - - urlretrieve(file_url, - os.path.join(path, "ssd_512_vgg16_atrous_wider_person.params")) - - elif mode == "images": - file_url = os.path.join(url, "images", "people.jpg") - if verbose: - print("Downloading example image...") - urlretrieve(file_url, os.path.join(path, "people.jpg")) - - elif mode == "test_data": - os.makedirs(os.path.join(path, "test_data"), exist_ok=True) - os.makedirs(os.path.join(path, "test_data", "Images"), exist_ok=True) - os.makedirs(os.path.join(path, "test_data", "Annotations"), exist_ok=True) - # download train.txt - file_url = os.path.join(url, "test_data", "train.txt") - if verbose: - print("Downloading filelist...") - urlretrieve(file_url, os.path.join(path, "test_data", "train.txt")) - # download image - file_url = os.path.join(url, "test_data", "Images", "000040.jpg") - if verbose: - print("Downloading image...") - urlretrieve(file_url, os.path.join(path, "test_data", "Images", "000040.jpg")) - # download annotations - file_url = os.path.join(url, "test_data", "Annotations", "000040.jpg.txt") - if verbose: - print("Downloading annotations...") - urlretrieve(file_url, os.path.join(path, "test_data", "Annotations", "000040.jpg.txt")) - - def reset(self): - """This method is not used in this implementation.""" - return NotImplementedError - - def optimize(self, target_device): - """This method is not used in this implementation.""" - return NotImplementedError - - def __create_model(self, classes): - """ - Base method for detector creation, based on gluoncv implementation. - :param classes: list of classes contained in the training set - :type classes: list - """ - # self._model = model_zoo.get_model(model_name, classes=classes, pretrained_base=True) - # self._model = model_zoo.get_model(model_name, classes=classes, pretrained=True) - # self._model.reset_class(classes, reuse_weights=[cname for cname in classes if cname in self._model.classes]) - if self._model is None or classes != self.classes: - model_name = 'ssd_{}_{}_custom'.format(self.img_size, self.backbone) - self._model = model_zoo.get_model(model_name, classes=classes, pretrained=False, pretrained_base=True, - root=self.temp_path) - with warnings.catch_warnings(record=True): - warnings.simplefilter("always") - self._model.initialize() - self._model.collect_params().reset_ctx(self.ctx) - _, _, _ = self._model(mx.nd.zeros((1, 3, self.img_size, self.img_size), self.ctx)) - - self._model.reset_class(classes) - self.classes = classes - - def fit(self, dataset, val_dataset=None, logging_path='', silent=True, verbose=True): - """ - This method is used to train the detector on the WIDER Face dataset. Validation if performed if a val_dataset is - provided. - :param dataset: training dataset; custom DetectionDataset types are supported as-is. COCO and Pascal VOC are - supported as ExternalDataset types, with 'coco' or 'voc' dataset_type attributes. - :type dataset: DetectionDataset or ExternalDataset - :param val_dataset: validation dataset object - :type val_dataset: ExternalDataset or DetectionDataset - :param logging_path: ignored - :type logging_path: str, optional - :param silent: ignored - :type silent: str, optional - :param verbose: if set to True, additional information is printed to STDOUT, defaults to True - :type verbose: bool - :return: returns stats regarding the training and validation process - :rtype: dict - """ - save_prefix = 'ssd_{}_{}_{}'.format(self.img_size, self.backbone, dataset.dataset_type) - - # convert dataset to compatible format - dataset = self.__prepare_dataset(dataset) - - # set save dir for checkpoint saving - self.__create_model(dataset.classes) - if verbose: - print("Saving models as: {}".format(save_prefix)) - - checkpoints_folder = os.path.join(self.temp_path, '{}_checkpoints'.format(save_prefix)) - if self.checkpoint_after_iter != 0 and not os.path.exists(checkpoints_folder): - # user set checkpoint_after_iter so checkpoints must be created - # create checkpoint dir - os.makedirs(checkpoints_folder, exist_ok=True) - - start_epoch = 0 - if self.checkpoint_load_iter > 0: - # user set checkpoint_load_iter, so load a checkpoint - checkpoint_name = self.checkpoint_str_format.format(self.checkpoint_load_iter) - checkpoint_path = os.path.join(checkpoints_folder, checkpoint_name) - try: - self._model.load_parameters(checkpoint_path) - start_epoch = self.checkpoint_load_iter + 1 - except FileNotFoundError as e: - e.strerror = 'No such file or directory {}'.format(checkpoint_path) - - # set device - # NOTE: multi-gpu a little bugged - if 'cuda' in self.device: - if mx.context.num_gpus() > 0: - if self.device == 'cuda': - ctx = [mx.gpu(0)] - else: - ctx = [mx.gpu(int(self.device.split(':')[1]))] - else: - ctx = [mx.cpu()] - else: - ctx = [mx.cpu()] - - with warnings.catch_warnings(record=True): - warnings.simplefilter("always") - self._model.initialize() - self._model.collect_params().reset_ctx(ctx[0]) - if verbose: - print("Network:") - print(self._model) - - # get data loader - with autograd.train_mode(): - _, _, anchors = self._model(mx.nd.zeros((1, 3, self.img_size, self.img_size), ctx[0])) - anchors = anchors.as_in_context(mx.cpu()) - - # transform dataset & get loader - train_transform = presets.ssd.SSDDefaultTrainTransform(self.img_size, self.img_size, anchors) - dataset = dataset.transform(train_transform) - - batchify_fn = Tuple(Stack(), Stack(), Stack()) - train_loader = gluon.data.DataLoader( - dataset, self.batch_size, shuffle=True, batchify_fn=batchify_fn, - last_batch='rollover', num_workers=self.num_workers - ) - - trainer = gluon.Trainer(self._model.collect_params(), - 'sgd', {'learning_rate': self.lr, - 'wd': self.weight_decay, - 'momentum': self.momentum}, - update_on_kvstore=None) - mbox_loss = SSDMultiBoxLoss() - ce_metric = mx.metric.Loss('cross_entropy_loss') - smoothl1_metric = mx.metric.Loss('smoothl1_loss') - - self._model.collect_params().reset_ctx(ctx) - self._model.hybridize(static_alloc=True, static_shape=True) - - # start training - training_dict = {"cross_entropy_loss": [], "smoothl1_loss": [], "val_map": []} - n_iters = 0 - for epoch in range(start_epoch, self.epochs): - autograd.set_training(True) - cur_lr = self.__get_lr_at(epoch) - trainer.set_learning_rate(cur_lr) - - self._model.hybridize(static_alloc=True, static_shape=True) - - tic = time.time() - # TODO: epoch + 1 - print('[Epoch {}/{} lr={}]'.format(epoch, self.epochs, trainer.learning_rate)) - ce_metric.reset() - smoothl1_metric.reset() - - for i, batch in enumerate(train_loader): - n_iters += 1 - data = gluon.utils.split_and_load(batch[0], ctx_list=ctx, batch_axis=0) - cls_targets = gluon.utils.split_and_load(batch[1], ctx_list=ctx, batch_axis=0) - box_targets = gluon.utils.split_and_load(batch[2], ctx_list=ctx, batch_axis=0) - - with autograd.record(): - cls_preds = [] - box_preds = [] - for x in data: - cls_pred, box_pred, _ = self._model(x) - cls_preds.append(cls_pred) - box_preds.append(box_pred) - sum_loss, cls_loss, box_loss = mbox_loss( - cls_preds, box_preds, cls_targets, box_targets) - autograd.backward(sum_loss) - - trainer.step(1) - - ce_metric.update(0, [l * self.batch_size for l in cls_loss]) - smoothl1_metric.update(0, [l * self.batch_size for l in box_loss]) - if n_iters % self.log_after == self.log_after - 1: - name1, loss1 = ce_metric.get() - name2, loss2 = smoothl1_metric.get() - # TODO: epoch + 1 - print('[Epoch {}][Batch {}] {}={:.3f}, {}={:.3f}'.format( - epoch, i, name1, loss1, name2, loss2 - )) - toc = time.time() - - # perform evaluation during training - if epoch % self.val_after == self.val_after - 1 and val_dataset is not None: - if verbose: - print("Model evaluation at epoch {}".format(epoch)) - eval_dict = self.eval(val_dataset) - training_dict["val_map"].append(eval_dict["map"]) - - # checkpoint saving - if self.checkpoint_after_iter > 0 and epoch % self.checkpoint_after_iter == self.checkpoint_after_iter - 1: - if verbose: - print('Saving model at epoch {}'.format(epoch)) - checkpoint_name = self.checkpoint_str_format.format(epoch) - checkpoint_filepath = os.path.join(checkpoints_folder, checkpoint_name) - self._model.save_parameters(checkpoint_filepath) - - name1, loss1 = ce_metric.get() - name2, loss2 = smoothl1_metric.get() - training_dict["cross_entropy_loss"].append(loss1) - training_dict["smoothl1_loss"].append(loss2) - # TODO: epoch + 1 - print('[Epoch {}] Training cost: {:.3f}, {}={:.3f}, {}={:.3f}'.format( - epoch, toc - tic, name1, loss1, name2, loss2 - )) - - return training_dict - - def __get_lr_at(self, epoch): - """ - Returns learning rate at current epoch depending on learning rate schedule. - :param epoch: current epoch - :type epoch: int - :return: learning rate at current epoch - :rtype: float - """ - if self.lr_schedule == '' or self.lr_schedule is None: - return self.lr - if self.lr_schedule == 'warmup': - stop_epoch = max(3, int(0.03 * self.epochs)) - if epoch <= stop_epoch: - return self.lr * (0.5 ** (stop_epoch - epoch)) - else: - return self.lr - else: - return self.lr - - def eval(self, dataset, use_subset=False, subset_size=100, verbose=False, - nms_thresh=0.45, nms_topk=400, post_nms=100): - """ - This method performs evaluation on a given dataset and returns a dictionary with the evaluation results. - :param dataset: dataset object, to perform evaluation on - :type dataset: opendr.perception.object_detection_2d.datasets.DetectionDataset or opendr.engine.data.ExternalDataset - :param use_subset: if True, only a subset of the dataset is evaluated, defaults to False - :type use_subset: bool, optional - :param subset_size: if use_subset is True, subset_size controls the size of the subset to be evaluated - :type subset_size: int, optional - :param verbose: if True, additional information is printed on stdout - :type verbose: bool, optional - :param nms_thresh: Non-maximum suppression threshold. You can specify < 0 or > 1 to disable NMS. - :type nms_thresh: float, default is 0.45 - :param nms_topk: Apply NMS to top k detection results, use -1 to disable so that every Detection result is used in NMS. - :type nms_topk: int, default is 400 - :param post_nms: Only return top post_nms detection results, the rest is discarded. - The number is based on COCO dataset which has maximum 100 objects per image. You can adjust this number if - expecting more objects. You can use -1 to return all detections. - :type post_nms: int, default is 100 - :return: dictionary containing evaluation metric names nad values - :rtype: dict - """ - autograd.set_training(False) - # NOTE: multi-gpu is a little bugged - if 'cuda' in self.device: - if mx.context.num_gpus() > 0: - if self.device == 'cuda': - ctx = [mx.gpu(0)] - else: - ctx = [mx.gpu(int(self.device.split(':')[1]))] - else: - ctx = [mx.cpu()] - else: - ctx = [mx.cpu()] - print(self.device, ctx) - - with warnings.catch_warnings(record=True): - warnings.simplefilter("always") - self._model.initialize() - self._model.collect_params().reset_ctx(ctx) - self._model.hybridize(static_alloc=True, static_shape=True) - self._model.set_nms(nms_thresh=nms_thresh, nms_topk=nms_topk, post_nms=post_nms) - - dataset, eval_metric = self.__prepare_val_dataset(dataset, data_shape=self.img_size) - - eval_metric.reset() - - val_transform = presets.ssd.SSDDefaultValTransform(self.img_size, self.img_size) - dataset = dataset.transform(val_transform) - - val_batchify_fn = Tuple(Stack(), Pad(pad_val=-1)) - if not use_subset: - if verbose: - print('Evaluation on entire dataset...') - val_loader = gluon.data.DataLoader( - dataset, self.batch_size, shuffle=False, batchify_fn=val_batchify_fn, last_batch='keep', - num_workers=self.num_workers) - else: - print('Evaluation on subset of dataset...') - val_loader = gluon.data.DataLoader( - dataset, self.batch_size, sampler=gluon.data.RandomSampler(subset_size), - batchify_fn=val_batchify_fn, last_batch='keep', - num_workers=self.num_workers - ) - - for batch in tqdm(val_loader, total=len(val_loader)): - data = gluon.utils.split_and_load(batch[0], ctx_list=ctx, batch_axis=0, even_split=False) - label = gluon.utils.split_and_load(batch[1], ctx_list=ctx, batch_axis=0, even_split=False) - det_bboxes = [] - det_ids = [] - det_scores = [] - gt_bboxes = [] - gt_ids = [] - gt_difficults = [] - for x, y in zip(data, label): - # get prediction results - ids, scores, bboxes = self._model(x) - det_ids.append(ids) - det_scores.append(scores) - # clip to image size - det_bboxes.append(bboxes.clip(0, batch[0].shape[2])) - # split ground truths - gt_ids.append(y.slice_axis(axis=-1, begin=4, end=5)) - gt_bboxes.append(y.slice_axis(axis=-1, begin=0, end=4)) - gt_difficults.append(y.slice_axis(axis=-1, begin=5, end=6) if y.shape[-1] > 5 else np.zeros(ids.shape)) - - # update metric - eval_metric.update(det_bboxes, det_ids, det_scores, gt_bboxes, gt_ids, gt_difficults) - map_name, mean_ap = eval_metric.get() - - if verbose: - val_msg = '\n'.join(['{}={}'.format(k, v) for k, v in zip(map_name, mean_ap)]) - print(val_msg) - eval_dict = {k.lower(): v for k, v in zip(map_name, mean_ap)} - return eval_dict - - def infer(self, img, threshold=0.2, keep_size=False, custom_nms: NMSCustom=None, - nms_thresh=0.45, nms_topk=400, post_nms=100): - """ - Performs inference on a single image and returns the resulting bounding boxes. - :param img: image to perform inference on - :type img: opendr.engine.data.Image - :param threshold: confidence threshold - :type threshold: float, optional - :param keep_size: if True, the image is not resized to fit the data shape used during training - :type keep_size: bool, optional - :param custom_nms: Custom NMS method to be employed on inference - :type perception.object_detection_2d.nms.utils.nms_custom.NMSCustom - :param nms_thresh: Non-maximum suppression threshold. You can specify < 0 or > 1 to disable NMS. - :type nms_thresh: float, default is 0.45 - :param nms_topk: Apply NMS to top k detection results, use -1 to disable so that every Detection result is used in NMS. - :type nms_topk: int, default is 400 - :param post_nms: Only return top post_nms detection results, the rest is discarded. - The number is based on COCO dataset which has maximum 100 objects per image. You can adjust this number if - expecting more objects. You can use -1 to return all detections. - :type post_nms: int, default is 100 - :return: list of bounding boxes - :rtype: BoundingBoxList - """ - - assert self._model is not None, "Model has not been loaded, call load(path) first" - - if custom_nms: - self._model.set_nms(nms_thresh=0.85, nms_topk=5000, post_nms=1000) - else: - self._model.set_nms(nms_thresh=nms_thresh, nms_topk=nms_topk, post_nms=post_nms) - if not isinstance(img, Image): - img = Image(img) - _img = img.convert("channels_last", "rgb") - - height, width, _ = _img.shape - img_mx = mx.image.image.nd.from_numpy(np.float32(_img)) - - if keep_size: - x, img_mx = transform_test(img_mx) - else: - x, img_mx = presets.ssd.transform_test(img_mx, short=self.img_size) - h_mx, w_mx, _ = img_mx.shape - x = pad_test(x, min_size=self.img_size) - x = x.as_in_context(self.ctx) - class_IDs, scores, boxes = self._model(x) - - class_IDs = class_IDs[0, :, 0].asnumpy() - scores = scores[0, :, 0].asnumpy() - mask = np.where(class_IDs >= 0)[0] - if custom_nms is None: - mask = np.intersect1d(mask, np.where(scores > threshold)[0]) - if mask.size == 0: - return BoundingBoxList([]) - - scores = scores[mask, np.newaxis] - class_IDs = class_IDs[mask, np.newaxis] - boxes = boxes[0, mask, :].asnumpy() - if x.shape[2] > h_mx: - boxes[:, [1, 3]] -= (x.shape[2] - h_mx) - elif x.shape[3] > w_mx: - boxes[:, [0, 2]] -= (x.shape[3] - w_mx) - boxes[:, [0, 2]] /= w_mx - boxes[:, [1, 3]] /= h_mx - boxes[:, [0, 2]] *= width - boxes[:, [1, 3]] *= height - - if custom_nms is not None: - bounding_boxes, _ = custom_nms.run_nms(boxes=boxes, scores=scores, threshold=threshold, img=_img) - else: - bounding_boxes = BoundingBoxList([]) - for idx, box in enumerate(boxes): - bbox = BoundingBox(left=box[0], top=box[1], - width=box[2] - box[0], - height=box[3] - box[1], - name=class_IDs[idx, :], - score=scores[idx, :]) - bounding_boxes.data.append(bbox) - - return bounding_boxes - - @staticmethod - def __prepare_dataset(dataset, verbose=True): - """ - This internal method prepares the train dataset depending on what type of dataset is provided. - COCO is prepared according to: https://cv.gluon.ai/build/examples_datasets/mscoco.html - - If the dataset is of the DetectionDataset format, then it's a custom implementation of a dataset and all - required operations should be handled by the user, so the dataset object is just returned. - - :param dataset: the dataset - :type dataset: ExternalDataset or DetectionDataset - :param verbose: if True, additional information is printed on stdout - :type verbose: bool, optional - - :return: the modified dataset - :rtype: VOCDetection, COCODetection or custom DetectionDataset depending on dataset argument - """ - supported_datasets = ['coco', 'voc'] - if isinstance(dataset, ExternalDataset): - if dataset.dataset_type.lower() not in supported_datasets: - raise UserWarning("ExternalDataset dataset_type must be one of: ", supported_datasets) - - dataset_root = dataset.path - - if verbose: - print("Loading {} type dataset...".format(dataset.dataset_type)) - - if dataset.dataset_type.lower() == 'voc': - from gluoncv.data import VOCDetection - - dataset = VOCDetection(root=dataset_root, - splits=[(2007, 'trainval'), (2012, 'trainval')]) - - elif dataset.dataset_type.lower() == 'coco': - from gluoncv.data import COCODetection - - dataset = COCODetection(root=dataset_root, - splits=['instances_train2017']) - if verbose: - print("ExternalDataset loaded.") - return dataset - elif isinstance(dataset, DetectionDataset) or issubclass(type(dataset), DetectionDataset): - dataset.set_image_transform(ImageToNDArrayTransform()) - dataset.set_target_transform(BoundingBoxListToNumpyArray()) - return dataset - else: - raise ValueError("Dataset type {} not supported".format(type(dataset))) - - @staticmethod - def __prepare_val_dataset(dataset, save_prefix='tmp', data_shape=512, verbose=True): - """ - This internal method prepares the train dataset depending on what type of dataset is provided. - COCO is prepared according to: https://cv.gluon.ai/build/examples_datasets/mscoco.html - - If the dataset is of the DetectionDataset format, then it's a custom implementation of a dataset and all - required operations should be handled by the user, so the dataset object is just returned. - - :param dataset: the dataset - :type dataset: ExternalDataset or DetectionDataset - :param save_prefix: path where detections are stored temporarily for COCO dataset evaluation - :type save_prefix: str, optional - :param data_shape: data shape in pixels used for evaluation - :type data_shape: int - :param verbose: if True, additional information is printed on stdout - :type verbose: bool, optional - :return: the modified dataset - :rtype: VOCDetection, COCODetection or custom DetectionDataset depending on dataset argument - """ - supported_datasets = ['coco', 'voc'] - if isinstance(dataset, ExternalDataset): - if dataset.dataset_type.lower() not in supported_datasets: - raise UserWarning("dataset_type must be one of: ", supported_datasets) - - dataset_root = dataset.path - - if dataset.dataset_type.lower() == 'voc': - from gluoncv.data import VOCDetection - - dataset = VOCDetection(root=dataset_root, - splits=[(2007, 'test')]) - val_metric = VOC07MApMetric(iou_thresh=0.5, class_names=dataset.classes) - return dataset, val_metric - elif dataset.dataset_type.lower() == 'coco': - from gluoncv.data import COCODetection - - dataset = COCODetection(root=dataset_root, splits='instances_val2017', - skip_empty=False) - val_metric = COCODetectionMetric( - dataset, os.path.join(save_prefix, 'eval'), cleanup=False, data_shape=(data_shape, data_shape)) - return dataset, val_metric - elif isinstance(dataset, DetectionDataset) or issubclass(type(dataset), DetectionDataset): - eval_metric = DetectionDatasetCOCOEval(dataset.classes, data_shape) - dataset.set_image_transform(ImageToNDArrayTransform()) - dataset.set_target_transform(BoundingBoxListToNumpyArray()) - return dataset, eval_metric - else: - print("Dataset type {} not supported".format(type(dataset))) - return dataset, None +# Copyright 2020-2022 OpenDR European Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# general imports +import os +import time +import json +import numpy as np +import warnings +from tqdm import tqdm +from urllib.request import urlretrieve + +# gluoncv ssd imports +from gluoncv.data.transforms import presets +from gluoncv.data.batchify import Tuple, Stack, Pad +import mxnet as mx +from mxnet import gluon +from mxnet import autograd +from gluoncv import model_zoo +from gluoncv import utils as gutils +from gluoncv.loss import SSDMultiBoxLoss +from gluoncv.utils.metrics.voc_detection import VOC07MApMetric +from gluoncv.utils.metrics.coco_detection import COCODetectionMetric + +# OpenDR engine imports +from opendr.engine.learners import Learner +from opendr.engine.data import Image +from opendr.engine.target import BoundingBox, BoundingBoxList +from opendr.engine.datasets import ExternalDataset +from opendr.engine.constants import OPENDR_SERVER_URL + +# algorithm imports +from opendr.perception.object_detection_2d.utils.eval_utils import DetectionDatasetCOCOEval +from opendr.perception.object_detection_2d.datasets import DetectionDataset +from opendr.perception.object_detection_2d.datasets.transforms import ImageToNDArrayTransform, \ + BoundingBoxListToNumpyArray, \ + transform_test, pad_test +from opendr.perception.object_detection_2d.nms.utils import NMSCustom + +gutils.random.seed(0) + + +class SingleShotDetectorLearner(Learner): + supported_backbones = {"vgg16_atrous": [512, 300], + "resnet50_v1": [512], + "mobilenet1.0": [512], + "mobilenet0.25": [300], + "resnet34_v1b": [300]} + + def __init__(self, lr=1e-3, epochs=120, batch_size=8, + device='cuda', backbone='vgg16_atrous', + img_size=512, lr_schedule='', temp_path='temp', + checkpoint_after_iter=5, checkpoint_load_iter=0, + val_after=5, log_after=100, num_workers=8, + weight_decay=5e-4, momentum=0.9): + super(SingleShotDetectorLearner, self).__init__(lr=lr, batch_size=batch_size, lr_schedule=lr_schedule, + checkpoint_after_iter=checkpoint_after_iter, + checkpoint_load_iter=checkpoint_load_iter, + temp_path=temp_path, device=device, backbone=backbone) + self.epochs = epochs + self.log_after = log_after + self.val_after = val_after + self.num_workers = num_workers + self.checkpoint_str_format = "checkpoint_epoch_{}.params" + self.backbone = backbone.lower() + + if self.backbone not in self.supported_backbones: + raise ValueError(self.backbone + " backbone is not supported. Call .info() function for a complete list of " + "available backbones.") + else: + if img_size not in self.supported_backbones[self.backbone]: + raise ValueError("Image size {} is not supported for backbone {}." + "Supported image sizes: {}".format(img_size, self.backbone, + self.supported_backbones[self.backbone])) + + if 'cuda' in self.device: + if mx.context.num_gpus() > 0: + if self.device == 'cuda': + self.ctx = mx.gpu(0) + else: + self.ctx = mx.gpu(int(self.device.split(':')[1])) + else: + self.ctx = mx.cpu() + else: + self.ctx = mx.cpu() + + self.img_size = img_size + self.weight_decay = weight_decay + self.momentum = momentum + + model_name = 'ssd_{}_{}_voc'.format(self.img_size, self.backbone) + net = model_zoo.get_model(model_name, pretrained=False, pretrained_base=True, root=self.temp_path) + self._model = net + with warnings.catch_warnings(record=True): + warnings.simplefilter("always") + self._model.initialize() + self._model.collect_params().reset_ctx(self.ctx) + _, _, _ = self._model(mx.nd.zeros((1, 3, self.img_size, self.img_size), self.ctx)) + self.classes = ['None'] + + # Initialize temp path + if not os.path.exists(self.temp_path): + os.makedirs(self.temp_path) + + def info(self): + print("The following backbone and image sizes are supported:") + for k, v in self.supported_backbones.items(): + print('{}: {}'.format(k, v)) + + def save(self, path, verbose=False): + """ + Method for saving the current model in the path provided. + :param path: path to folder where model will be saved + :type path: str + :param verbose: whether to print a success message or not, defaults to False + :type verbose: bool, optional + """ + os.makedirs(path, exist_ok=True) + + # model_name = 'ssd_' + self.backbone + model_name = os.path.basename(path) + if verbose: + print(model_name) + metadata = {"model_paths": [], "framework": "mxnet", "format": "params", + "has_data": False, "inference_params": {}, "optimized": False, + "optimizer_info": {}, "backbone": self.backbone, "classes": self.classes} + param_filepath = model_name + ".params" + metadata["model_paths"].append(param_filepath) + + self._model.save_parameters(os.path.join(path, metadata["model_paths"][0])) + if verbose: + print("Model parameters saved.") + + with open(os.path.join(path, model_name + '.json'), 'w', encoding='utf-8') as f: + json.dump(metadata, f, ensure_ascii=False, indent=4) + if verbose: + print("Model metadata saved.") + return True + + def load(self, path, verbose=False): + """ + Loads the model from the path provided, based on the metadata .json file included. + :param path: path of the directory where the model was saved + :type path: str + :param verbose: whether to print a success message or not, defaults to False + :type verbose: bool, optional + """ + + model_name = os.path.basename(os.path.normpath(path)) + if verbose: + print("Model name:", model_name, "-->", os.path.join(path, model_name + ".json")) + with open(os.path.join(path, model_name + ".json")) as f: + metadata = json.load(f) + + self.backbone = metadata["backbone"] + self.__create_model(metadata["classes"]) + + self._model.load_parameters(os.path.join(path, metadata["model_paths"][0])) + self._model.collect_params().reset_ctx(self.ctx) + self._model.hybridize(static_alloc=True, static_shape=True) + if verbose: + print("Loaded parameters and metadata.") + return True + + def download(self, path=None, mode="pretrained", verbose=False, + url=OPENDR_SERVER_URL + "/perception/object_detection_2d/ssd/"): + """ + Downloads all files necessary for inference, evaluation and training. Valid mode options are: ["pretrained", + "images", "test_data"]. + :param path: folder to which files will be downloaded, if None self.temp_path will be used + :type path: str, optional + :param mode: one of: ["pretrained", "images", "test_data"], where "pretrained" downloads a pretrained + network depending on the self.backbone type, "images" downloads example inference data, "backbone" downloads a + pretrained resnet backbone for training, and "annotations" downloads additional annotation files for training + :type mode: str, optional + :param verbose: if True, additional information is printed on stdout + :type verbose: bool, optional + :param url: URL to file location on FTP server + :type url: str, optional + """ + valid_modes = ["pretrained", "images", "test_data"] + if mode not in valid_modes: + raise UserWarning("mode parameter not valid:", mode, ", file should be one of:", valid_modes) + + if path is None: + path = self.temp_path + + if not os.path.exists(path): + os.makedirs(path) + + if mode == "pretrained": + path = os.path.join(path, "ssd_default_person") + if not os.path.exists(path): + os.makedirs(path) + + if verbose: + print("Downloading pretrained model...") + + file_url = os.path.join(url, "pretrained", + "ssd_512_vgg16_atrous_wider_person", + "ssd_512_vgg16_atrous_wider_person.json") + if verbose: + print("Downloading metadata...") + file_path = os.path.join(path, "ssd_default_person.json") + if not os.path.exists(file_path): + urlretrieve(file_url, file_path) + + if verbose: + print("Downloading params...") + file_url = os.path.join(url, "pretrained", "ssd_512_vgg16_atrous_wider_person", + "ssd_512_vgg16_atrous_wider_person.params") + + file_path = os.path.join(path, "ssd_512_vgg16_atrous_wider_person.params") + if not os.path.exists(file_path): + urlretrieve(file_url, file_path) + + elif mode == "images": + file_url = os.path.join(url, "images", "people.jpg") + if verbose: + print("Downloading example image...") + file_path = os.path.join(path, "people.jpg") + if not os.path.exists(file_path): + urlretrieve(file_url, file_path) + + elif mode == "test_data": + os.makedirs(os.path.join(path, "test_data"), exist_ok=True) + os.makedirs(os.path.join(path, "test_data", "Images"), exist_ok=True) + os.makedirs(os.path.join(path, "test_data", "Annotations"), exist_ok=True) + # download train.txt + file_url = os.path.join(url, "test_data", "train.txt") + if verbose: + print("Downloading filelist...") + file_path = os.path.join(path, "test_data", "train.txt") + if not os.path.exists(file_path): + urlretrieve(file_url, file_path) + # download image + file_url = os.path.join(url, "test_data", "Images", "000040.jpg") + if verbose: + print("Downloading image...") + file_path = os.path.join(path, "test_data", "Images", "000040.jpg") + if not os.path.exists(file_path): + urlretrieve(file_url, file_path) + # download annotations + file_url = os.path.join(url, "test_data", "Annotations", "000040.jpg.txt") + if verbose: + print("Downloading annotations...") + file_path = os.path.join(path, "test_data", "Annotations", "000040.jpg.txt") + if not os.path.exists(file_path): + urlretrieve(file_url, file_path) + + def reset(self): + """This method is not used in this implementation.""" + return NotImplementedError + + def optimize(self, target_device): + """This method is not used in this implementation.""" + return NotImplementedError + + def __create_model(self, classes): + """ + Base method for detector creation, based on gluoncv implementation. + :param classes: list of classes contained in the training set + :type classes: list + """ + # self._model = model_zoo.get_model(model_name, classes=classes, pretrained_base=True) + # self._model = model_zoo.get_model(model_name, classes=classes, pretrained=True) + # self._model.reset_class(classes, reuse_weights=[cname for cname in classes if cname in self._model.classes]) + if self._model is None or classes != self.classes: + model_name = 'ssd_{}_{}_custom'.format(self.img_size, self.backbone) + self._model = model_zoo.get_model(model_name, classes=classes, pretrained=False, pretrained_base=True, + root=self.temp_path) + with warnings.catch_warnings(record=True): + warnings.simplefilter("always") + self._model.initialize() + self._model.collect_params().reset_ctx(self.ctx) + _, _, _ = self._model(mx.nd.zeros((1, 3, self.img_size, self.img_size), self.ctx)) + + self._model.reset_class(classes) + self.classes = classes + + def fit(self, dataset, val_dataset=None, logging_path='', silent=True, verbose=True): + """ + This method is used to train the detector on the WIDER Face dataset. Validation if performed if a val_dataset is + provided. + :param dataset: training dataset; custom DetectionDataset types are supported as-is. COCO and Pascal VOC are + supported as ExternalDataset types, with 'coco' or 'voc' dataset_type attributes. + :type dataset: DetectionDataset or ExternalDataset + :param val_dataset: validation dataset object + :type val_dataset: ExternalDataset or DetectionDataset + :param logging_path: ignored + :type logging_path: str, optional + :param silent: ignored + :type silent: str, optional + :param verbose: if set to True, additional information is printed to STDOUT, defaults to True + :type verbose: bool + :return: returns stats regarding the training and validation process + :rtype: dict + """ + save_prefix = 'ssd_{}_{}_{}'.format(self.img_size, self.backbone, dataset.dataset_type) + + # convert dataset to compatible format + dataset = self.__prepare_dataset(dataset) + + # set save dir for checkpoint saving + self.__create_model(dataset.classes) + if verbose: + print("Saving models as: {}".format(save_prefix)) + + checkpoints_folder = os.path.join(self.temp_path, '{}_checkpoints'.format(save_prefix)) + if self.checkpoint_after_iter != 0 and not os.path.exists(checkpoints_folder): + # user set checkpoint_after_iter so checkpoints must be created + # create checkpoint dir + os.makedirs(checkpoints_folder, exist_ok=True) + + start_epoch = 0 + if self.checkpoint_load_iter > 0: + # user set checkpoint_load_iter, so load a checkpoint + checkpoint_name = self.checkpoint_str_format.format(self.checkpoint_load_iter) + checkpoint_path = os.path.join(checkpoints_folder, checkpoint_name) + try: + self._model.load_parameters(checkpoint_path) + start_epoch = self.checkpoint_load_iter + 1 + except FileNotFoundError as e: + e.strerror = 'No such file or directory {}'.format(checkpoint_path) + + # set device + # NOTE: multi-gpu a little bugged + if 'cuda' in self.device: + if mx.context.num_gpus() > 0: + if self.device == 'cuda': + ctx = [mx.gpu(0)] + else: + ctx = [mx.gpu(int(self.device.split(':')[1]))] + else: + ctx = [mx.cpu()] + else: + ctx = [mx.cpu()] + + with warnings.catch_warnings(record=True): + warnings.simplefilter("always") + self._model.initialize() + self._model.collect_params().reset_ctx(ctx[0]) + if verbose: + print("Network:") + print(self._model) + + # get data loader + with autograd.train_mode(): + _, _, anchors = self._model(mx.nd.zeros((1, 3, self.img_size, self.img_size), ctx[0])) + anchors = anchors.as_in_context(mx.cpu()) + + # transform dataset & get loader + train_transform = presets.ssd.SSDDefaultTrainTransform(self.img_size, self.img_size, anchors) + dataset = dataset.transform(train_transform) + + batchify_fn = Tuple(Stack(), Stack(), Stack()) + train_loader = gluon.data.DataLoader( + dataset, self.batch_size, shuffle=True, batchify_fn=batchify_fn, + last_batch='rollover', num_workers=self.num_workers + ) + + trainer = gluon.Trainer(self._model.collect_params(), + 'sgd', {'learning_rate': self.lr, + 'wd': self.weight_decay, + 'momentum': self.momentum}, + update_on_kvstore=None) + mbox_loss = SSDMultiBoxLoss() + ce_metric = mx.metric.Loss('cross_entropy_loss') + smoothl1_metric = mx.metric.Loss('smoothl1_loss') + + self._model.collect_params().reset_ctx(ctx) + self._model.hybridize(static_alloc=True, static_shape=True) + + # start training + training_dict = {"cross_entropy_loss": [], "smoothl1_loss": [], "val_map": []} + n_iters = 0 + for epoch in range(start_epoch, self.epochs): + autograd.set_training(True) + cur_lr = self.__get_lr_at(epoch) + trainer.set_learning_rate(cur_lr) + + self._model.hybridize(static_alloc=True, static_shape=True) + + tic = time.time() + # TODO: epoch + 1 + print('[Epoch {}/{} lr={}]'.format(epoch, self.epochs, trainer.learning_rate)) + ce_metric.reset() + smoothl1_metric.reset() + + for i, batch in enumerate(train_loader): + n_iters += 1 + data = gluon.utils.split_and_load(batch[0], ctx_list=ctx, batch_axis=0) + cls_targets = gluon.utils.split_and_load(batch[1], ctx_list=ctx, batch_axis=0) + box_targets = gluon.utils.split_and_load(batch[2], ctx_list=ctx, batch_axis=0) + + with autograd.record(): + cls_preds = [] + box_preds = [] + for x in data: + cls_pred, box_pred, _ = self._model(x) + cls_preds.append(cls_pred) + box_preds.append(box_pred) + sum_loss, cls_loss, box_loss = mbox_loss( + cls_preds, box_preds, cls_targets, box_targets) + autograd.backward(sum_loss) + + trainer.step(1) + + ce_metric.update(0, [l * self.batch_size for l in cls_loss]) + smoothl1_metric.update(0, [l * self.batch_size for l in box_loss]) + if n_iters % self.log_after == self.log_after - 1: + name1, loss1 = ce_metric.get() + name2, loss2 = smoothl1_metric.get() + # TODO: epoch + 1 + print('[Epoch {}][Batch {}] {}={:.3f}, {}={:.3f}'.format( + epoch, i, name1, loss1, name2, loss2 + )) + toc = time.time() + + # perform evaluation during training + if epoch % self.val_after == self.val_after - 1 and val_dataset is not None: + if verbose: + print("Model evaluation at epoch {}".format(epoch)) + eval_dict = self.eval(val_dataset) + training_dict["val_map"].append(eval_dict["map"]) + + # checkpoint saving + if self.checkpoint_after_iter > 0 and epoch % self.checkpoint_after_iter == self.checkpoint_after_iter - 1: + if verbose: + print('Saving model at epoch {}'.format(epoch)) + checkpoint_name = self.checkpoint_str_format.format(epoch) + checkpoint_filepath = os.path.join(checkpoints_folder, checkpoint_name) + self._model.save_parameters(checkpoint_filepath) + + name1, loss1 = ce_metric.get() + name2, loss2 = smoothl1_metric.get() + training_dict["cross_entropy_loss"].append(loss1) + training_dict["smoothl1_loss"].append(loss2) + # TODO: epoch + 1 + print('[Epoch {}] Training cost: {:.3f}, {}={:.3f}, {}={:.3f}'.format( + epoch, toc - tic, name1, loss1, name2, loss2 + )) + + return training_dict + + def __get_lr_at(self, epoch): + """ + Returns learning rate at current epoch depending on learning rate schedule. + :param epoch: current epoch + :type epoch: int + :return: learning rate at current epoch + :rtype: float + """ + if self.lr_schedule == '' or self.lr_schedule is None: + return self.lr + if self.lr_schedule == 'warmup': + stop_epoch = max(3, int(0.03 * self.epochs)) + if epoch <= stop_epoch: + return self.lr * (0.5 ** (stop_epoch - epoch)) + else: + return self.lr + else: + return self.lr + + def eval(self, dataset, use_subset=False, subset_size=100, verbose=False, + nms_thresh=0.45, nms_topk=400, post_nms=100): + """ + This method performs evaluation on a given dataset and returns a dictionary with the evaluation results. + :param dataset: dataset object, to perform evaluation on + :type dataset: opendr.perception.object_detection_2d.datasets.DetectionDataset or opendr.engine.data.ExternalDataset + :param use_subset: if True, only a subset of the dataset is evaluated, defaults to False + :type use_subset: bool, optional + :param subset_size: if use_subset is True, subset_size controls the size of the subset to be evaluated + :type subset_size: int, optional + :param verbose: if True, additional information is printed on stdout + :type verbose: bool, optional + :param nms_thresh: Non-maximum suppression threshold. You can specify < 0 or > 1 to disable NMS. + :type nms_thresh: float, default is 0.45 + :param nms_topk: Apply NMS to top k detection results, use -1 to disable so that every Detection result is used in NMS. + :type nms_topk: int, default is 400 + :param post_nms: Only return top post_nms detection results, the rest is discarded. + The number is based on COCO dataset which has maximum 100 objects per image. You can adjust this number if + expecting more objects. You can use -1 to return all detections. + :type post_nms: int, default is 100 + :return: dictionary containing evaluation metric names nad values + :rtype: dict + """ + autograd.set_training(False) + # NOTE: multi-gpu is a little bugged + if 'cuda' in self.device: + if mx.context.num_gpus() > 0: + if self.device == 'cuda': + ctx = [mx.gpu(0)] + else: + ctx = [mx.gpu(int(self.device.split(':')[1]))] + else: + ctx = [mx.cpu()] + else: + ctx = [mx.cpu()] + print(self.device, ctx) + + with warnings.catch_warnings(record=True): + warnings.simplefilter("always") + self._model.initialize() + self._model.collect_params().reset_ctx(ctx) + self._model.hybridize(static_alloc=True, static_shape=True) + self._model.set_nms(nms_thresh=nms_thresh, nms_topk=nms_topk, post_nms=post_nms) + + dataset, eval_metric = self.__prepare_val_dataset(dataset, data_shape=self.img_size) + + eval_metric.reset() + + val_transform = presets.ssd.SSDDefaultValTransform(self.img_size, self.img_size) + dataset = dataset.transform(val_transform) + + val_batchify_fn = Tuple(Stack(), Pad(pad_val=-1)) + if not use_subset: + if verbose: + print('Evaluation on entire dataset...') + val_loader = gluon.data.DataLoader( + dataset, self.batch_size, shuffle=False, batchify_fn=val_batchify_fn, last_batch='keep', + num_workers=self.num_workers) + else: + print('Evaluation on subset of dataset...') + val_loader = gluon.data.DataLoader( + dataset, self.batch_size, sampler=gluon.data.RandomSampler(subset_size), + batchify_fn=val_batchify_fn, last_batch='keep', + num_workers=self.num_workers + ) + + for batch in tqdm(val_loader, total=len(val_loader)): + data = gluon.utils.split_and_load(batch[0], ctx_list=ctx, batch_axis=0, even_split=False) + label = gluon.utils.split_and_load(batch[1], ctx_list=ctx, batch_axis=0, even_split=False) + det_bboxes = [] + det_ids = [] + det_scores = [] + gt_bboxes = [] + gt_ids = [] + gt_difficults = [] + for x, y in zip(data, label): + # get prediction results + ids, scores, bboxes = self._model(x) + det_ids.append(ids) + det_scores.append(scores) + # clip to image size + det_bboxes.append(bboxes.clip(0, batch[0].shape[2])) + # split ground truths + gt_ids.append(y.slice_axis(axis=-1, begin=4, end=5)) + gt_bboxes.append(y.slice_axis(axis=-1, begin=0, end=4)) + gt_difficults.append(y.slice_axis(axis=-1, begin=5, end=6) if y.shape[-1] > 5 else np.zeros(ids.shape)) + + # update metric + eval_metric.update(det_bboxes, det_ids, det_scores, gt_bboxes, gt_ids, gt_difficults) + map_name, mean_ap = eval_metric.get() + + if verbose: + val_msg = '\n'.join(['{}={}'.format(k, v) for k, v in zip(map_name, mean_ap)]) + print(val_msg) + eval_dict = {k.lower(): v for k, v in zip(map_name, mean_ap)} + return eval_dict + + def infer(self, img, threshold=0.2, keep_size=False, custom_nms: NMSCustom=None, + nms_thresh=0.45, nms_topk=400, post_nms=100): + """ + Performs inference on a single image and returns the resulting bounding boxes. + :param img: image to perform inference on + :type img: opendr.engine.data.Image + :param threshold: confidence threshold + :type threshold: float, optional + :param keep_size: if True, the image is not resized to fit the data shape used during training + :type keep_size: bool, optional + :param custom_nms: Custom NMS method to be employed on inference + :type perception.object_detection_2d.nms.utils.nms_custom.NMSCustom + :param nms_thresh: Non-maximum suppression threshold. You can specify < 0 or > 1 to disable NMS. + :type nms_thresh: float, default is 0.45 + :param nms_topk: Apply NMS to top k detection results, use -1 to disable so that every Detection result is used in NMS. + :type nms_topk: int, default is 400 + :param post_nms: Only return top post_nms detection results, the rest is discarded. + The number is based on COCO dataset which has maximum 100 objects per image. You can adjust this number if + expecting more objects. You can use -1 to return all detections. + :type post_nms: int, default is 100 + :return: list of bounding boxes + :rtype: BoundingBoxList + """ + + assert self._model is not None, "Model has not been loaded, call load(path) first" + + if custom_nms: + self._model.set_nms(nms_thresh=0.85, nms_topk=5000, post_nms=1000) + else: + self._model.set_nms(nms_thresh=nms_thresh, nms_topk=nms_topk, post_nms=post_nms) + if not isinstance(img, Image): + img = Image(img) + _img = img.convert("channels_last", "rgb") + + height, width, _ = _img.shape + img_mx = mx.image.image.nd.from_numpy(np.float32(_img)) + + if keep_size: + x, img_mx = transform_test(img_mx) + else: + x, img_mx = presets.ssd.transform_test(img_mx, short=self.img_size) + h_mx, w_mx, _ = img_mx.shape + x = pad_test(x, min_size=self.img_size) + x = x.as_in_context(self.ctx) + class_IDs, scores, boxes = self._model(x) + + class_IDs = class_IDs[0, :, 0].asnumpy() + scores = scores[0, :, 0].asnumpy() + mask = np.where(class_IDs >= 0)[0] + if custom_nms is None: + mask = np.intersect1d(mask, np.where(scores > threshold)[0]) + if mask.size == 0: + return BoundingBoxList([]) + + scores = scores[mask, np.newaxis] + class_IDs = class_IDs[mask, np.newaxis] + boxes = boxes[0, mask, :].asnumpy() + if x.shape[2] > h_mx: + boxes[:, [1, 3]] -= (x.shape[2] - h_mx) + elif x.shape[3] > w_mx: + boxes[:, [0, 2]] -= (x.shape[3] - w_mx) + boxes[:, [0, 2]] /= w_mx + boxes[:, [1, 3]] /= h_mx + boxes[:, [0, 2]] *= width + boxes[:, [1, 3]] *= height + + if custom_nms is not None: + bounding_boxes, _ = custom_nms.run_nms(boxes=boxes, scores=scores, threshold=threshold, img=_img) + else: + bounding_boxes = BoundingBoxList([]) + for idx, box in enumerate(boxes): + bbox = BoundingBox(left=box[0], top=box[1], + width=box[2] - box[0], + height=box[3] - box[1], + name=class_IDs[idx, :], + score=scores[idx, :]) + bounding_boxes.data.append(bbox) + + return bounding_boxes + + @staticmethod + def __prepare_dataset(dataset, verbose=True): + """ + This internal method prepares the train dataset depending on what type of dataset is provided. + COCO is prepared according to: https://cv.gluon.ai/build/examples_datasets/mscoco.html + + If the dataset is of the DetectionDataset format, then it's a custom implementation of a dataset and all + required operations should be handled by the user, so the dataset object is just returned. + + :param dataset: the dataset + :type dataset: ExternalDataset or DetectionDataset + :param verbose: if True, additional information is printed on stdout + :type verbose: bool, optional + + :return: the modified dataset + :rtype: VOCDetection, COCODetection or custom DetectionDataset depending on dataset argument + """ + supported_datasets = ['coco', 'voc'] + if isinstance(dataset, ExternalDataset): + if dataset.dataset_type.lower() not in supported_datasets: + raise UserWarning("ExternalDataset dataset_type must be one of: ", supported_datasets) + + dataset_root = dataset.path + + if verbose: + print("Loading {} type dataset...".format(dataset.dataset_type)) + + if dataset.dataset_type.lower() == 'voc': + from gluoncv.data import VOCDetection + + dataset = VOCDetection(root=dataset_root, + splits=[(2007, 'trainval'), (2012, 'trainval')]) + + elif dataset.dataset_type.lower() == 'coco': + from gluoncv.data import COCODetection + + dataset = COCODetection(root=dataset_root, + splits=['instances_train2017']) + if verbose: + print("ExternalDataset loaded.") + return dataset + elif isinstance(dataset, DetectionDataset) or issubclass(type(dataset), DetectionDataset): + dataset.set_image_transform(ImageToNDArrayTransform()) + dataset.set_target_transform(BoundingBoxListToNumpyArray()) + return dataset + else: + raise ValueError("Dataset type {} not supported".format(type(dataset))) + + @staticmethod + def __prepare_val_dataset(dataset, save_prefix='tmp', data_shape=512, verbose=True): + """ + This internal method prepares the train dataset depending on what type of dataset is provided. + COCO is prepared according to: https://cv.gluon.ai/build/examples_datasets/mscoco.html + + If the dataset is of the DetectionDataset format, then it's a custom implementation of a dataset and all + required operations should be handled by the user, so the dataset object is just returned. + + :param dataset: the dataset + :type dataset: ExternalDataset or DetectionDataset + :param save_prefix: path where detections are stored temporarily for COCO dataset evaluation + :type save_prefix: str, optional + :param data_shape: data shape in pixels used for evaluation + :type data_shape: int + :param verbose: if True, additional information is printed on stdout + :type verbose: bool, optional + :return: the modified dataset + :rtype: VOCDetection, COCODetection or custom DetectionDataset depending on dataset argument + """ + supported_datasets = ['coco', 'voc'] + if isinstance(dataset, ExternalDataset): + if dataset.dataset_type.lower() not in supported_datasets: + raise UserWarning("dataset_type must be one of: ", supported_datasets) + + dataset_root = dataset.path + + if dataset.dataset_type.lower() == 'voc': + from gluoncv.data import VOCDetection + + dataset = VOCDetection(root=dataset_root, + splits=[(2007, 'test')]) + val_metric = VOC07MApMetric(iou_thresh=0.5, class_names=dataset.classes) + return dataset, val_metric + elif dataset.dataset_type.lower() == 'coco': + from gluoncv.data import COCODetection + + dataset = COCODetection(root=dataset_root, splits='instances_val2017', + skip_empty=False) + val_metric = COCODetectionMetric( + dataset, os.path.join(save_prefix, 'eval'), cleanup=False, data_shape=(data_shape, data_shape)) + return dataset, val_metric + elif isinstance(dataset, DetectionDataset) or issubclass(type(dataset), DetectionDataset): + eval_metric = DetectionDatasetCOCOEval(dataset.classes, data_shape) + dataset.set_image_transform(ImageToNDArrayTransform()) + dataset.set_target_transform(BoundingBoxListToNumpyArray()) + return dataset, eval_metric + else: + print("Dataset type {} not supported".format(type(dataset))) + return dataset, None diff --git a/src/opendr/perception/object_detection_3d/voxel_object_detection_3d/second_detector/core/cc/nms/nms_cpu.h b/src/opendr/perception/object_detection_3d/voxel_object_detection_3d/second_detector/core/cc/nms/nms_cpu.h index cbf644aac3..cc7a9f235a 100644 --- a/src/opendr/perception/object_detection_3d/voxel_object_detection_3d/second_detector/core/cc/nms/nms_cpu.h +++ b/src/opendr/perception/object_detection_3d/voxel_object_detection_3d/second_detector/core/cc/nms/nms_cpu.h @@ -6,7 +6,9 @@ #include #include #include +#include #include + namespace py = pybind11; using namespace pybind11::literals; template inline py::array_t constant(ShapeContainer shape, DType value) { diff --git a/src/opendr/perception/object_tracking_2d/deep_sort/algorithm/deep_sort_tracker.py b/src/opendr/perception/object_tracking_2d/deep_sort/algorithm/deep_sort_tracker.py index 178f933078..559139c597 100644 --- a/src/opendr/perception/object_tracking_2d/deep_sort/algorithm/deep_sort_tracker.py +++ b/src/opendr/perception/object_tracking_2d/deep_sort/algorithm/deep_sort_tracker.py @@ -56,7 +56,7 @@ def infer(self, imageWithDetections: ImageWithDetections, frame_id=None, swap_le if frame_id is not None: self.frame = frame_id - image = imageWithDetections.numpy().transpose(2, 1, 0) + image = imageWithDetections.numpy().transpose(1, 2, 0) detections = imageWithDetections.boundingBoxList bbox_xywh = [] @@ -73,7 +73,7 @@ def infer(self, imageWithDetections: ImageWithDetections, frame_id=None, swap_le cls_conf.append(detection.confidence) cls_ids.append(detection.name) - bbox_xywh = np.array(bbox_xywh) + bbox_xywh = np.array(bbox_xywh).reshape(-1, 4) cls_conf = np.array(cls_conf) cls_ids = np.array(cls_ids) diff --git a/src/opendr/planning/end_to_end_planning/README.md b/src/opendr/planning/end_to_end_planning/README.md index 6324e0318a..2c764c4f96 100644 --- a/src/opendr/planning/end_to_end_planning/README.md +++ b/src/opendr/planning/end_to_end_planning/README.md @@ -7,7 +7,7 @@ This method uses reinforcement learning to train an agent that is able to genera The end-to-end planning agent is interacting with gym environment which communicates with Webots. The environment is provided with the [webots world](../../../../src/opendr/planning/end_to_end_planning/envs/webots/worlds/train-no-dynamic-random-obstacles.wbt) -that needs to be opened with Webots version 2021a in order to demonstrate the end-to-end planner. +that needs to be opened with Webots version 2022b in order to demonstrate the end-to-end planner. ### Using Ardupilot simulation environment diff --git a/src/opendr/planning/end_to_end_planning/__init__.py b/src/opendr/planning/end_to_end_planning/__init__.py index 0c6a128f84..9d31e9071e 100644 --- a/src/opendr/planning/end_to_end_planning/__init__.py +++ b/src/opendr/planning/end_to_end_planning/__init__.py @@ -1,4 +1,6 @@ from opendr.planning.end_to_end_planning.e2e_planning_learner import EndToEndPlanningRLLearner -from opendr.planning.end_to_end_planning.envs.UAV_depth_planning_env import UAVDepthPlanningEnv +import os +if os.environ.get("ROS_DISTRO") == "melodic" or os.environ.get("ROS_DISTRO") == "noetic": + from opendr.planning.end_to_end_planning.envs.UAV_depth_planning_env import UAVDepthPlanningEnv __all__ = ['EndToEndPlanningRLLearner', 'UAVDepthPlanningEnv'] diff --git a/src/opendr/planning/end_to_end_planning/dependencies.ini b/src/opendr/planning/end_to_end_planning/dependencies.ini index 01535305cb..a663fd2e11 100644 --- a/src/opendr/planning/end_to_end_planning/dependencies.ini +++ b/src/opendr/planning/end_to_end_planning/dependencies.ini @@ -9,9 +9,5 @@ python=vcstool empy gym==0.20.0 stable-baselines3==1.1.0 -[runtime] -# 'python' key expects a value using the Python requirements file format -# https://pip.pypa.io/en/stable/reference/pip_install/#requirements-file-format -python=stable-baselines3 -linux=ros-noetic-webots-ros - ros-noetic-ros-numpy + +opendr=opendr-toolkit-engine diff --git a/src/opendr/planning/end_to_end_planning/envs/UAV_depth_planning_env.py b/src/opendr/planning/end_to_end_planning/envs/UAV_depth_planning_env.py index 8450957b74..a86ef68882 100644 --- a/src/opendr/planning/end_to_end_planning/envs/UAV_depth_planning_env.py +++ b/src/opendr/planning/end_to_end_planning/envs/UAV_depth_planning_env.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. import gym -import ros_numpy +from cv_bridge import CvBridge import webots_ros.srv from gym import spaces import numpy as np @@ -70,6 +70,7 @@ def __init__(self, no_dynamics=True, discrete_actions=False): self.no_dynamics = no_dynamics # ROS connection + self.bridge = CvBridge() rospy.init_node('gym_depth_planning_environment') self.r = rospy.Rate(25) self.ros_pub_pose = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10) @@ -91,21 +92,18 @@ def __init__(self, no_dynamics=True, discrete_actions=False): rospy.loginfo("Webots model is not started!") return self.randomizer = ObstacleRandomizer(self.model_name) - rospy.Subscriber("/" + self.model_name + "/touch_sensor_collision/value", BoolStamped, self.collision_callback) - rospy.Subscriber("/" + self.model_name + "/touch_sensor_safety1/value", BoolStamped, self.safety1_callback) - rospy.Subscriber("/" + self.model_name + "/touch_sensor_safety2/value", BoolStamped, self.safety2_callback) - rospy.Subscriber("/" + self.model_name + "/range_finder/range_image", Image, self.range_callback, queue_size=1) - rospy.Subscriber("/" + self.model_name + "/lidar/range_image", Image, self.laser_callback, queue_size=1) + rospy.Subscriber("/touch_sensor_collision/value", BoolStamped, self.collision_callback) + rospy.Subscriber("/touch_sensor_safety1/value", BoolStamped, self.safety1_callback) + rospy.Subscriber("/touch_sensor_safety2/value", BoolStamped, self.safety2_callback) + rospy.Subscriber("/range_finder/range_image", Image, self.range_callback, queue_size=1) self.ros_srv_touch_sensor_collision_enable = rospy.ServiceProxy( - "/" + self.model_name + "/touch_sensor_collision/enable", webots_ros.srv.set_int) + "/touch_sensor_collision/enable", webots_ros.srv.set_int) self.ros_srv_touch_sensor_safety1_enable = rospy.ServiceProxy( - "/" + self.model_name + "/touch_sensor_safety1/enable", webots_ros.srv.set_int) + "/touch_sensor_safety1/enable", webots_ros.srv.set_int) self.ros_srv_touch_sensor_safety2_enable = rospy.ServiceProxy( - "/" + self.model_name + "/touch_sensor_safety2/enable", webots_ros.srv.set_int) + "/touch_sensor_safety2/enable", webots_ros.srv.set_int) self.ros_srv_range_sensor_enable = rospy.ServiceProxy( - "/" + self.model_name + "/range_finder/enable", webots_ros.srv.set_int) - self.ros_srv_laser_sensor_enable = rospy.ServiceProxy( - "/" + self.model_name + "/lidar/enable", webots_ros.srv.set_int) + "/range_finder/enable", webots_ros.srv.set_int) try: self.ros_srv_touch_sensor_collision_enable(1) self.ros_srv_range_sensor_enable(1) @@ -114,7 +112,6 @@ def __init__(self, no_dynamics=True, discrete_actions=False): try: self.ros_srv_touch_sensor_safety1_enable(1) self.ros_srv_touch_sensor_safety2_enable(1) - self.ros_srv_laser_sensor_enable(1) except rospy.ServiceException as exc: print("Service did not process request: " + str(exc)) @@ -130,22 +127,22 @@ def __init__(self, no_dynamics=True, discrete_actions=False): self.r.sleep() if no_dynamics: - self.ros_srv_gps1_sensor_enable = rospy.ServiceProxy( - "/" + self.model_name + "/gps1/enable", webots_ros.srv.set_int) + self.ros_srv_gps_sensor_enable = rospy.ServiceProxy( + "/gps/enable", webots_ros.srv.set_int) self.ros_srv_inertial_unit_enable = rospy.ServiceProxy( - "/" + self.model_name + "/inertial_unit/enable", webots_ros.srv.set_int) + "/inertial_unit/enable", webots_ros.srv.set_int) self.ros_srv_get_self = rospy.ServiceProxy( - "/" + self.model_name + "/supervisor/get_self", webots_ros.srv.get_uint64) + "/supervisor/get_self", webots_ros.srv.get_uint64) self.ros_srv_get_field = rospy.ServiceProxy( - "/" + self.model_name + "/supervisor/node/get_field", webots_ros.srv.node_get_field) + "/supervisor/node/get_field", webots_ros.srv.node_get_field) self.ros_srv_field_set_v3 = rospy.ServiceProxy( - "/" + self.model_name + "/supervisor/field/set_vec3f", webots_ros.srv.field_set_vec3f) + "/supervisor/field/set_vec3f", webots_ros.srv.field_set_vec3f) self.ros_srv_field_set_rotation = rospy.ServiceProxy( - "/" + self.model_name + "/supervisor/field/set_rotation", webots_ros.srv.field_set_rotation) - rospy.Subscriber("/" + self.model_name + "/inertial_unit/quaternion", Imu, self.imu_callback) - rospy.Subscriber("/" + self.model_name + "/gps1/values", PointStamped, self.gps_callback) + "/supervisor/field/set_rotation", webots_ros.srv.field_set_rotation) + rospy.Subscriber("/inertial_unit/quaternion", Imu, self.imu_callback) + rospy.Subscriber("/gps/values", PointStamped, self.gps_callback) try: - self.ros_srv_gps1_sensor_enable(1) + self.ros_srv_gps_sensor_enable(1) self.ros_srv_inertial_unit_enable(1) except rospy.ServiceException as exc: print("Service did not process request: " + str(exc)) @@ -302,13 +299,9 @@ def pose_callback(self, data): self.current_yaw = euler_from_quaternion(data.pose.orientation)["yaw"] def range_callback(self, data): - image_arr = ros_numpy.numpify(data) + image_arr = self.bridge.imgmsg_to_cv2(data) self.range_image = ((np.clip(image_arr.reshape((64, 64, 1)), 0, 15) / 15.) * 255).astype(np.uint8) - def laser_callback(self, data): - image_arr = ros_numpy.numpify(data) - self.closer_object_length = np.min(np.clip(image_arr, 0, 5)) - def model_name_callback(self, data): if data.data[:5] == "robot": self.model_name = data.data @@ -328,13 +321,13 @@ def safety2_callback(self, data): self.safety2_flag = True def gps_callback(self, data): # for no dynamics - self.current_position.x = data.point.z - self.current_position.y = data.point.x - self.current_position.z = data.point.y + self.current_position.x = -data.point.x + self.current_position.y = -data.point.y + self.current_position.z = data.point.z def imu_callback(self, data): # for no dynamics self.current_orientation = data.orientation - self.current_yaw = euler_from_quaternion(data.orientation)["roll"] + self.current_yaw = euler_from_quaternion(data.orientation)["yaw"] def go_position(self, x, y, z, yaw=0, check_collision=False): if self.no_dynamics: @@ -343,11 +336,11 @@ def go_position(self, x, y, z, yaw=0, check_collision=False): goal.header.seq = 1 goal.header.stamp = rospy.Time.now() - goal.pose.position.x = y - goal.pose.position.y = z - goal.pose.position.z = x + goal.pose.position.x = -x + goal.pose.position.y = -y + goal.pose.position.z = z - goal.pose.orientation = euler_to_quaternion(0, yaw, 0) + goal.pose.orientation = euler_to_quaternion(np.pi/2, 0, -np.pi/2+yaw) try: self.ros_srv_field_set_v3(self.robot_translation_field, 0, goal.pose.position) self.ros_srv_field_set_rotation(self.robot_rotation_field, 0, goal.pose.orientation) diff --git a/src/opendr/planning/end_to_end_planning/envs/webots/protos/box.proto b/src/opendr/planning/end_to_end_planning/envs/webots/protos/box.proto index 8716af34c0..9c34af8955 100644 --- a/src/opendr/planning/end_to_end_planning/envs/webots/protos/box.proto +++ b/src/opendr/planning/end_to_end_planning/envs/webots/protos/box.proto @@ -1,16 +1,13 @@ -#VRML_SIM R2021a utf8 +#VRML_SIM R2022b utf8 # license: Copyright Cyberbotics Ltd. Licensed for use only with Webots. # license url: https://cyberbotics.com/webots_assets_license -# tags: static # This bounding object with a pipe shape is formed by a group of boxes. - PROTO box [ - field SFFloat height 0.2 - # Defines the height of the pipe. - field SFFloat radius 0.5 # Defines the radius of the pipe. - field SFFloat thickness 0.05 # Defines the thickness of the pipe. + field SFFloat height 0.2 # Defines the height of the pipe. + field SFFloat radius 0.5 # Defines the radius of the pipe. + field SFFloat thickness 0.05 # Defines the thickness of the pipe. field SFInt32 subdivision 8 # Defines the number of polygons used to represent the pipe and so its resolution. - field SFFloat accuracy 0.0001 # Defines how much boxes position can differ on y axis: a 0 value represents an error-free model but it will slow down the simulation. + field SFFloat accuracy 0.0001 # Defines how much boxes position can differ on y axis: a 0 value represents an error-free model but it will slow down the simulation. ] { %{ @@ -73,7 +70,7 @@ PROTO box [ local az = boxRadius * math.cos(gamma) local angle = gamma + 0.5 * math.pi -- add small offset to boxes y translation to reduce constraints - -- on the top and bottm face due to co-planarity + -- on the top and bottom face due to co-planarity local offset = wbrandom.real(-1.0, 1.0) * fields.accuracy.value; }% Transform { diff --git a/src/opendr/planning/end_to_end_planning/envs/webots/worlds/train-no-dynamic-random-obstacles.wbt b/src/opendr/planning/end_to_end_planning/envs/webots/worlds/train-no-dynamic-random-obstacles.wbt index 197d4f6773..61d53ceb3f 100644 --- a/src/opendr/planning/end_to_end_planning/envs/webots/worlds/train-no-dynamic-random-obstacles.wbt +++ b/src/opendr/planning/end_to_end_planning/envs/webots/worlds/train-no-dynamic-random-obstacles.wbt @@ -1,24 +1,30 @@ -#VRML_SIM R2021a utf8 +#VRML_SIM R2022b utf8 + +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/appearances/protos/Grass.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/appearances/protos/Parquetry.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/floors/protos/Floor.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/apartment_structure/protos/Wall.proto" +EXTERNPROTO "../protos/box.proto" + WorldInfo { gravity 9.80665 basicTimeStep 1 FPS 15 optimalThreadCount 4 - coordinateSystem "NUE" randomSeed 52 } Viewpoint { - orientation 0.0017367083261278448 -0.9261534688347032 -0.3771428588872342 3.069692752730281 - position -0.14937970282282664 34.32912524388594 -29.04411809573074 + orientation 0.23912921076912644 -0.010500223692226803 -0.9709309789368902 3.1000770696059305 + position 16.599575477443874 0.11479760710878642 11.449782726362042 followType "Mounted Shot" } DEF DEF_VEHICLE Robot { - translation 0.173907 2.57 -2.01714 - rotation 0 1 0 0.14106945956065933 + translation 2.01714 -0.173907 2.57 + rotation 0.5773502691896257 -0.5773502691896257 -0.5773502691896257 2.0943951023931957 children [ Lidar { translation 0 0.07 0 - rotation 0 1 0 3.141592653589793 + rotation 3.4621799999783786e-06 -0.999999999993755 -7.095049999955691e-07 3.14159 horizontalResolution 32 fieldOfView 1.57 verticalFieldOfView 0.1 @@ -28,19 +34,19 @@ DEF DEF_VEHICLE Robot { } RangeFinder { translation 0 0.1 0 - rotation 0 -1 0 3.141592653589793 + rotation -0.5773502691896258 -0.5773502691896258 -0.5773502691896258 2.0943951023931957 maxRange 15 } TouchSensor { translation 0 0.03 0 - rotation 0 1 0 1.57 + rotation 0 1 0 1.5708 name "touch sensor-collision" boundingObject box { } } TouchSensor { translation 0 0.03 0.5 - rotation 0 1 0 1.57 + rotation 0 1 0 1.5708 name "touch sensor-safety1" boundingObject box { radius 1 @@ -49,7 +55,7 @@ DEF DEF_VEHICLE Robot { } TouchSensor { translation 0 0.03 1 - rotation 0 1 0 1.57 + rotation 0 1 0 1.5708 name "touch sensor-safety2" boundingObject box { radius 1.5 @@ -77,7 +83,7 @@ DEF DEF_VEHICLE Robot { } Camera { translation 0 0.12 0 - rotation 0.12942795977353752 0.9831056944488316 0.12942795977353752 -1.5878343071795866 + rotation 0.1294279597735375 0.9831056944488314 0.1294279597735375 -1.58783 name "camera1" width 128 height 128 @@ -86,7 +92,7 @@ DEF DEF_VEHICLE Robot { name "compass1" } GPS { - name "gps1" + name "gps" } Accelerometer { name "accelerometer1" @@ -95,12 +101,11 @@ DEF DEF_VEHICLE Robot { name "gyro1" } InertialUnit { - rotation 0 -1 0 1.5707963267948968 + rotation 0 1 0 1.5707947122222805 name "inertial_unit" } Transform { - translation -0.09999999999999999 0 0 - rotation -0.5773502691896258 0.5773502691896258 0.5773502691896258 2.094395 + translation 0 0 0.1 children [ Shape { appearance Appearance { @@ -115,8 +120,8 @@ DEF DEF_VEHICLE Robot { ] } Transform { - translation 0 0 0.09999999999999999 - rotation 0 0.7071067811865476 0.7071067811865476 -3.1415923071795864 + translation -0.09999999999999999 0 0 + rotation -0.7071067811865476 0 0.7071067811865476 -3.1415923071795864 children [ Shape { appearance Appearance { @@ -129,7 +134,7 @@ DEF DEF_VEHICLE Robot { } Transform { translation 0.09999999999999999 0 0 - rotation 0.5773502691896258 0.5773502691896258 0.5773502691896258 -2.094395307179586 + rotation 0 -1 0 -1.5707963071795863 children [ Shape { appearance Appearance { @@ -142,8 +147,7 @@ DEF DEF_VEHICLE Robot { ] } Transform { - translation 0 0 -0.09999999999999999 - rotation 1 0 0 -1.5707963071795863 + translation 0 0 -0.1 children [ Shape { appearance Appearance { @@ -171,16 +175,17 @@ Background { ] } DirectionalLight { - direction 0 -1 0 } Floor { - translation 0 -1 0 + translation 0 0 -1 + rotation 0 0 1 1.5707963267948966 size 500 750 appearance Grass { } } Floor { - translation 0 -0.96 4 + translation -4 0 -0.96 + rotation 0 0 1 1.5707963267948966 name "floor(13)" size 0.5 30 appearance Parquetry { @@ -188,7 +193,8 @@ Floor { } } Floor { - translation 14 -0.98 8 + translation -8 -14 -0.98 + rotation 0 0 1 1.5707963267948966 name "floor(5)" size 100 50 appearance PBRAppearance { @@ -197,8 +203,8 @@ Floor { } } DEF cyl1 Solid { - translation -19.604630321122315 2.324472693591628 33.571763188537474 - rotation 0.008454983970179998 0.5745628144430251 -0.8184168164849583 2.855354311428324 + translation -33.571763188537474 19.604630321122315 2.324472693591628 + rotation 0.8184168164849583 -0.008454983970179998 0.5745628144430251 2.855354311428324 children [ Shape { appearance PBRAppearance { @@ -217,8 +223,8 @@ DEF cyl1 Solid { boundingObject USE cyl_geo1 } DEF cyl2 Solid { - translation -13.747862151581423 2.2037661733607323 20.440751891815367 - rotation -0.046082095001687674 0.2841215809381919 0.9576802011973716 2.291645054624233 + translation -20.440751891815367 13.747862151581423 2.2037661733607323 + rotation -0.9576802011973716 0.046082095001687674 0.2841215809381919 2.291645054624233 children [ Shape { appearance PBRAppearance { @@ -237,8 +243,8 @@ DEF cyl2 Solid { boundingObject USE cyl_geo2 } DEF cyl3 Solid { - translation -14.389081419732586 2.865288247046378 26.698438622531555 - rotation 0.037805321362627146 -0.9972687076596976 -0.06344984160283776 -2.2281994020336473 + translation -26.698438622531555 14.389081419732586 2.865288247046378 + rotation 0.06344984160283776 -0.037805321362627146 -0.9972687076596976 -2.2281994020336473 children [ Shape { appearance PBRAppearance { @@ -257,8 +263,8 @@ DEF cyl3 Solid { boundingObject USE cyl_geo3 } DEF cyl4 Solid { - translation -18.478910598526205 2.975906443581888 30.029891046849826 - rotation 0.436261871860521 0.17512820480707927 -0.8826129905240483 -3.0124718491193443 + translation -30.029891046849826 18.478910598526205 2.975906443581888 + rotation 0.8826129905240483 -0.436261871860521 0.17512820480707927 -3.0124718491193443 children [ Shape { appearance PBRAppearance { @@ -277,8 +283,8 @@ DEF cyl4 Solid { boundingObject USE cyl_geo4 } DEF cyl5 Solid { - translation -16.57710468047925 2.4982480911172904 24.09970968828449 - rotation -0.07876246896092191 -0.9167052863683216 0.3917242543263733 0.9303512269603899 + translation -24.09970968828449 16.57710468047925 2.4982480911172904 + rotation -0.3917242543263733 0.07876246896092191 -0.9167052863683216 0.9303512269603899 children [ Shape { appearance PBRAppearance { @@ -297,8 +303,8 @@ DEF cyl5 Solid { boundingObject USE cyl_geo5 } DEF box1 Solid { - translation -0.26435729418541554 2.44667080338155 21.633230654472253 - rotation 0.2846212101353512 0.1247924621830743 0.9504828289471485 -2.5557071516379524 + translation -21.633230654472253 0.26435729418541554 2.44667080338155 + rotation -0.9504828289471485 -0.2846212101353512 0.1247924621830743 -2.5557071516379524 children [ Shape { appearance PBRAppearance { @@ -307,7 +313,7 @@ DEF box1 Solid { metalness 0 } geometry DEF box_geo1 Box { - size 1.144210412169449 0.606098167324667 0.6703828008453012 + size 0.6703828008453012 1.144210412169449 0.606098167324667 } castShadows FALSE } @@ -316,8 +322,8 @@ DEF box1 Solid { boundingObject USE box_geo1 } DEF box2 Solid { - translation -1.6898234337915463 2.2675614976470575 49.944469797724835 - rotation -0.6204835974021974 0.31919922577254956 0.7163183367896099 2.929261604379051 + translation -49.944469797724835 1.6898234337915463 2.2675614976470575 + rotation -0.7163183367896099 0.6204835974021974 0.31919922577254956 2.929261604379051 children [ Shape { appearance PBRAppearance { @@ -326,7 +332,7 @@ DEF box2 Solid { metalness 0 } geometry DEF box_geo2 Box { - size 0.8528384366701209 1.5923867066800264 1.6555731912544518 + size 1.6555731912544518 0.8528384366701209 1.5923867066800264 } castShadows FALSE } @@ -335,8 +341,8 @@ DEF box2 Solid { boundingObject USE box_geo2 } DEF box3 Solid { - translation -8.238761971490536 2.6721509445938736 20.292422865902708 - rotation -0.008495842259129496 0.8701560773823055 -0.492702975086357 -3.124774550627343 + translation -20.292422865902708 8.238761971490536 2.6721509445938736 + rotation 0.492702975086357 0.008495842259129496 0.8701560773823055 -3.124774550627343 children [ Shape { appearance PBRAppearance { @@ -345,7 +351,7 @@ DEF box3 Solid { metalness 0 } geometry DEF box_geo3 Box { - size 1.9899789593315744 1.665194050916234 1.114861834585034 + size 1.114861834585034 1.9899789593315744 1.665194050916234 } castShadows FALSE } @@ -354,8 +360,8 @@ DEF box3 Solid { boundingObject USE box_geo3 } DEF box4 Solid { - translation -8.527463770969087 2.641006035191632 44.90744150542008 - rotation 0.5794103506313973 0.6631584645241805 0.47381905460959706 -2.2430503148315895 + translation -44.90744150542008 8.527463770969087 2.641006035191632 + rotation -0.47381905460959706 -0.5794103506313973 0.6631584645241805 -2.2430503148315895 children [ Shape { appearance PBRAppearance { @@ -364,7 +370,7 @@ DEF box4 Solid { metalness 0 } geometry DEF box_geo4 Box { - size 1.1501776483206156 2.2316284316140305 1.6228519285122363 + size 1.6228519285122363 1.1501776483206156 2.2316284316140305 } castShadows FALSE } @@ -373,8 +379,8 @@ DEF box4 Solid { boundingObject USE box_geo4 } DEF box5 Solid { - translation -6.8392747579709265 2.2856439867583433 38.86442228484968 - rotation -0.930668272300889 0.3156648658130647 -0.1849655628048051 3.098971634530017 + translation -38.86442228484968 6.8392747579709265 2.2856439867583433 + rotation 0.1849655628048051 0.930668272300889 0.3156648658130647 3.098971634530017 children [ Shape { appearance PBRAppearance { @@ -383,7 +389,7 @@ DEF box5 Solid { metalness 0 } geometry DEF box_geo5 Box { - size 0.9299983006419481 1.8591651370902504 2.198602344698272 + size 2.198602344698272 0.9299983006419481 1.8591651370902504 } castShadows FALSE } @@ -392,8 +398,8 @@ DEF box5 Solid { boundingObject USE box_geo5 } DEF sph1 Solid { - translation 8.39941640511953 2.832797125554921 34.151569808427524 - rotation -0.2604920627631049 0.8436140650017107 -0.46953082387497425 -2.2344190120762484 + translation -34.151569808427524 -8.39941640511953 2.832797125554921 + rotation 0.46953082387497425 0.2604920627631049 0.8436140650017107 -2.2344190120762484 children [ Shape { appearance PBRAppearance { @@ -411,8 +417,8 @@ DEF sph1 Solid { boundingObject USE sph_geo1 } DEF sph2 Solid { - translation 5.669701484639425 2.98237944209821 35.244313566036006 - rotation -0.2604920627631049 0.8436140650017107 -0.46953082387497425 -2.2344190120762484 + translation -35.244313566036006 -5.669701484639425 2.98237944209821 + rotation 0.46953082387497425 0.2604920627631049 0.8436140650017107 -2.2344190120762484 children [ Shape { appearance PBRAppearance { @@ -430,8 +436,8 @@ DEF sph2 Solid { boundingObject USE sph_geo2 } DEF sph3 Solid { - translation 7.009574816911507 2.679998597601765 45.118047101108615 - rotation -0.2604920627631049 0.8436140650017107 -0.46953082387497425 -2.2344190120762484 + translation -45.118047101108615 -7.009574816911507 2.679998597601765 + rotation 0.46953082387497425 0.2604920627631049 0.8436140650017107 -2.2344190120762484 children [ Shape { appearance PBRAppearance { @@ -449,8 +455,8 @@ DEF sph3 Solid { boundingObject USE sph_geo3 } DEF sph4 Solid { - translation 3.505564259041 2.013593906239073 35.57358399548293 - rotation -0.2604920627631049 0.8436140650017107 -0.46953082387497425 -2.2344190120762484 + translation -35.57358399548293 -3.505564259041 2.013593906239073 + rotation 0.46953082387497425 0.2604920627631049 0.8436140650017107 -2.2344190120762484 children [ Shape { appearance PBRAppearance { @@ -468,8 +474,8 @@ DEF sph4 Solid { boundingObject USE sph_geo4 } DEF sph5 Solid { - translation 7.917116623970895 2.104425536420231 31.708422025337523 - rotation -0.2604920627631049 0.8436140650017107 -0.46953082387497425 -2.2344190120762484 + translation -31.708422025337523 -7.917116623970895 2.104425536420231 + rotation 0.46953082387497425 0.2604920627631049 0.8436140650017107 -2.2344190120762484 children [ Shape { appearance PBRAppearance { @@ -487,11 +493,11 @@ DEF sph5 Solid { boundingObject USE sph_geo5 } DEF wall1 Wall { - translation -4.504321090318505 -1 4 - size 0.1 7 30 + translation -4 4.504321090318505 -1 + size 30 0.1 7 } DEF wall2 Wall { - translation 4.504321090318505 -1 4 + translation -4 -4.504321090318505 -1 name "wall(2)" - size 0.1 7 30 + size 30 0.1 7 } diff --git a/src/opendr/planning/end_to_end_planning/utils/obstacle_randomizer.py b/src/opendr/planning/end_to_end_planning/utils/obstacle_randomizer.py index 7db54550e0..b03e2d9d03 100644 --- a/src/opendr/planning/end_to_end_planning/utils/obstacle_randomizer.py +++ b/src/opendr/planning/end_to_end_planning/utils/obstacle_randomizer.py @@ -22,15 +22,15 @@ class ObstacleRandomizer(): def __init__(self, model_name): self.number_of_obstacles = 10 self.model_name = model_name - self.ros_srv_get_from_def = rospy.ServiceProxy("/" + self.model_name + "/supervisor/get_from_def", + self.ros_srv_get_from_def = rospy.ServiceProxy("/supervisor/get_from_def", webots_ros.srv.supervisor_get_from_def) - self.ros_srv_get_field = rospy.ServiceProxy("/" + self.model_name + "/supervisor/node/get_field", + self.ros_srv_get_field = rospy.ServiceProxy("/supervisor/node/get_field", webots_ros.srv.node_get_field) - self.ros_srv_field_set_v3 = rospy.ServiceProxy("/" + self.model_name + "/supervisor/field/set_vec3f", + self.ros_srv_field_set_v3 = rospy.ServiceProxy("/supervisor/field/set_vec3f", webots_ros.srv.field_set_vec3f) - self.ros_srv_field_set_rotation = rospy.ServiceProxy("/" + self.model_name + "/supervisor/field/set_rotation", + self.ros_srv_field_set_rotation = rospy.ServiceProxy("/supervisor/field/set_rotation", webots_ros.srv.field_set_rotation) - self.ros_srv_field_set_float = rospy.ServiceProxy("/" + self.model_name + "/supervisor/field/set_float", + self.ros_srv_field_set_float = rospy.ServiceProxy("/supervisor/field/set_float", webots_ros.srv.field_set_float) try: self.cyl_solid_nodes = [self.ros_srv_get_from_def(name="cyl" + str(i)).node for i in range(1, 6)] @@ -97,9 +97,9 @@ def randomize_cylinders(self, num=5, lower_size=1, higher_size=3): for i in range(num): t_field = self.cyl_solid_translation_fields[i] p = Point() - p.x = np.random.normal(0, 2.5) - p.z = np.random.uniform(-7, 20) - p.y = np.random.uniform(2, 3) + p.y = np.random.normal(0, 2.5) + p.x = -np.random.uniform(-7, 20) + p.z = np.random.uniform(2, 3) self.ros_srv_field_set_v3(t_field, 0, p) rot_field = self.cyl_solid_rotation_fields[i] q = Quaternion() @@ -118,16 +118,16 @@ def randomize_cylinders(self, num=5, lower_size=1, higher_size=3): for i in range(num, 5): t_field = self.cyl_solid_translation_fields[i] p = Point() - p.y = -10 + p.z = -10 self.ros_srv_field_set_v3(t_field, 0, p) def randomize_boxs(self, num=5, lower_size=0.5, higher_size=2.5): for i in range(num): t_field = self.box_solid_translation_fields[i] p = Point() - p.x = np.random.normal(0, 2.5) - p.z = np.random.uniform(-7, 20) - p.y = np.random.uniform(2, 3) + p.y = np.random.normal(0, 2.5) + p.x = -np.random.uniform(-7, 20) + p.z = np.random.uniform(2, 3) self.ros_srv_field_set_v3(t_field, 0, p) rot_field = self.box_solid_rotation_fields[i] q = Quaternion() @@ -145,16 +145,16 @@ def randomize_boxs(self, num=5, lower_size=0.5, higher_size=2.5): for i in range(num, 5): t_field = self.box_solid_translation_fields[i] p = Point() - p.y = -10 + p.z = -10 self.ros_srv_field_set_v3(t_field, 0, p) def randomize_spheres(self, num=5, lower_radius=0.5, higher_radius=1.5): for i in range(num): t_field = self.sph_solid_translation_fields[i] p = Point() - p.x = np.random.normal(0, 2.5) - p.z = np.random.uniform(-7, 20) - p.y = np.random.uniform(2, 3) + p.y = np.random.normal(0, 2.5) + p.x = -np.random.uniform(-7, 20) + p.z = np.random.uniform(2, 3) self.ros_srv_field_set_v3(t_field, 0, p) rad_field = self.sph_geometry_radius_fields[i] rad = np.random.uniform(lower_radius, higher_radius) @@ -164,7 +164,7 @@ def randomize_spheres(self, num=5, lower_radius=0.5, higher_radius=1.5): for i in range(num, 5): t_field = self.sph_solid_translation_fields[i] p = Point() - p.y = -10 + p.z = -10 self.ros_srv_field_set_v3(t_field, 0, p) def randomize_walls(self, with_walls=True, lower_width=4, higher_width=10): @@ -173,13 +173,13 @@ def randomize_walls(self, with_walls=True, lower_width=4, higher_width=10): width = np.random.uniform(lower_width, higher_width) self.keep_configuration["wall_width"] = width if with_walls: - p.y = -1 + p.z = -1 else: - p.y = -9 - p.x = -width / 2 - p.z = 4 + p.z = -9 + p.y = -width / 2 + p.x = -4 self.ros_srv_field_set_v3(field, 0, p) - p.x = width / 2 + p.y = width / 2 field = self.wall_translation_fields[1] self.ros_srv_field_set_v3(field, 0, p) diff --git a/src/opendr/simulation/human_model_generation/dependencies.ini b/src/opendr/simulation/human_model_generation/dependencies.ini index 580c774824..d6adc94e87 100644 --- a/src/opendr/simulation/human_model_generation/dependencies.ini +++ b/src/opendr/simulation/human_model_generation/dependencies.ini @@ -1,6 +1,6 @@ [runtime] # 'python' key expects a value using the Python requirements file format -# https://pip.pypa.io/en/stable/reference/pip_install/#requirements-file-format +# https://pip.pypa.io/en/stable/reference/pip_install/#requirements-file-format python=torch==1.9.0 torchvision==0.10.0 pyglet>=1.5.16 diff --git a/src/opendr/simulation/human_model_generation/utilities/PIFu/lib/mesh_util.py b/src/opendr/simulation/human_model_generation/utilities/PIFu/lib/mesh_util.py index 0ad38a66f1..0bcb968fbc 100644 --- a/src/opendr/simulation/human_model_generation/utilities/PIFu/lib/mesh_util.py +++ b/src/opendr/simulation/human_model_generation/utilities/PIFu/lib/mesh_util.py @@ -42,7 +42,7 @@ def eval_func(points): # Finally we do marching cubes try: - verts, faces, normals, values = measure.marching_cubes(sdf, 0.5) + verts, faces, normals, values = measure.marching_cubes(sdf, 0.5, method='lewiner') # transform verts into world coordinate system verts = np.matmul(mat[:3, :3], verts.T) + mat[:3, 3:4] verts = verts.T diff --git a/tests/test_license.py b/tests/test_license.py old mode 100755 new mode 100644 index 2d117b9b61..a806b9fcac --- a/tests/test_license.py +++ b/tests/test_license.py @@ -110,6 +110,16 @@ def setUp(self): 'src/opendr/perception/facial_expression_recognition/landmark_based_facial_expression_recognition/algorithm', 'src/opendr/perception/facial_expression_recognition/image_based_facial_emotion_estimation/algorithm', 'projects/python/perception/facial_expression_recognition/image_based_facial_emotion_estimation', + 'projects/opendr_ws_2/src/opendr_perception/test', + 'projects/opendr_ws_2/src/opendr_ros2_bridge/test', + 'projects/opendr_ws_2/src/vision_opencv', + 'projects/opendr_ws_2/install', + 'projects/opendr_ws_2/src/data_generation/test', + 'projects/opendr_ws_2/src/opendr_planning/test', + 'projects/opendr_ws_2/src/opendr_bridge/test', + 'projects/opendr_ws_2/src/opendr_interface/test', + 'projects/opendr_ws_2/src/opendr_data_generation/test', + 'projects/opendr_ws_2/src/opendr_simulation/test', ] skippedFilePaths = [ @@ -121,7 +131,12 @@ def setUp(self): 'src/opendr/perception/multimodal_human_centric/audiovisual_emotion_learner/algorithm/efficientface_utils.py', 'src/opendr/perception/multimodal_human_centric/audiovisual_emotion_learner/algorithm/spatial_transforms.py', 'src/opendr/perception/multimodal_human_centric/audiovisual_emotion_learner/algorithm/transformer_timm.py', - 'src/opendr/perception/multimodal_human_centric/audiovisual_emotion_learner/algorithm/utils.py' + 'src/opendr/perception/multimodal_human_centric/audiovisual_emotion_learner/algorithm/utils.py', + 'projects/opendr_ws_2/src/opendr_perception/setup.py', + 'projects/opendr_ws_2/src/opendr_planning/setup.py', + 'projects/opendr_ws_2/src/opendr_bridge/setup.py', + 'projects/opendr_ws_2/src/data_generation/setup.py', + 'projects/opendr_ws_2/src/opendr_simulation/setup.py', ] skippedDirectories = [ diff --git a/tests/test_pep8.py b/tests/test_pep8.py index e4a445a9af..a12f81b0a5 100755 --- a/tests/test_pep8.py +++ b/tests/test_pep8.py @@ -33,6 +33,7 @@ 'lib', 'src/opendr/perception/panoptic_segmentation/efficient_ps/algorithm/EfficientPS', 'projects/python/control/eagerx', + 'projects/opendr_ws_2/src/vision_opencv', 'projects/opendr_ws/devel', 'venv', 'build',