├── README.md └── cyberdog_vision ├── CMakeLists.txt ├── cmake ├── FindXMBODY.cmake ├── FindXMFACE.cmake ├── FindXMGESTURE.cmake ├── FindXMKEYPOINTS.cmake ├── FindXMREID.cmake └── FindXMTRACK.cmake ├── include └── cyberdog_vision │ ├── auto_track.hpp │ ├── body_detection.hpp │ ├── common_type.hpp │ ├── face_manager.hpp │ ├── face_recognition.hpp │ ├── gesture_recognition.hpp │ ├── keypoints_detection.hpp │ ├── person_reid.hpp │ ├── semaphore_op.hpp │ ├── shared_memory_op.hpp │ └── vision_manager.hpp ├── package.xml └── src ├── auto_track.cpp ├── body_detection.cpp ├── face_manager.cpp ├── face_recognition.cpp ├── gesture_recognition.cpp ├── keypoints_detection.cpp ├── main.cpp ├── person_reid.cpp └── vision_manager.cpp /README.md: -------------------------------------------------------------------------------- 1 | # cyberdog_vision 2 | 3 | ## Introduction 4 | 5 | Cyberdog_vision is a package based on ROS2. This package mainly used for AI algorithm scheduling management and AI algorithm inference. The AI algorithms included is as follows: 6 | 7 | - Face Recognition with facial attributes (e.g. age, emotion) 8 | - Body Detection 9 | - ReID 10 | - Static Gesture Recognition 11 | - Human Keypoints Detection 12 | - Auto Track 13 | 14 | ## Dependencies 15 | 16 | In addition to the basic pkgs of ros2, the external dependencies are as follows: 17 | 18 | - cyberdog_common 19 | - protocol 20 | 21 | ## Installation 22 | 23 | You can install this package on Linux system as follows: 24 | 25 | - create your workspace 26 | - clone the code from git 27 | - compile and install 28 | 29 | ``` 30 | colcon build --packages-up-to cyberdog_vision --install-base=/opt/ros2/cyberdog/ --merge-install 31 | ``` 32 | 33 | ## Usage 34 | 35 | The AI algorithm can be used alone in the following ways: 36 | 37 | ``` 38 | # 1. Start camera to get image stream 39 | ros2 run camera_test camera_server 40 | 41 | # 2. Start vision_manager node 42 | ros2 run cyberdog_vision vision_manager 43 | 44 | # 3. Configure node 45 | ros2 service call /vision_manager/change_state lifecycle_msgs/srv/ChangeState "{transition: {id: 1}}" 46 | 47 | # 4. Select AI algorithm you need, take body tracking as an example 48 | ros2 service call /algo_manager protocol/srv/AlgoManager "{algo_enable: ['algo_module':1,'algo_module':4]}" 49 | 50 | # 5. Activate node 51 | ros2 service call /vision_manager/change_state lifecycle_msgs/srv/ChangeState "{transition: {id: 3}}" 52 | 53 | # 6. Deactivate node 54 | ros2 service call /vision_manager/change_state lifecycle_msgs/srv/ChangeState "{transition: {id: 4}}" 55 | 56 | # 7. Cleanup node 57 | ros2 service call /vision_manager/change_state lifecycle_msgs/srv/ChangeState "{transition: {id: 2}}" 58 | ``` 59 | 60 | ## Node info 61 | 62 | /vision_manager 63 | **Subscribers:** 64 | /parameter_events: rcl_interfaces/msg/ParameterEvent 65 | **Publishers:** 66 | /facemanager/face_result: protocol/msg/FaceResult 67 | /person: protocol/msg/Person 68 | /processing_status: protocol/msg/TrackingStatus 69 | /vision_manager/transition_event: lifecycle_msgs/msg/TransitionEvent 70 | /parameter_events: rcl_interfaces/msg/ParameterEvent 71 | /rosout: rcl_interfaces/msg/Log 72 | **Service Servers:** 73 | /algo_manager: protocol/srv/AlgoManager 74 | /facemanager: protocol/srv/FaceManager 75 | /tracking_object: protocol/srv/BodyRegion 76 | /vision_manager/change_state: lifecycle_msgs/srv/ChangeState 77 | /vision_manager/describe_parameters: rcl_interfaces/srv/DescribeParameters 78 | /vision_manager/get_available_states: lifecycle_msgs/srv/GetAvailableStates 79 | /vision_manager/get_available_transitions: lifecycle_msgs/srv/GetAvailableTransitions 80 | /vision_manager/get_parameter_types: rcl_interfaces/srv/GetParameterTypes 81 | /vision_manager/get_parameters: rcl_interfaces/srv/GetParameters 82 | /vision_manager/get_state: lifecycle_msgs/srv/GetState 83 | /vision_manager/get_transition_graph: lifecycle_msgs/srv/GetAvailableTransitions 84 | /vision_manager/list_parameters: rcl_interfaces/srv/ListParameters 85 | /vision_manager/set_parameters: rcl_interfaces/srv/SetParameters 86 | /vision_manager/set_parameters_atomically: rcl_interfaces/srv/SetParametersAtomically 87 | **Service Clients:** 88 | /camera_service: protocol/srv/CameraService 89 | **Action Servers:** 90 | 91 | **Action Clients:** 92 | -------------------------------------------------------------------------------- /cyberdog_vision/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(cyberdog_vision) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | set(CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake) 9 | set(CUDA_TOOLKIT_ROOT_DIR /usr/local/cuda-10.2/) 10 | 11 | # third_party_library 12 | set(THIRD_PARTY_DIR ${CMAKE_SOURCE_DIR}) 13 | set(THIRD_PARTY_URL https://cnbj2m-fds.api.xiaomi.net/bsp-internal/ROS/carpo-camera/AI/3rdparty.tar) 14 | execute_process( 15 | COMMAND rm -rf ${CMAKE_SOURCE_DIR}/3rdparty 16 | COMMAND mkdir -p ${CMAKE_SOURCE_DIR}/3rdparty 17 | COMMAND wget -q ${THIRD_PARTY_URL} -P ${CMAKE_SOURCE_DIR}/3rdparty 18 | ) 19 | execute_process( 20 | COMMAND tar xvf ${CMAKE_SOURCE_DIR}/3rdparty/3rdparty.tar 21 | WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/3rdparty 22 | ) 23 | execute_process(COMMAND rm ${CMAKE_SOURCE_DIR}/3rdparty/3rdparty.tar) 24 | add_subdirectory(3rdparty) 25 | 26 | # find dependencies 27 | # uncomment the following section in order to fill in 28 | # further dependencies manually. 29 | # find_package( REQUIRED) 30 | find_package(ament_cmake REQUIRED) 31 | find_package(rclcpp REQUIRED) 32 | find_package(rclcpp_lifecycle REQUIRED) 33 | find_package(cyberdog_common REQUIRED) 34 | find_package(protocol REQUIRED) 35 | find_package(OpenCV 4 REQUIRED) 36 | find_package(CUDA REQUIRED) 37 | find_package(XMBODY REQUIRED) 38 | find_package(XMFACE REQUIRED) 39 | find_package(XMGESTURE REQUIRED) 40 | find_package(XMKEYPOINTS REQUIRED) 41 | find_package(XMREID REQUIRED) 42 | find_package(XMTRACK REQUIRED) 43 | 44 | include_directories(${CMAKE_SOURCE_DIR}/include) 45 | 46 | if(BUILD_TESTING) 47 | find_package(ament_lint_auto REQUIRED) 48 | # the following line skips the linter which checks for copyrights 49 | # uncomment the line when a copyright and license is not present in all source files 50 | #set(ament_cmake_copyright_FOUND TRUE) 51 | # the following line skips cpplint (only works in a git repo) 52 | # uncomment the line when this package is not in a git repo 53 | #set(ament_cmake_cpplint_FOUND TRUE) 54 | ament_lint_auto_find_test_dependencies() 55 | endif() 56 | 57 | add_executable(vision_manager 58 | src/main.cpp 59 | src/vision_manager.cpp 60 | src/body_detection.cpp 61 | src/face_recognition.cpp 62 | src/gesture_recognition.cpp 63 | src/keypoints_detection.cpp 64 | src/person_reid.cpp 65 | src/auto_track.cpp 66 | src/face_manager.cpp 67 | ) 68 | 69 | ament_target_dependencies(vision_manager 70 | XMBODY 71 | XMFACE 72 | XMGESTURE 73 | XMKEYPOINTS 74 | XMREID 75 | XMTRACK 76 | rclcpp 77 | rclcpp_lifecycle 78 | cyberdog_common 79 | protocol 80 | OpenCV 81 | CUDA 82 | ) 83 | 84 | install(TARGETS 85 | vision_manager 86 | DESTINATION lib/${PROJECT_NAME} 87 | ) 88 | 89 | ament_package() 90 | -------------------------------------------------------------------------------- /cyberdog_vision/cmake/FindXMBODY.cmake: -------------------------------------------------------------------------------- 1 | # Try to find libXMBODY 2 | # Once done this will define 3 | # XMBODY_FOUND - system has libXMBODY 4 | # XMBODY_INCLUDE_DIRS - the libXMBODY include directory 5 | # XMBODY_LIBRARIES - libXMBODY library 6 | 7 | include(FindPackageHandleStandardArgs) 8 | 9 | find_path(XmBody_INCLUDE_DIR ContentMotionAPI.h ${CMAKE_SOURCE_DIR}/3rdparty/body_gesture/include) 10 | find_library(XmBody_LIBRARIE libContentMotionAPI.so ${CMAKE_SOURCE_DIR}/3rdparty/body_gesture/lib) 11 | 12 | find_package_handle_standard_args(XMBODY DEFAULT_MSG XmBody_INCLUDE_DIR XmBody_LIBRARIE) 13 | 14 | if(XMBODY_FOUND) 15 | set(XMBODY_INCLUDE_DIRS ${XmBody_INCLUDE_DIR}) 16 | set(XMBODY_LIBRARIES ${XmBody_LIBRARIE}) 17 | elseif(XMBODY_FOUND) 18 | message(FATAL_ERROR "Not found XMBODY") 19 | endif() 20 | 21 | mark_as_advanced( 22 | XMBODY_INCLUDE_DIRS 23 | XMBODY_LIBRARIES 24 | ) -------------------------------------------------------------------------------- /cyberdog_vision/cmake/FindXMFACE.cmake: -------------------------------------------------------------------------------- 1 | # Try to find libXMFACE 2 | # Once done this will define 3 | # XMFACE_FOUND - system has libXMFACE 4 | # XMFACE_INCLUDE_DIRS - the libXMFACE include directory 5 | # XMFACE_LIBRARIES - libXMFACE library 6 | 7 | include(FindPackageHandleStandardArgs) 8 | 9 | find_path(XmFace_INCLUDE_DIR XMFaceAPI.h ${CMAKE_SOURCE_DIR}/3rdparty/face_recognition/include) 10 | find_library(XmFace_LIBRARIE libXMFaceAPI.so ${CMAKE_SOURCE_DIR}/3rdparty/face_recognition/lib) 11 | 12 | find_package_handle_standard_args(XMFACE REQUIRED_VARS XmFace_INCLUDE_DIR XmFace_LIBRARIE) 13 | 14 | if(XMFACE_FOUND) 15 | set(XMFACE_INCLUDE_DIRS ${XmFace_INCLUDE_DIR}) 16 | set(XMFACE_LIBRARIES ${XmFace_LIBRARIE}) 17 | elseif(XMFACE_FOUND) 18 | message(FATAL_ERROR "Not found XMFACE") 19 | endif() 20 | 21 | mark_as_advanced( 22 | XMFACE_INCLUDE_DIRS 23 | XMFACE_LIBRARIES 24 | ) -------------------------------------------------------------------------------- /cyberdog_vision/cmake/FindXMGESTURE.cmake: -------------------------------------------------------------------------------- 1 | # Try to find libXMGESTURE 2 | # Once done this will define 3 | # XMGESTURE_FOUND - system has libXMGESTURE 4 | # XMGESTURE_INCLUDE_DIRS - the libXMGESTURE include directory 5 | # XMGESTURE_LIBRARIES - libXMGESTURE library 6 | 7 | include(FindPackageHandleStandardArgs) 8 | 9 | find_path(XmGesture_INCLUDE_DIR hand_gesture.h ${CMAKE_SOURCE_DIR}/3rdparty/body_gesture/include) 10 | find_library(XmGesture_LIBRARIE libhand_gesture.so ${CMAKE_SOURCE_DIR}/3rdparty/body_gesture/lib) 11 | 12 | find_package_handle_standard_args(XMGESTURE REQUIRED_VARS XmGesture_INCLUDE_DIR XmGesture_LIBRARIE) 13 | 14 | if(XMGESTURE_FOUND) 15 | set(XMGESTURE_INCLUDE_DIRS ${XmGesture_INCLUDE_DIR}) 16 | set(XMGESTURE_LIBRARIES ${XmGesture_LIBRARIE}) 17 | elseif(XMGESTURE_FOUND) 18 | message(FATAL_ERROR "Not found XMGESTURE") 19 | endif() 20 | 21 | mark_as_advanced( 22 | XMGESTURE_INCLUDE_DIRS 23 | XMGESTURE_LIBRARIES 24 | ) -------------------------------------------------------------------------------- /cyberdog_vision/cmake/FindXMKEYPOINTS.cmake: -------------------------------------------------------------------------------- 1 | # Try to find libXMKEYPOINTS 2 | # Once done this will define 3 | # XMKEYPOINTS_FOUND - system has libXMKEYPOINTS 4 | # XMKEYPOINTS_INCLUDE_DIRS - the libXMKEYPOINTS include directory 5 | # XMKEYPOINTS_LIBRARIES - libXMKEYPOINTS library 6 | 7 | include(FindPackageHandleStandardArgs) 8 | 9 | find_path(XmKeypoints_INCLUDE_DIR person_keypoints.h ${CMAKE_SOURCE_DIR}/3rdparty/keypoints_detection/include) 10 | find_library(XmKeypoints_LIBRARIE libperson_keypoints.so ${CMAKE_SOURCE_DIR}/3rdparty/keypoints_detection/lib) 11 | 12 | find_package_handle_standard_args(XMKEYPOINTS REQUIRED_VARS XmKeypoints_INCLUDE_DIR XmKeypoints_LIBRARIE) 13 | 14 | if(XMKEYPOINTS_FOUND) 15 | set(XMKEYPOINTS_INCLUDE_DIRS ${XmKeypoints_INCLUDE_DIR}) 16 | set(XMKEYPOINTS_LIBRARIES ${XmKeypoints_LIBRARIE}) 17 | elseif(XMKEYPOINTS_FOUND) 18 | message(FATAL_ERROR "Not found XMKEYPOINTS") 19 | endif() 20 | 21 | mark_as_advanced( 22 | XMKEYPOINTS_INCLUDE_DIRS 23 | XMKEYPOINTS_LIBRARIES 24 | ) -------------------------------------------------------------------------------- /cyberdog_vision/cmake/FindXMREID.cmake: -------------------------------------------------------------------------------- 1 | # Try to find libXMREID 2 | # Once done this will define 3 | # XMREID_FOUND - system has libXMREID 4 | # XMREID_INCLUDE_DIRS - the libXMREID include directory 5 | # XMREID_LIBRARIES - libXMREID library 6 | 7 | include(FindPackageHandleStandardArgs) 8 | 9 | find_path(XmReID_INCLUDE_DIR ReIDToolAPI.h ${CMAKE_SOURCE_DIR}/3rdparty/person_reid/include) 10 | find_library(XmReID_LIBRARIE libReIDTools.so ${CMAKE_SOURCE_DIR}/3rdparty/person_reid/lib) 11 | 12 | find_package_handle_standard_args(XMREID REQUIRED_VARS XmReID_INCLUDE_DIR XmReID_LIBRARIE) 13 | 14 | if(XMREID_FOUND) 15 | set(XMREID_INCLUDE_DIRS ${XmReID_INCLUDE_DIR}) 16 | set(XMREID_LIBRARIES ${XmReID_LIBRARIE}) 17 | elseif(XMREID_FOUND) 18 | message(FATAL_ERROR "Not found XMREID") 19 | endif() 20 | 21 | mark_as_advanced( 22 | XMREID_INCLUDE_DIRS 23 | XMREID_LIBRARIES 24 | ) -------------------------------------------------------------------------------- /cyberdog_vision/cmake/FindXMTRACK.cmake: -------------------------------------------------------------------------------- 1 | # Try to find libXMTRACK 2 | # Once done this will define 3 | # XMTRACK_FOUND - system has libXMTRACK 4 | # XMTRACK_INCLUDE_DIRS - the libXMTRACK include directory 5 | # XMTRACK_LIBRARIES - libXMTRACK library 6 | 7 | include(FindPackageHandleStandardArgs) 8 | 9 | find_path(XmTrack_INCLUDE_DIR tracker.hpp ${CMAKE_SOURCE_DIR}/3rdparty/auto_track/include) 10 | find_library(XmTrack_LIBRARIE libtracker.so ${CMAKE_SOURCE_DIR}/3rdparty/auto_track/lib) 11 | 12 | find_package_handle_standard_args(XMTRACK REQUIRED_VARS XmTrack_INCLUDE_DIR XmTrack_LIBRARIE) 13 | 14 | if(XMTRACK_FOUND) 15 | set(XMTRACK_INCLUDE_DIRS ${XmTrack_INCLUDE_DIR}) 16 | set(XMTRACK_LIBRARIES ${XmTrack_LIBRARIE}) 17 | elseif(XMTRACK_FOUND) 18 | message(FATAL_ERROR "Not found XMTRACK") 19 | endif() 20 | 21 | mark_as_advanced( 22 | XMTRACK_INCLUDE_DIRS 23 | XMTRACK_LIBRARIES 24 | ) -------------------------------------------------------------------------------- /cyberdog_vision/include/cyberdog_vision/auto_track.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef CYBERDOG_VISION__AUTO_TRACK_HPP_ 16 | #define CYBERDOG_VISION__AUTO_TRACK_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | #include "tracker.hpp" 22 | 23 | namespace cyberdog_vision 24 | { 25 | 26 | class AutoTrack 27 | { 28 | public: 29 | explicit AutoTrack(const std::string & model_path); 30 | ~AutoTrack(); 31 | 32 | bool SetTracker(const cv::Mat & img, const cv::Rect & bbox); 33 | bool Track(const cv::Mat & img, cv::Rect & bbox); 34 | void SetLossTh(int loss_th); 35 | void ResetTracker(); 36 | bool GetLostStatus(); 37 | 38 | private: 39 | std::shared_ptr tracker_ptr_; 40 | 41 | int gpu_id_; 42 | int loss_th_; 43 | int fail_count_; 44 | bool is_init_; 45 | bool is_lost_; 46 | }; 47 | 48 | } // namespace cyberdog_vision 49 | 50 | #endif // CYBERDOG_VISION__AUTO_TRACK_HPP_ 51 | -------------------------------------------------------------------------------- /cyberdog_vision/include/cyberdog_vision/body_detection.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef CYBERDOG_VISION__BODY_DETECTION_HPP_ 16 | #define CYBERDOG_VISION__BODY_DETECTION_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | #include "ContentMotionAPI.h" 22 | #include "common_type.hpp" 23 | 24 | namespace cyberdog_vision 25 | { 26 | 27 | class BodyDetection 28 | { 29 | public: 30 | explicit BodyDetection(const std::string & model_path); 31 | ~BodyDetection(); 32 | 33 | int Detect(const cv::Mat & img, BodyFrameInfo & infos); 34 | 35 | private: 36 | std::shared_ptr body_ptr_; 37 | int gpu_id_; 38 | }; 39 | 40 | } // namespace cyberdog_vision 41 | 42 | #endif // CYBERDOG_VISION__BODY_DETECTION_HPP_ 43 | -------------------------------------------------------------------------------- /cyberdog_vision/include/cyberdog_vision/common_type.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef CYBERDOG_VISION__COMMON_TYPE_HPP_ 16 | #define CYBERDOG_VISION__COMMON_TYPE_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include "opencv2/opencv.hpp" 24 | 25 | #include "std_msgs/msg/header.hpp" 26 | 27 | #include "XMFaceAPI.h" 28 | #include "ContentMotionAPI.h" 29 | 30 | namespace cyberdog_vision 31 | { 32 | 33 | using BodyFrameInfo = std::vector; 34 | 35 | struct InferBbox 36 | { 37 | cv::Rect body_box; 38 | float score; 39 | InferBbox() 40 | { 41 | body_box = cv::Rect(0, 0, 0, 0); 42 | score = 0.f; 43 | } 44 | }; 45 | 46 | struct GestureInfo 47 | { 48 | cv::Rect rect; 49 | int label; 50 | GestureInfo() 51 | { 52 | rect = cv::Rect(0, 0, 0, 0); 53 | label = 9; 54 | } 55 | }; 56 | 57 | struct StampedImage 58 | { 59 | std_msgs::msg::Header header; 60 | cv::Mat img; 61 | }; 62 | 63 | struct GlobalImageBuf 64 | { 65 | bool is_filled; 66 | std::mutex mtx; 67 | std::condition_variable cond; 68 | std::vector img_buf; 69 | GlobalImageBuf() 70 | { 71 | is_filled = false; 72 | } 73 | }; 74 | 75 | struct BodyResults 76 | { 77 | bool is_filled; 78 | std::mutex mtx; 79 | std::condition_variable cond; 80 | StampedImage detection_img; 81 | std::vector body_infos; 82 | BodyResults() 83 | { 84 | is_filled = false; 85 | } 86 | }; 87 | 88 | struct AlgoStruct 89 | { 90 | bool is_called; 91 | std::mutex mtx; 92 | std::condition_variable cond; 93 | AlgoStruct() 94 | { 95 | is_called = false; 96 | } 97 | }; 98 | 99 | struct AlgoProcess 100 | { 101 | bool process_complated; 102 | std::mutex mtx; 103 | std::condition_variable cond; 104 | AlgoProcess() 105 | { 106 | process_complated = false; 107 | } 108 | }; 109 | 110 | inline void ImgConvert(const cv::Mat & img, XMImage & xm_img) 111 | { 112 | xm_img.data = img.data; 113 | xm_img.width = img.cols; 114 | xm_img.height = img.rows; 115 | xm_img.channel = img.channels(); 116 | xm_img.type = ColorType::BGR; 117 | } 118 | 119 | inline std::vector BodyConvert(BodyFrameInfo & infos) 120 | { 121 | std::vector infer_bboxes; 122 | for (auto & info : infos) { 123 | InferBbox box; 124 | box.body_box = cv::Rect(info.left, info.top, info.width, info.height); 125 | box.score = info.score; 126 | infer_bboxes.push_back(box); 127 | } 128 | return infer_bboxes; 129 | } 130 | 131 | } // namespace cyberdog_vision 132 | 133 | #endif // CYBERDOG_VISION__COMMON_TYPE_HPP_ 134 | -------------------------------------------------------------------------------- /cyberdog_vision/include/cyberdog_vision/face_manager.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef CYBERDOG_VISION__FACE_MANAGER_HPP_ 16 | #define CYBERDOG_VISION__FACE_MANAGER_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include "cyberdog_vision/face_recognition.hpp" 22 | 23 | namespace cyberdog_vision 24 | { 25 | 26 | struct FaceId 27 | { 28 | std::string name; 29 | bool is_host; 30 | }; 31 | 32 | template 33 | struct SizedVector 34 | { 35 | public: 36 | SizedVector() 37 | { 38 | m_index = 0; 39 | } 40 | 41 | void push_back(const T & obj) 42 | { 43 | if (m_vector.size() >= COUNT) { 44 | m_index %= COUNT; 45 | m_vector[m_index] = obj; 46 | m_index++; 47 | } else { 48 | m_vector.push_back(obj); 49 | } 50 | } 51 | 52 | size_t size() 53 | { 54 | return m_vector.size(); 55 | } 56 | 57 | std::vector & vector() 58 | { 59 | return m_vector; 60 | } 61 | 62 | void clear() 63 | { 64 | m_vector.clear(); 65 | m_index = 0; 66 | } 67 | 68 | bool full() 69 | { 70 | return m_vector.size() == COUNT; 71 | } 72 | 73 | private: 74 | std::vector m_vector; 75 | size_t m_index; 76 | }; 77 | 78 | 79 | class FaceManager 80 | { 81 | public: 82 | static FaceManager * getInstance(); 83 | static const std::string getFaceDataPath(); 84 | std::map> & getFeatures(); 85 | int addFaceIDCacheInfo(std::string & name, bool is_host); 86 | int addFaceFeatureCacheInfo(std::vector & faceinfo); 87 | int cancelAddFace(); 88 | int checkFacePose(std::vector & faceinfo, std::string & msg); 89 | int confirmFace(std::string & name, bool is_host); 90 | int updateFaceId(std::string & ori_name, std::string & new_name); 91 | bool updateFeaturesFile(); 92 | int deleteFace(std::string & face_name); 93 | std::string getAllFaces(); 94 | bool findFace(const std::string & face_name); 95 | bool isHost(const std::string & face_name); 96 | 97 | private: 98 | FaceManager(); 99 | ~FaceManager(); 100 | void initialize(); 101 | bool loadFeatures(); 102 | 103 | enum FaceStatsType 104 | { 105 | statsFaceNum = 0, 106 | statsFaceYaw, 107 | statsFacePitch, 108 | statsFaceRow, 109 | statsFaceArea, 110 | statsFaceTypeMax, 111 | }; 112 | 113 | std::map> m_features; 114 | std::map m_hostMap; 115 | std::mutex m_mutex; 116 | 117 | bool m_inFaceAdding; 118 | /* save last face info, wait for user confirm */ 119 | FaceId m_faceIdCached; 120 | std::vector m_faceFeatsCached; 121 | 122 | /**/ 123 | SizedVector m_faceStats[statsFaceTypeMax]; 124 | }; 125 | 126 | } // namespace cyberdog_vision 127 | 128 | #endif // CYBERDOG_VISION__FACE_MANAGER_HPP_ 129 | -------------------------------------------------------------------------------- /cyberdog_vision/include/cyberdog_vision/face_recognition.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef CYBERDOG_VISION__FACE_RECOGNITION_HPP_ 16 | #define CYBERDOG_VISION__FACE_RECOGNITION_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include "XMFaceAPI.h" 24 | #include "common_type.hpp" 25 | 26 | namespace cyberdog_vision 27 | { 28 | 29 | class FaceRecognition 30 | { 31 | public: 32 | FaceRecognition( 33 | const std::string & model_path, bool open_emotion, bool open_age); 34 | ~FaceRecognition(); 35 | 36 | int GetFaceInfo(const cv::Mat & img, std::vector & faces_info); 37 | int GetRecognitionResult( 38 | const cv::Mat & img, const std::map> & endlib_feats, 39 | std::vector & faces_info); 40 | 41 | private: 42 | void FillParam(const std::string & model_path, FaceParam & param); 43 | XMFaceAPI * face_ptr_; 44 | }; 45 | 46 | } // namespace cyberdog_vision 47 | 48 | #endif // CYBERDOG_VISION__FACE_RECOGNITION_HPP_ 49 | -------------------------------------------------------------------------------- /cyberdog_vision/include/cyberdog_vision/gesture_recognition.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef CYBERDOG_VISION__GESTURE_RECOGNITION_HPP_ 16 | #define CYBERDOG_VISION__GESTURE_RECOGNITION_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include "hand_gesture.h" // NOLINT 23 | #include "common_type.hpp" 24 | 25 | namespace cyberdog_vision 26 | { 27 | 28 | class GestureRecognition 29 | { 30 | public: 31 | explicit GestureRecognition(const std::string & model_path); 32 | ~GestureRecognition(); 33 | 34 | int GetGestureInfo( 35 | const cv::Mat & img, const std::vector & body_boxes, 36 | std::vector & infos); 37 | 38 | void SetRecognitionNum(int num); 39 | 40 | private: 41 | std::shared_ptr gesture_ptr_; 42 | int max_person_num_; 43 | }; 44 | 45 | } // namespace cyberdog_vision 46 | 47 | #endif // CYBERDOG_VISION__GESTURE_RECOGNITION_HPP_ 48 | -------------------------------------------------------------------------------- /cyberdog_vision/include/cyberdog_vision/keypoints_detection.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef CYBERDOG_VISION__KEYPOINTS_DETECTION_HPP_ 16 | #define CYBERDOG_VISION__KEYPOINTS_DETECTION_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include "person_keypoints.h" // NOLINT 23 | #include "common_type.hpp" 24 | 25 | namespace cyberdog_vision 26 | { 27 | 28 | class KeypointsDetection 29 | { 30 | public: 31 | explicit KeypointsDetection(const std::string & model_path); 32 | ~KeypointsDetection(); 33 | 34 | void GetKeypointsInfo( 35 | const cv::Mat & img, const std::vector & body_boxes, 36 | std::vector> & bodies_keypoints); 37 | 38 | private: 39 | std::shared_ptr keypoints_ptr_; 40 | }; 41 | 42 | } // namespace cyberdog_vision 43 | 44 | #endif // CYBERDOG_VISION__KEYPOINTS_DETECTION_HPP_ 45 | -------------------------------------------------------------------------------- /cyberdog_vision/include/cyberdog_vision/person_reid.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef CYBERDOG_VISION__PERSON_REID_HPP_ 16 | #define CYBERDOG_VISION__PERSON_REID_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | #include "ReIDToolAPI.h" 22 | #include "common_type.hpp" 23 | 24 | namespace cyberdog_vision 25 | { 26 | 27 | enum SimType 28 | { 29 | kSimOne2One = 0, 30 | kSimOne2Group, 31 | kSimGroup2Group 32 | }; 33 | 34 | class PersonReID 35 | { 36 | public: 37 | explicit PersonReID(const std::string & model_path); 38 | ~PersonReID(); 39 | 40 | int SetTracker(const cv::Mat & img, const cv::Rect & body_box, std::vector & reid_feat); 41 | int GetReIDInfo( 42 | const cv::Mat & img, const std::vector & body_bboxes, int & id, 43 | cv::Rect & tracked); 44 | int GetFeatureLen(); 45 | void ResetTracker(); 46 | bool GetLostStatus(); 47 | 48 | private: 49 | int GetFeature(const cv::Mat & img, const cv::Rect & body_box, std::vector & reid_feat); 50 | float GetSim( 51 | std::vector & feat_det, 52 | std::vector & feat_library, const SimType & sim_type); 53 | 54 | int gpu_id_; 55 | int tracking_id_; 56 | int object_loss_th_; 57 | int library_frame_num_; 58 | int unmatch_count_; 59 | float feat_sim_th_; 60 | float feat_update_th_; 61 | bool is_tracking_; 62 | bool is_lost_; 63 | 64 | void * reid_ptr_; 65 | std::vector tracker_feat_; 66 | }; 67 | 68 | } // namespace cyberdog_vision 69 | 70 | #endif // CYBERDOG_VISION__PERSON_REID_HPP_ 71 | -------------------------------------------------------------------------------- /cyberdog_vision/include/cyberdog_vision/semaphore_op.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef CYBERDOG_VISION__SEMAPHORE_OP_HPP_ 16 | #define CYBERDOG_VISION__SEMAPHORE_OP_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #define IPCKEY_PATH "/" 24 | 25 | static struct timespec ts = {10, 0}; 26 | namespace cyberdog_vision 27 | { 28 | 29 | union semun { 30 | int val; // value for SETVAL 31 | struct semid_ds * buf; // buffer for IPC_STAT&IPC_SET 32 | unsigned short * array; // NOLINT, array for GETALL&SETALL 33 | struct seminfo * __buf; // buffer for IPC_INFO 34 | }; 35 | 36 | inline int CreateSem(unsigned char proj_id, int sem_num, int & sem_set_id) 37 | { 38 | // convert to a system ipc key 39 | key_t key = ftok(IPCKEY_PATH, proj_id); 40 | if (key < 0) { 41 | std::cout << "Convert to ipc key fail when create semaphore. " << std::endl; 42 | return -1; 43 | } 44 | 45 | // get the semaphore set id associated with a key 46 | sem_set_id = semget(key, sem_num, IPC_CREAT | 0666); 47 | if (sem_set_id < 0) { 48 | std::cout << "Get the semaphore id fail. " << std::endl; 49 | return -1; 50 | } 51 | std::cout << "Semaphore set id is: " << sem_set_id << std::endl; 52 | 53 | return 0; 54 | } 55 | 56 | inline int SetSemInitVal(int sem_set_id, int sem_index, int init_val) 57 | { 58 | // set the initial value of the semaphore 59 | union semun sem_union; 60 | sem_union.val = init_val; 61 | if ((semctl(sem_set_id, sem_index, SETVAL, sem_union)) < 0) { 62 | std::cout << "Set the initial value fail, semaphore index: " << sem_index << std::endl; 63 | return -1; 64 | } 65 | 66 | return 0; 67 | } 68 | 69 | inline int GetSemVal(int sem_set_id, int sem_index) 70 | { 71 | return semctl(sem_set_id, sem_index, GETVAL, 0); 72 | } 73 | 74 | inline int WaitSem(int sem_set_id, int sem_index) 75 | { 76 | // std::cout << "Sem_P" << std::endl; 77 | struct sembuf sem_buf; 78 | sem_buf.sem_num = sem_index; 79 | sem_buf.sem_op = -1; 80 | sem_buf.sem_flg = 0; 81 | 82 | if (semtimedop(sem_set_id, &sem_buf, 1, &ts) < 0) { 83 | std::cout << "Semaphore P operation fail, error code: " << errno << std::endl; 84 | return -1; 85 | } 86 | 87 | return 0; 88 | } 89 | 90 | inline int SignalSem(int sem_set_id, int sem_index) 91 | { 92 | // std::cout << "Sem_V" << std::endl; 93 | struct sembuf sem_buf; 94 | sem_buf.sem_num = sem_index; 95 | sem_buf.sem_op = 1; 96 | sem_buf.sem_flg = 0; 97 | 98 | if (semtimedop(sem_set_id, &sem_buf, 1, &ts) < 0) { 99 | std::cout << "Semaphore V operation fail, error code: " << errno << std::endl; 100 | return -1; 101 | } 102 | 103 | return 0; 104 | } 105 | 106 | inline int DelSem(int sem_set_id) 107 | { 108 | union semun sem_union; 109 | if (semctl(sem_set_id, 0, IPC_RMID, sem_union) < 0) { 110 | std::cout << "Del semaphore fail. " << std::endl; 111 | return -1; 112 | } 113 | std::cout << "Semaphore is deleted. " << std::endl; 114 | 115 | return 0; 116 | } 117 | 118 | } // namespace cyberdog_vision 119 | 120 | #endif // CYBERDOG_VISION__SEMAPHORE_OP_HPP_ 121 | -------------------------------------------------------------------------------- /cyberdog_vision/include/cyberdog_vision/shared_memory_op.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef CYBERDOG_VISION__SHARED_MEMORY_OP_HPP_ 16 | #define CYBERDOG_VISION__SHARED_MEMORY_OP_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | 23 | #define IPCKEY_PATH "/" 24 | #define IMAGE_SIZE 640 * 480 * 3 25 | 26 | namespace cyberdog_vision 27 | { 28 | 29 | struct SharedImage 30 | { 31 | double img_stamp; 32 | unsigned char img_data[IMAGE_SIZE]; 33 | }; 34 | 35 | inline int CreateShm(unsigned char proj_id, size_t size, int & shm_id) 36 | { 37 | key_t key = ftok(IPCKEY_PATH, proj_id); 38 | if (key < 0) { 39 | std::cout << "Convert to ipc key fail when create shared memory. " << std::endl; 40 | return -1; 41 | } 42 | 43 | shm_id = shmget(key, size, IPC_CREAT | 0666); 44 | if (shm_id < 0) { 45 | std::cout << "Get the shared memory id fail. " << std::endl; 46 | return -1; 47 | } 48 | 49 | return 0; 50 | } 51 | 52 | inline char * GetShmAddr(int shm_id, size_t size) 53 | { 54 | char * addr = reinterpret_cast(shmat(shm_id, NULL, 0)); 55 | if (addr == reinterpret_cast(-1)) { 56 | std::cout << "Get the shared memory addr fail. " << std::endl; 57 | return nullptr; 58 | } 59 | memset(addr, 0, size); 60 | 61 | return addr; 62 | } 63 | 64 | inline int DelShm(int shm_id) 65 | { 66 | if (shmctl(shm_id, IPC_RMID, NULL) < 0) { 67 | std::cout << "Remove the shared memory fail. " << std::endl; 68 | return -1; 69 | } 70 | 71 | return 0; 72 | } 73 | 74 | inline int DetachShm(char * shm_addr) 75 | { 76 | if (shmdt(shm_addr) < 0) { 77 | std::cout << "Detach the shared memory fail. " << std::endl; 78 | return -1; 79 | } 80 | 81 | return 0; 82 | } 83 | 84 | } // namespace cyberdog_vision 85 | 86 | #endif // CYBERDOG_VISION__SHARED_MEMORY_OP_HPP_ 87 | -------------------------------------------------------------------------------- /cyberdog_vision/include/cyberdog_vision/vision_manager.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef CYBERDOG_VISION__VISION_MANAGER_HPP_ 16 | #define CYBERDOG_VISION__VISION_MANAGER_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include "opencv2/opencv.hpp" 24 | 25 | #include "rclcpp/rclcpp.hpp" 26 | #include "protocol/msg/body.hpp" 27 | #include "protocol/msg/face.hpp" 28 | #include "protocol/msg/body_info.hpp" 29 | #include "protocol/msg/face_info.hpp" 30 | #include "protocol/msg/track_result.hpp" 31 | #include "protocol/msg/algo_list.hpp" 32 | #include "protocol/msg/person.hpp" 33 | #include "protocol/msg/tracking_status.hpp" 34 | #include "protocol/msg/face_result.hpp" 35 | #include "protocol/srv/body_region.hpp" 36 | #include "protocol/srv/camera_service.hpp" 37 | #include "protocol/srv/algo_manager.hpp" 38 | #include "protocol/srv/face_manager.hpp" 39 | #include "protocol/msg/connector_status.hpp" 40 | #include "rclcpp_lifecycle/lifecycle_node.hpp" 41 | #include "rclcpp_lifecycle/lifecycle_publisher.hpp" 42 | 43 | #include "cyberdog_vision/shared_memory_op.hpp" 44 | #include "cyberdog_vision/body_detection.hpp" 45 | #include "cyberdog_vision/face_recognition.hpp" 46 | #include "cyberdog_vision/gesture_recognition.hpp" 47 | #include "cyberdog_vision/keypoints_detection.hpp" 48 | #include "cyberdog_vision/person_reid.hpp" 49 | #include "cyberdog_vision/auto_track.hpp" 50 | #include "cyberdog_vision/face_manager.hpp" 51 | 52 | namespace cyberdog_vision 53 | { 54 | 55 | using BodyT = protocol::msg::Body; 56 | using BodyInfoT = protocol::msg::BodyInfo; 57 | using FaceT = protocol::msg::Face; 58 | using FaceInfoT = protocol::msg::FaceInfo; 59 | using KeypointT = protocol::msg::Keypoint; 60 | using AlgoListT = protocol::msg::AlgoList; 61 | using PersonInfoT = protocol::msg::Person; 62 | using TrackResultT = protocol::msg::TrackResult; 63 | using BodyRegionT = protocol::srv::BodyRegion; 64 | using CameraServiceT = protocol::srv::CameraService; 65 | using AlgoManagerT = protocol::srv::AlgoManager; 66 | using FaceManagerT = protocol::srv::FaceManager; 67 | using FaceResultT = protocol::msg::FaceResult; 68 | using TrackingStatusT = protocol::msg::TrackingStatus; 69 | using ReturnResultT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; 70 | 71 | class VisionManager : public rclcpp_lifecycle::LifecycleNode 72 | { 73 | public: 74 | VisionManager(); 75 | ~VisionManager(); 76 | 77 | protected: 78 | ReturnResultT on_configure(const rclcpp_lifecycle::State & state) override; 79 | ReturnResultT on_activate(const rclcpp_lifecycle::State & state) override; 80 | ReturnResultT on_deactivate(const rclcpp_lifecycle::State & state) override; 81 | ReturnResultT on_cleanup(const rclcpp_lifecycle::State & state) override; 82 | ReturnResultT on_shutdown(const rclcpp_lifecycle::State & state) override; 83 | 84 | private: 85 | int Init(); 86 | int InitIPC(); 87 | void CreateObjectAI(); 88 | void CreateObjectROS(); 89 | void CreateThread(); 90 | void ImageProc(); 91 | void MainAlgoManager(); 92 | void DependAlgoManager(); 93 | void BodyDet(); 94 | void FaceRecognize(); 95 | void FocusTrack(); 96 | void ReIDProc(); 97 | void GestureRecognize(); 98 | void KeypointsDet(); 99 | 100 | int LoadFaceLibrary(std::map> & library); 101 | int GetMatchBody(const sensor_msgs::msg::RegionOfInterest & roi); 102 | void SetAlgoState(const AlgoListT & algo_list, const bool & value); 103 | 104 | void TrackingService( 105 | const std::shared_ptr, 106 | const std::shared_ptr req, 107 | std::shared_ptr res); 108 | 109 | void AlgoManagerService( 110 | const std::shared_ptr, 111 | const std::shared_ptr req, 112 | std::shared_ptr res); 113 | 114 | bool CallService( 115 | rclcpp::Client::SharedPtr & client, const uint8_t & cmd, 116 | const std::string & args); 117 | 118 | void FaceManagerService( 119 | const std::shared_ptr, 120 | const std::shared_ptr req, 121 | std::shared_ptr res); 122 | 123 | void publishFaceResult( 124 | int result, std::string & face_msg); 125 | 126 | void FaceDetProc(std::string); 127 | 128 | void SetThreadState(const std::string & thread_flag, bool & state); 129 | void WakeThread(AlgoStruct & algo); 130 | void ResetThread(AlgoStruct & algo); 131 | void ResetAlgo(); 132 | void ResetCudaDevs(); 133 | void DestoryThread(); 134 | 135 | private: 136 | rclcpp::Service::SharedPtr tracking_service_; 137 | rclcpp::Service::SharedPtr algo_manager_service_; 138 | rclcpp::Service::SharedPtr facemanager_service_; 139 | rclcpp::Client::SharedPtr camera_clinet_; 140 | 141 | rclcpp_lifecycle::LifecyclePublisher::SharedPtr person_pub_; 142 | rclcpp_lifecycle::LifecyclePublisher::SharedPtr status_pub_; 143 | rclcpp_lifecycle::LifecyclePublisher::SharedPtr face_result_pub_; 144 | 145 | std::shared_ptr img_proc_thread_; 146 | std::shared_ptr main_manager_thread_; 147 | std::shared_ptr depend_manager_thread_; 148 | std::shared_ptr body_det_thread_; 149 | std::shared_ptr face_thread_; 150 | std::shared_ptr focus_thread_; 151 | std::shared_ptr gesture_thread_; 152 | std::shared_ptr reid_thread_; 153 | std::shared_ptr keypoints_thread_; 154 | 155 | std::shared_ptr body_ptr_; 156 | std::shared_ptr face_ptr_; 157 | std::shared_ptr focus_ptr_; 158 | std::shared_ptr gesture_ptr_; 159 | std::shared_ptr reid_ptr_; 160 | std::shared_ptr keypoints_ptr_; 161 | 162 | std::map> face_library_; 163 | 164 | GlobalImageBuf global_img_buf_; 165 | BodyResults body_results_; 166 | 167 | AlgoStruct face_struct_; 168 | AlgoStruct body_struct_; 169 | AlgoStruct gesture_struct_; 170 | AlgoStruct keypoints_struct_; 171 | AlgoStruct reid_struct_; 172 | AlgoStruct focus_struct_; 173 | AlgoProcess algo_proc_; 174 | 175 | std::mutex result_mtx_; 176 | PersonInfoT algo_result_; 177 | 178 | TrackingStatusT processing_status_; 179 | 180 | int shm_id_; 181 | int sem_set_id_; 182 | char * shm_addr_; 183 | 184 | size_t buf_size_; 185 | bool open_face_; 186 | bool open_body_; 187 | bool open_gesture_; 188 | bool open_keypoints_; 189 | bool open_reid_; 190 | bool open_focus_; 191 | bool open_face_manager_; 192 | bool is_activate_; 193 | bool face_detect_; 194 | 195 | bool main_algo_deactivated_; 196 | bool depend_deactivated_; 197 | bool body_deactivated_; 198 | bool face_deactivated_; 199 | bool focus_deactivated_; 200 | bool reid_deactivated_; 201 | bool gesture_deactivated_; 202 | bool keypoints_deactivated_; 203 | 204 | bool face_complated_; 205 | bool body_complated_; 206 | bool gesture_complated_; 207 | bool keypoints_complated_; 208 | bool reid_complated_; 209 | bool focus_complated_; 210 | }; 211 | 212 | } // namespace cyberdog_vision 213 | 214 | #endif // CYBERDOG_VISION__VISION_MANAGER_HPP_ 215 | -------------------------------------------------------------------------------- /cyberdog_vision/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | cyberdog_vision 5 | 0.0.0 6 | TODO: Package description 7 | lff 8 | Apache License, Version 2.0 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | rclcpp_lifecycle 14 | cyberdog_common 15 | protocol 16 | 17 | ament_lint_auto 18 | ament_lint_common 19 | 20 | 21 | ament_cmake 22 | 23 | 24 | -------------------------------------------------------------------------------- /cyberdog_vision/src/auto_track.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | 18 | #include "cyberdog_vision/auto_track.hpp" 19 | #include "cyberdog_vision/common_type.hpp" 20 | #include "cyberdog_common/cyberdog_log.hpp" 21 | 22 | namespace cyberdog_vision 23 | { 24 | 25 | AutoTrack::AutoTrack(const std::string & model_path) 26 | : gpu_id_(0), loss_th_(300), fail_count_(0), is_init_(false), is_lost_(false) 27 | { 28 | INFO("===Init AutoTrack==="); 29 | std::string backbone_path = model_path + "/test_backbone.onnx"; 30 | std::string head_path = model_path + "/test_rpn.onnx"; 31 | std::string reid_path = model_path + "/any_reid_v3_sim.onnx"; 32 | tracker_ptr_ = std::make_shared(backbone_path, head_path, reid_path, gpu_id_); 33 | } 34 | 35 | bool AutoTrack::SetTracker(const cv::Mat & img, const cv::Rect & bbox) 36 | { 37 | if (img.empty()) { 38 | WARN("Image empty set tracker fail."); 39 | return false; 40 | } 41 | if (0 == bbox.width && 0 == bbox.height) { 42 | WARN("Bbox to track is not valid."); 43 | return false; 44 | } 45 | 46 | XMImage xm_img; 47 | ImgConvert(img, xm_img); 48 | fail_count_ = 0; 49 | bool is_success = tracker_ptr_->init(xm_img, bbox); 50 | if (is_success) { 51 | is_init_ = true; 52 | is_lost_ = false; 53 | INFO("Set auto track success."); 54 | } 55 | return is_success; 56 | } 57 | 58 | bool AutoTrack::Track(const cv::Mat & img, cv::Rect & bbox) 59 | { 60 | if (img.empty()) { 61 | WARN("Image empty cannot track."); 62 | return false; 63 | } 64 | 65 | if (!is_init_) { 66 | WARN("Please set tracker before auto track. "); 67 | return false; 68 | } 69 | 70 | XMImage xm_img; 71 | ImgConvert(img, xm_img); 72 | tracker_ptr_->track(xm_img); 73 | TRACKER::TrackBox track_box = tracker_ptr_->getBox(); 74 | if (!track_box.track_sucess) { 75 | fail_count_++; 76 | if (fail_count_ > loss_th_) { 77 | WARN("Object lost, please set tracker to restart. "); 78 | is_init_ = false; 79 | is_lost_ = true; 80 | } 81 | return false; 82 | } 83 | bbox = track_box.rect & cv::Rect(0, 0, img.cols, img.rows); 84 | fail_count_ = 0; 85 | return true; 86 | } 87 | 88 | void AutoTrack::SetLossTh(int loss_th) 89 | { 90 | loss_th_ = loss_th; 91 | } 92 | 93 | void AutoTrack::ResetTracker() 94 | { 95 | is_init_ = false; 96 | } 97 | 98 | bool AutoTrack::GetLostStatus() 99 | { 100 | return is_lost_; 101 | } 102 | 103 | AutoTrack::~AutoTrack() 104 | {} 105 | 106 | } // namespace cyberdog_vision 107 | -------------------------------------------------------------------------------- /cyberdog_vision/src/body_detection.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | 18 | #include "cyberdog_vision/body_detection.hpp" 19 | #include "cyberdog_common/cyberdog_log.hpp" 20 | 21 | namespace cyberdog_vision 22 | { 23 | 24 | BodyDetection::BodyDetection(const std::string & model_path) 25 | : gpu_id_(0) 26 | { 27 | INFO("===Init BodyDetection==="); 28 | std::string model_det = model_path + "/detect.onnx"; 29 | std::string model_cls = model_path + "/cls_human_mid.onnx"; 30 | body_ptr_ = std::make_shared(); 31 | if (0 != body_ptr_->Init(model_det, "", model_cls, gpu_id_)) { 32 | throw std::logic_error("Init body detection algo fial. "); 33 | } 34 | } 35 | 36 | int BodyDetection::Detect(const cv::Mat & img, BodyFrameInfo & infos) 37 | { 38 | if (img.empty()) { 39 | WARN("Image is empty cannot perform detection. "); 40 | return -1; 41 | } 42 | 43 | XMImage xm_img; 44 | ImgConvert(img, xm_img); 45 | struct LogInfo log_info; 46 | if (0 != body_ptr_->GetContentMotionAnalyse(xm_img, infos, log_info, gpu_id_)) { 47 | WARN("Detacte body fail. "); 48 | return -1; 49 | } 50 | 51 | return 0; 52 | } 53 | 54 | BodyDetection::~BodyDetection() 55 | { 56 | if (body_ptr_ != nullptr) { 57 | body_ptr_->Close(); 58 | } 59 | } 60 | 61 | } // namespace cyberdog_vision 62 | -------------------------------------------------------------------------------- /cyberdog_vision/src/face_manager.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include "cyberdog_vision/face_manager.hpp" 27 | #include "cyberdog_common/cyberdog_log.hpp" 28 | #include "protocol/msg/face_result.hpp" 29 | 30 | namespace cyberdog_vision 31 | { 32 | 33 | static const char * label_path = "/home/mi/.faces/faceinfo.yaml"; 34 | // key value to judge whether face is legal or not. 35 | static const float FACE_NUMBER_STABLE_VAL = 0.0f; 36 | static const float FACE_POSE_STABLE_THRES = 3.0f; 37 | static const float FACE_POSE_YAW_LEGAL_THRES = 30.0f; 38 | static const float FACE_POSE_PITCH_LEGAL_THRES = 20.0f; 39 | static const float FACE_POSE_ROW_LEGAL_THRES = 30.0f; 40 | static const float FACE_AREA_STABLE_THRES = 0.0010f; 41 | static const float FACE_AREA_LEGAL_THRES = 0.005f; 42 | 43 | void get_mean_stdev(std::vector & vec, float & mean, double & stdev) 44 | { 45 | size_t count = vec.size(); 46 | float sum = std::accumulate(vec.begin(), vec.end(), 0.0); 47 | mean = sum / count; 48 | 49 | double accum = 0.0; 50 | for (size_t i = 0; i < count; i++) { 51 | accum += (vec[i] - mean) * (vec[i] - mean); 52 | } 53 | 54 | stdev = sqrt(accum / count); 55 | } 56 | 57 | FaceManager * FaceManager::getInstance() 58 | { 59 | static FaceManager s_instance; 60 | return &s_instance; 61 | } 62 | 63 | FaceManager::FaceManager() 64 | { 65 | m_inFaceAdding = false; 66 | initialize(); 67 | } 68 | 69 | FaceManager::~FaceManager() 70 | { 71 | } 72 | 73 | const std::string FaceManager::getFaceDataPath() 74 | { 75 | return label_path; 76 | } 77 | 78 | void FaceManager::initialize() 79 | { 80 | if (!loadFeatures()) { 81 | INFO("Failed to load face features."); 82 | } 83 | } 84 | 85 | std::map> & FaceManager::getFeatures() 86 | { 87 | std::lock_guard lock(m_mutex); 88 | 89 | return m_features; 90 | } 91 | 92 | bool FaceManager::updateFeaturesFile() 93 | { 94 | std::map>::iterator feature_iter; 95 | 96 | string faceinfofile = std::string(label_path); 97 | string face_name; 98 | bool is_host; 99 | vector face_feature; 100 | cv::FileStorage fs(faceinfofile, cv::FileStorage::WRITE); 101 | if (!fs.isOpened()) { 102 | INFO("cannot open xml file to write."); 103 | return false; 104 | } 105 | fs << "UserFaceInfo" << "["; 106 | for (feature_iter = m_features.begin(); feature_iter != m_features.end(); feature_iter++) { 107 | face_name = feature_iter->first; 108 | face_feature = feature_iter->second; 109 | is_host = m_hostMap[face_name]; 110 | fs << "{" << "name" << face_name << "is_host" << is_host << "feature" << face_feature << "}"; 111 | } 112 | fs << "]"; 113 | fs.release(); 114 | return true; 115 | } 116 | 117 | bool FaceManager::loadFeatures() 118 | { 119 | std::lock_guard lock(m_mutex); 120 | 121 | if (access(label_path, 0) != 0) { 122 | INFO("faces path not found."); 123 | umask(0); 124 | mkdir("/home/mi/.faces/", 0755); 125 | return true; 126 | } 127 | 128 | std::string faceinfofile = std::string(label_path); 129 | int is_host; 130 | std::string face_name; 131 | vector face_feature; 132 | 133 | cv::FileStorage file_read(faceinfofile, cv::FileStorage::READ); 134 | if (!file_read.isOpened()) { 135 | INFO("cannot open yaml file to write"); 136 | return false; 137 | } 138 | 139 | cv::FileNode UserFaceInfo = file_read["UserFaceInfo"]; 140 | for (cv::FileNodeIterator it = UserFaceInfo.begin(); it != UserFaceInfo.end(); ++it) { 141 | (*it)["name"] >> face_name; 142 | (*it)["feature"] >> face_feature; 143 | (*it)["is_host"] >> is_host; 144 | 145 | m_features[face_name] = face_feature; 146 | m_hostMap[face_name] = is_host; 147 | 148 | INFO("Load known face info %s host: %d", face_name.c_str(), static_cast(is_host)); 149 | } 150 | file_read.release(); 151 | 152 | return true; 153 | } 154 | 155 | int FaceManager::checkFacePose(std::vector & faceinfos, std::string & msg) 156 | { 157 | float mean[5]; 158 | double stdev[5]; 159 | 160 | m_faceStats[statsFaceNum].push_back(faceinfos.size()); 161 | if (faceinfos.size() != 1) { 162 | m_faceStats[statsFaceYaw].push_back(0.0f); 163 | m_faceStats[statsFacePitch].push_back(0.0f); 164 | m_faceStats[statsFaceRow].push_back(0.0f); 165 | m_faceStats[statsFaceArea].push_back(0.0f); 166 | } else { 167 | m_faceStats[statsFaceYaw].push_back(faceinfos[0].poses[0]); 168 | m_faceStats[statsFacePitch].push_back(faceinfos[0].poses[1]); 169 | m_faceStats[statsFaceRow].push_back(faceinfos[0].poses[2]); 170 | m_faceStats[statsFaceArea].push_back( 171 | static_cast((faceinfos[0].rect.right - faceinfos[0].rect.left) * 172 | (faceinfos[0].rect.bottom - faceinfos[0].rect.top)) / (640 * 480)); 173 | } 174 | 175 | // 1.face number should be exactly 1 176 | if (m_faceStats[statsFaceNum].full()) { 177 | get_mean_stdev(m_faceStats[statsFaceNum].vector(), mean[statsFaceNum], stdev[statsFaceNum]); 178 | if (stdev[statsFaceNum] == FACE_NUMBER_STABLE_VAL) { 179 | if (mean[statsFaceNum] == 1.0f) { 180 | } else if (mean[statsFaceNum] == 0.0f) { 181 | msg = "No face found!!"; 182 | return protocol::msg::FaceResult::RESULT_NO_FACE_FOUND; 183 | } else { 184 | msg = "More than 1 face found!!"; 185 | return protocol::msg::FaceResult::RESULT_MULTI_FACE_FOUND; 186 | } 187 | } else { 188 | msg = "keep stable!!"; 189 | return protocol::msg::FaceResult::RESULT_KEEP_STABLE; 190 | } 191 | } else { 192 | msg = "keep stable!!"; 193 | return protocol::msg::FaceResult::RESULT_KEEP_STABLE; 194 | } 195 | 196 | // face distance 197 | if (m_faceStats[statsFaceArea].full()) { 198 | get_mean_stdev(m_faceStats[statsFaceArea].vector(), mean[statsFaceArea], stdev[statsFaceArea]); 199 | if (stdev[statsFaceArea] < FACE_AREA_STABLE_THRES) { 200 | if (mean[statsFaceArea] > FACE_AREA_LEGAL_THRES) { 201 | } else { 202 | msg = "Distance is NOT OK!!"; 203 | return protocol::msg::FaceResult::RESULT_DISTANCE_NOT_SATISFIED; 204 | } 205 | } else { 206 | msg = "keep stable!!"; 207 | return protocol::msg::FaceResult::RESULT_KEEP_STABLE; 208 | } 209 | } 210 | 211 | // 3.face pose 212 | if (m_faceStats[statsFaceYaw].full() && 213 | m_faceStats[statsFacePitch].full() && 214 | m_faceStats[statsFaceRow].full()) 215 | { 216 | get_mean_stdev(m_faceStats[statsFaceYaw].vector(), mean[statsFaceYaw], stdev[statsFaceYaw]); 217 | get_mean_stdev( 218 | m_faceStats[statsFacePitch].vector(), mean[statsFacePitch], 219 | stdev[statsFacePitch]); 220 | get_mean_stdev(m_faceStats[statsFaceRow].vector(), mean[statsFaceRow], stdev[statsFaceRow]); 221 | if (stdev[statsFaceYaw] < FACE_POSE_STABLE_THRES && 222 | stdev[statsFacePitch] < FACE_POSE_STABLE_THRES && 223 | stdev[statsFaceRow] < FACE_POSE_STABLE_THRES) 224 | { 225 | if (abs(mean[statsFaceYaw]) <= FACE_POSE_YAW_LEGAL_THRES && 226 | abs(mean[statsFacePitch]) <= FACE_POSE_PITCH_LEGAL_THRES && 227 | abs(mean[statsFaceRow]) <= FACE_POSE_ROW_LEGAL_THRES) 228 | { 229 | msg = "check Face Pose success!!"; 230 | return protocol::msg::FaceResult::RESULT_SUCCESS; 231 | } else if (mean[statsFaceYaw] > FACE_POSE_YAW_LEGAL_THRES) { 232 | msg = "Degree is NOT OK: HEAD_LEFT!!"; 233 | return protocol::msg::FaceResult::RESULT_DEGREE_HEAD_LEFT; 234 | } else if (mean[statsFaceYaw] < -FACE_POSE_YAW_LEGAL_THRES) { 235 | msg = "Degree is NOT OK: HEAD_RIGHT!!"; 236 | return protocol::msg::FaceResult::RESULT_DEGREE_HEAD_RIGHT; 237 | } else if (mean[statsFacePitch] > FACE_POSE_PITCH_LEGAL_THRES) { 238 | msg = "Degree is NOT OK: HEAD_DOWN"; 239 | return protocol::msg::FaceResult::RESULT_DEGREE_HEAD_DOWN; 240 | } else if (mean[statsFacePitch] < -FACE_POSE_PITCH_LEGAL_THRES) { 241 | msg = "Degree is NOT OK: HEAD_UP!!"; 242 | return protocol::msg::FaceResult::RESULT_DEGREE_HEAD_UP; 243 | } else if (abs(mean[statsFaceRow]) > FACE_POSE_ROW_LEGAL_THRES) { 244 | msg = "Degree is NOT OK: HEAD_TILT !!"; 245 | return protocol::msg::FaceResult::RESULT_DEGREE_HEAD_TILT; 246 | } else { 247 | msg = "Degree is NOT OK!!"; 248 | return protocol::msg::FaceResult::RESULT_DEGREE_NOT_SATISFIED; 249 | } 250 | } else { 251 | msg = "keep stable!!"; 252 | return protocol::msg::FaceResult::RESULT_KEEP_STABLE; 253 | } 254 | } 255 | 256 | // can't reach here 257 | msg = "keep stable!!"; 258 | return protocol::msg::FaceResult::RESULT_KEEP_STABLE; 259 | } 260 | 261 | int FaceManager::addFaceIDCacheInfo(std::string & name, bool is_host) 262 | { 263 | m_faceIdCached.is_host = is_host; 264 | m_faceIdCached.name = name; 265 | 266 | return true; 267 | } 268 | 269 | int FaceManager::addFaceFeatureCacheInfo(std::vector & faceinfo) 270 | { 271 | m_faceFeatsCached = faceinfo[0].feats; 272 | return true; 273 | } 274 | 275 | int FaceManager::cancelAddFace() 276 | { 277 | m_faceFeatsCached.clear(); 278 | m_faceIdCached.name = ""; 279 | m_faceIdCached.is_host = false; 280 | return 0; 281 | } 282 | 283 | int FaceManager::confirmFace(std::string & name, bool is_host) 284 | { 285 | std::string filename; 286 | INFO( 287 | "confirm last face name: %s, is_host: %d", m_faceIdCached.name, 288 | static_cast(m_faceIdCached.is_host)); 289 | 290 | if (m_faceIdCached.name.compare(name) != 0 || is_host != m_faceIdCached.is_host) { 291 | INFO("confirmFace face name: %s but cache name: %s", name.c_str(), m_faceIdCached.name.c_str()); 292 | return -1; 293 | } 294 | if (m_faceFeatsCached.empty()) { 295 | INFO("Error:faceFeatsCached empty..."); 296 | return -1; 297 | } 298 | m_mutex.lock(); 299 | m_features[m_faceIdCached.name] = m_faceFeatsCached; 300 | m_hostMap[m_faceIdCached.name] = m_faceIdCached.is_host; 301 | m_mutex.unlock(); 302 | 303 | updateFeaturesFile(); 304 | /* clear face cache */ 305 | m_faceFeatsCached.clear(); 306 | m_faceIdCached.name = ""; 307 | m_faceIdCached.is_host = false; 308 | // m_faceCacheSize = 0; 309 | 310 | return 0; 311 | } 312 | 313 | int FaceManager::updateFaceId(std::string & ori_name, std::string & new_name) 314 | { 315 | if (m_features.find(ori_name) == m_features.end()) { 316 | INFO("Face name not found %s", ori_name.c_str()); 317 | return -1; 318 | } 319 | 320 | m_mutex.lock(); 321 | m_features[new_name] = m_features[ori_name]; 322 | m_features.erase(ori_name); 323 | m_hostMap[new_name] = m_hostMap[ori_name]; 324 | m_hostMap.erase(ori_name); 325 | m_mutex.unlock(); 326 | updateFeaturesFile(); 327 | return 0; 328 | } 329 | 330 | int FaceManager::deleteFace(std::string & face_name) 331 | { 332 | if (m_features.find(face_name) == m_features.end()) { 333 | INFO("Face name not found %s", face_name.c_str()); 334 | return 0; 335 | } 336 | 337 | m_mutex.lock(); 338 | m_features.erase(face_name); 339 | m_hostMap.erase(face_name); 340 | m_mutex.unlock(); 341 | 342 | updateFeaturesFile(); 343 | 344 | return 0; 345 | } 346 | 347 | 348 | std::string FaceManager::getAllFaces() 349 | { 350 | std::string all_face_info; 351 | std::string face_name; 352 | int is_host; 353 | std::map>::iterator feature_iter; 354 | 355 | for (feature_iter = m_features.begin(); feature_iter != m_features.end(); feature_iter++) { 356 | face_name = feature_iter->first; 357 | is_host = m_hostMap[face_name]; 358 | 359 | all_face_info = all_face_info + "id=" + face_name + ",host=" + std::to_string(is_host) + ";"; 360 | } 361 | 362 | return all_face_info; 363 | } 364 | 365 | 366 | bool FaceManager::findFace(const std::string & face_name) 367 | { 368 | return m_features.find(face_name) != m_features.end(); 369 | } 370 | 371 | bool FaceManager::isHost(const std::string & face_name) 372 | { 373 | if (m_hostMap.find(face_name) != m_hostMap.end()) { 374 | return m_hostMap[face_name]; 375 | } 376 | 377 | return false; 378 | } 379 | 380 | } // namespace cyberdog_vision 381 | -------------------------------------------------------------------------------- /cyberdog_vision/src/face_recognition.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include "cyberdog_vision/face_recognition.hpp" 20 | #include "cyberdog_common/cyberdog_log.hpp" 21 | 22 | namespace cyberdog_vision 23 | { 24 | 25 | FaceRecognition::FaceRecognition( 26 | const std::string & model_path, bool open_emotion, bool open_age) 27 | { 28 | INFO("===Init FaceRecognition==="); 29 | face_ptr_ = XMFaceAPI::Create(); 30 | 31 | FaceParam param; 32 | FillParam(model_path, param); 33 | param.open_emotion = open_emotion; 34 | param.open_age = open_age; 35 | param.det_scale = 512; 36 | param.feat_thres = 0.65; 37 | if (!face_ptr_->init(param)) { 38 | throw std::logic_error("Init face recognition algo fial. "); 39 | } 40 | 41 | std::string version; 42 | if (!face_ptr_->getVersion(version)) { 43 | INFO("Version of face sdk is %s", version.c_str()); 44 | } 45 | } 46 | 47 | int FaceRecognition::GetFaceInfo(const cv::Mat & img, std::vector & faces_info) 48 | { 49 | XMImage xm_img; 50 | ImgConvert(img, xm_img); 51 | if (!face_ptr_->getFaceInfo(xm_img, faces_info)) { 52 | return -1; 53 | } 54 | return 0; 55 | } 56 | 57 | int FaceRecognition::GetRecognitionResult( 58 | const cv::Mat & img, const std::map> & endlib_feats, 60 | std::vector & faces_info) 61 | { 62 | XMImage xm_img; 63 | ImgConvert(img, xm_img); 64 | if (!face_ptr_->getMatchInfo(xm_img, endlib_feats, faces_info)) { 65 | return -1; 66 | } 67 | return 0; 68 | } 69 | 70 | void FaceRecognition::FillParam(const std::string & model_path, FaceParam & param) 71 | { 72 | param.detect_mf = model_path + "/detect/mnetv2_gray_nop_light_epoch_235_512.onnx"; 73 | param.lmk_mf = model_path + "/landmark/groupdiv128a_vis_uncertain_sim.onnx"; 74 | param.feat_mf = model_path + "/feature/airfacenet_v1.onnx"; 75 | param.emotion_mf = model_path + "/emotion/speaker_v5_island_v4_data3_sim.onnx"; 76 | param.age_mf = model_path + "/age/tinyage_gray_112_age_gender_sim.onnx"; 77 | } 78 | 79 | FaceRecognition::~FaceRecognition() 80 | { 81 | if (face_ptr_ != nullptr) { 82 | XMFaceAPI::Destroy(face_ptr_); 83 | } 84 | } 85 | 86 | } // namespace cyberdog_vision 87 | -------------------------------------------------------------------------------- /cyberdog_vision/src/gesture_recognition.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include "cyberdog_vision/gesture_recognition.hpp" 20 | #include "cyberdog_common/cyberdog_log.hpp" 21 | 22 | namespace cyberdog_vision 23 | { 24 | 25 | GestureRecognition::GestureRecognition(const std::string & model_path) 26 | : max_person_num_(5) 27 | { 28 | INFO("===Init GestureRecognition==="); 29 | std::string model_det = model_path + "/hand_detect_1118_FP16.plan"; 30 | std::string model_cls = model_path + "/hand_gesture_recognition_FP16.plan"; 31 | gesture_ptr_ = std::make_shared(model_det, model_cls); 32 | } 33 | 34 | int GestureRecognition::GetGestureInfo( 35 | const cv::Mat & img, 36 | const std::vector & body_boxes, 37 | std::vector & infos) 38 | { 39 | if (body_boxes.empty()) { 40 | WARN("Have no body to detect gesture. "); 41 | return -1; 42 | } 43 | 44 | std::vector infer_bboxes; 45 | for (auto & body : body_boxes) { 46 | handgesture::bbox infer_bbox; 47 | infer_bbox.xmin = body.body_box.x; 48 | infer_bbox.ymin = body.body_box.y; 49 | infer_bbox.xmax = body.body_box.x + body.body_box.width; 50 | infer_bbox.ymax = body.body_box.y + body.body_box.height; 51 | infer_bbox.score = body.score; 52 | infer_bboxes.push_back(infer_bbox); 53 | } 54 | 55 | XMImage xm_img; 56 | ImgConvert(img, xm_img); 57 | gesture_ptr_->Inference(xm_img, infer_bboxes, max_person_num_); 58 | std::vector gesture_infos = gesture_ptr_->getResult(); 59 | 60 | for (size_t i = 0; i < gesture_infos.size(); ++i) { 61 | GestureInfo info; 62 | info.rect = cv::Rect( 63 | gesture_infos[i].left, gesture_infos[i].top, 64 | gesture_infos[i].right - gesture_infos[i].left, 65 | gesture_infos[i].bottom - gesture_infos[i].top); 66 | INFO("gesture: %d", gesture_infos[i].left); 67 | info.label = gesture_infos[i].gestureLabel; 68 | infos.push_back(info); 69 | } 70 | return 0; 71 | } 72 | 73 | void GestureRecognition::SetRecognitionNum(int num) 74 | { 75 | max_person_num_ = num; 76 | } 77 | 78 | GestureRecognition::~GestureRecognition() 79 | {} 80 | 81 | } // namespace cyberdog_vision 82 | -------------------------------------------------------------------------------- /cyberdog_vision/src/keypoints_detection.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include "cyberdog_vision/keypoints_detection.hpp" 20 | #include "cyberdog_common/cyberdog_log.hpp" 21 | 22 | namespace cyberdog_vision 23 | { 24 | 25 | KeypointsDetection::KeypointsDetection(const std::string & model_path) 26 | { 27 | INFO("===Init KeypointsDetection==="); 28 | std::string model_keypoints = model_path + "/human_keyoint_256x192x17.plan"; 29 | keypoints_ptr_ = std::make_shared(model_keypoints); 30 | } 31 | 32 | void KeypointsDetection::GetKeypointsInfo( 33 | const cv::Mat & img, 34 | const std::vector & body_boxes, 35 | std::vector> & bodies_keypoints) 36 | { 37 | bodies_keypoints.clear(); 38 | if (body_boxes.empty()) { 39 | WARN("No person detected cannot extract keypoints. "); 40 | return; 41 | } 42 | 43 | XMImage xm_img; 44 | ImgConvert(img, xm_img); 45 | std::vector infer_bboxes; 46 | for (auto & infer_bbox : body_boxes) { 47 | XMPoint point_tl = XMPoint(infer_bbox.body_box.x, infer_bbox.body_box.y); 48 | XMPoint point_br = XMPoint( 49 | infer_bbox.body_box.x + infer_bbox.body_box.width, 50 | infer_bbox.body_box.y + infer_bbox.body_box.height); 51 | infer_bboxes.push_back({point_tl, point_br}); 52 | } 53 | 54 | bool is_save_keypoints = false; 55 | bool is_show_names = false; 56 | keypoints_ptr_->Inference(xm_img, infer_bboxes, is_save_keypoints, is_show_names); 57 | std::vector> xm_points = keypoints_ptr_->Get_Persons_Keypoints(); 58 | for (size_t i = 0; i < xm_points.size(); ++i) { 59 | std::vector single_body_keypoints; 60 | for (size_t j = 0; j < xm_points[i].size(); ++j) { 61 | single_body_keypoints.push_back(cv::Point2f(xm_points[i][j].x, xm_points[i][j].y)); 62 | } 63 | bodies_keypoints.push_back(single_body_keypoints); 64 | } 65 | } 66 | 67 | KeypointsDetection::~KeypointsDetection() 68 | {} 69 | 70 | } // namespace cyberdog_vision 71 | -------------------------------------------------------------------------------- /cyberdog_vision/src/main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "cyberdog_vision/vision_manager.hpp" 18 | #include "cyberdog_common/cyberdog_log.hpp" 19 | 20 | int main(int argc, char * argv[]) 21 | { 22 | LOGGER_MAIN_INSTANCE("vision_manager"); 23 | rclcpp::init(argc, argv); 24 | auto node = std::make_shared(); 25 | rclcpp::executors::MultiThreadedExecutor exec_; 26 | exec_.add_node(node->get_node_base_interface()); 27 | exec_.spin(); 28 | rclcpp::shutdown(); 29 | 30 | return 0; 31 | } 32 | -------------------------------------------------------------------------------- /cyberdog_vision/src/person_reid.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | 18 | #include "cyberdog_vision/person_reid.hpp" 19 | #include "cyberdog_common/cyberdog_log.hpp" 20 | 21 | const int kFeatLen = 128; 22 | namespace cyberdog_vision 23 | { 24 | 25 | PersonReID::PersonReID(const std::string & model_path) 26 | : gpu_id_(0), tracking_id_(0), object_loss_th_(300), library_frame_num_(15), unmatch_count_(0), 27 | feat_sim_th_(0.8), feat_update_th_(0.9), is_tracking_(false), is_lost_(true), reid_ptr_(nullptr) 28 | { 29 | INFO("===Init PersonReID==="); 30 | std::string model_reid = model_path + "/reid_v4_mid.engine"; 31 | if (0 != REID_Init(reid_ptr_, model_reid.c_str(), gpu_id_)) { 32 | throw std::logic_error("Init person reid algo fail. "); 33 | } 34 | } 35 | 36 | int PersonReID::SetTracker( 37 | const cv::Mat & img, const cv::Rect & body_box, 38 | std::vector & reid_feat) 39 | { 40 | if (0 != GetFeature(img, body_box, reid_feat)) { 41 | WARN("GetFeature fail. "); 42 | return -1; 43 | } 44 | tracker_feat_.assign(reid_feat.begin(), reid_feat.end()); 45 | if (is_tracking_) { 46 | tracking_id_++; 47 | } 48 | is_tracking_ = true; 49 | is_lost_ = false; 50 | 51 | return 0; 52 | } 53 | 54 | int PersonReID::GetReIDInfo( 55 | const cv::Mat & img, const std::vector & body_bboxes, 56 | int & id, cv::Rect & tracked) 57 | { 58 | if (tracker_feat_.empty()) { 59 | WARN("Please set tracker before tracking. "); 60 | return -1; 61 | } 62 | 63 | id = -1; 64 | tracked = cv::Rect(0, 0, 0, 0); 65 | size_t index = 0; 66 | float max_sim = 0; 67 | std::vector match_feat; 68 | for (size_t i = 0; i < body_bboxes.size(); ++i) { 69 | // For every body bbox 70 | std::vector feat; 71 | if (0 != GetFeature(img, body_bboxes[i].body_box, feat)) { 72 | return -1; 73 | } 74 | 75 | float sim_val = GetSim(feat, tracker_feat_, SimType::kSimOne2Group); 76 | INFO("Object %d, sim: %f", i, sim_val); 77 | if (sim_val > max_sim) { 78 | index = i; 79 | max_sim = sim_val; 80 | match_feat.assign(feat.begin(), feat.end()); 81 | } 82 | } 83 | 84 | if (max_sim > feat_sim_th_) { 85 | // Match success 86 | unmatch_count_ = 0; 87 | id = tracking_id_; 88 | tracked = body_bboxes[index].body_box; 89 | INFO( 90 | "Match success, sim: %f, bbox: %d,%d,%d,%d", max_sim, tracked.x, tracked.y, tracked.width, 91 | tracked.height); 92 | if (max_sim > feat_update_th_) { 93 | // Update library feat 94 | if (static_cast(tracker_feat_.size()) / kFeatLen == library_frame_num_) { 95 | tracker_feat_.erase(tracker_feat_.begin(), tracker_feat_.begin() + kFeatLen); 96 | } 97 | tracker_feat_.insert(tracker_feat_.end(), match_feat.begin(), match_feat.end()); 98 | } 99 | } else { 100 | WARN("Match fail, current count: %d", unmatch_count_); 101 | unmatch_count_++; 102 | if (unmatch_count_ > object_loss_th_) { 103 | WARN("Object is lost. "); 104 | is_lost_ = true; 105 | ResetTracker(); 106 | } 107 | } 108 | return 0; 109 | } 110 | 111 | int PersonReID::GetFeatureLen() 112 | { 113 | return REID_GetFeatLen(); 114 | } 115 | 116 | void PersonReID::ResetTracker() 117 | { 118 | is_tracking_ = false; 119 | tracker_feat_.clear(); 120 | tracking_id_++; 121 | } 122 | 123 | bool PersonReID::GetLostStatus() 124 | { 125 | return is_lost_; 126 | } 127 | 128 | int PersonReID::GetFeature( 129 | const cv::Mat & img, const cv::Rect & body_box, 130 | std::vector & reid_feat) 131 | { 132 | reid_feat.clear(); 133 | cv::Mat reid_img = img(body_box).clone(); 134 | XMReIDImage xm_reid_img; 135 | xm_reid_img.data = reid_img.data; 136 | xm_reid_img.height = reid_img.rows; 137 | xm_reid_img.width = reid_img.cols; 138 | xm_reid_img.fmt = XM_IMG_FMT_BGR; 139 | float * feat = nullptr; 140 | if (0 != REID_ExtractFeat(reid_ptr_, &xm_reid_img, feat)) { 141 | WARN("Extract reid feature fail."); 142 | return -1; 143 | } 144 | reid_feat.resize(GetFeatureLen()); 145 | if (feat != nullptr) { 146 | memcpy(reid_feat.data(), feat, sizeof(float) * GetFeatureLen()); 147 | feat = nullptr; 148 | } 149 | return 0; 150 | } 151 | 152 | float PersonReID::GetSim( 153 | std::vector & feat_det, std::vector & feat_library, 154 | const SimType & sim_type) 155 | { 156 | float sim_value; 157 | switch (sim_type) { 158 | case SimType::kSimOne2One: 159 | sim_value = REID_GetSimOfOne2One(reid_ptr_, feat_det.data(), feat_library.data()); 160 | break; 161 | case SimType::kSimOne2Group: 162 | sim_value = REID_GetSimOfOne2Group( 163 | reid_ptr_, feat_det.data(), 164 | feat_library.data(), feat_library.size() / kFeatLen); 165 | break; 166 | case SimType::kSimGroup2Group: 167 | sim_value = REID_GetSimOfGroup2Group( 168 | reid_ptr_, feat_det.data(), 169 | feat_det.size() / kFeatLen, feat_library.data(), feat_library.size() / kFeatLen); 170 | break; 171 | } 172 | return sim_value; 173 | } 174 | 175 | PersonReID::~PersonReID() 176 | { 177 | if (reid_ptr_) { 178 | REID_Release(reid_ptr_); 179 | } 180 | } 181 | 182 | } // namespace cyberdog_vision 183 | -------------------------------------------------------------------------------- /cyberdog_vision/src/vision_manager.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include "cyberdog_vision/vision_manager.hpp" 26 | #include "cyberdog_vision/semaphore_op.hpp" 27 | #include "cyberdog_common/cyberdog_log.hpp" 28 | 29 | #define SHM_PROJ_ID 'A' 30 | #define SEM_PROJ_ID 'B' 31 | 32 | const int kKeypointsNum = 17; 33 | const char kModelPath[] = "/SDCARD/vision"; 34 | const char kLibraryPath[] = "/home/mi/.faces/faceinfo.yaml"; 35 | namespace cyberdog_vision 36 | { 37 | 38 | VisionManager::VisionManager() 39 | : rclcpp_lifecycle::LifecycleNode("vision_manager"), 40 | img_proc_thread_(nullptr), main_manager_thread_(nullptr), 41 | depend_manager_thread_(nullptr), body_det_thread_(nullptr), 42 | face_thread_(nullptr), focus_thread_(nullptr), 43 | gesture_thread_(nullptr), reid_thread_(nullptr), 44 | keypoints_thread_(nullptr), body_ptr_(nullptr), 45 | face_ptr_(nullptr), focus_ptr_(nullptr), 46 | gesture_ptr_(nullptr), reid_ptr_(nullptr), 47 | keypoints_ptr_(nullptr), shm_addr_(nullptr), buf_size_(6), 48 | open_face_(false), open_body_(false), open_gesture_(false), 49 | open_keypoints_(false), open_reid_(false), open_focus_(false), 50 | open_face_manager_(false), is_activate_(false), main_algo_deactivated_(false), 51 | depend_deactivated_(false), body_deactivated_(false), 52 | face_deactivated_(false), focus_deactivated_(false), 53 | reid_deactivated_(false), gesture_deactivated_(false), 54 | keypoints_deactivated_(false), face_complated_(false), 55 | body_complated_(false), gesture_complated_(false), 56 | keypoints_complated_(false), reid_complated_(false), 57 | focus_complated_(false) 58 | { 59 | setvbuf(stdout, NULL, _IONBF, BUFSIZ); 60 | INFO("Vision manager constructor complated."); 61 | } 62 | 63 | ReturnResultT VisionManager::on_configure(const rclcpp_lifecycle::State & /*state*/) 64 | { 65 | INFO("Configuring vision_manager. "); 66 | if (0 != Init()) { 67 | return ReturnResultT::FAILURE; 68 | } 69 | INFO("Configure complated. "); 70 | return ReturnResultT::SUCCESS; 71 | } 72 | 73 | ReturnResultT VisionManager::on_activate(const rclcpp_lifecycle::State & /*state*/) 74 | { 75 | INFO("Activating vision_manager. "); 76 | if (!CallService(camera_clinet_, 0, "face-interval=1")) { 77 | ERROR("Start camera stream fail. "); 78 | return ReturnResultT::FAILURE; 79 | } 80 | INFO("Start camera stream success. "); 81 | is_activate_ = true; 82 | CreateObjectAI(); 83 | CreateThread(); 84 | person_pub_->on_activate(); 85 | status_pub_->on_activate(); 86 | face_result_pub_->on_activate(); 87 | processing_status_.status = TrackingStatusT::STATUS_SELECTING; 88 | INFO("Activate complated. "); 89 | return ReturnResultT::SUCCESS; 90 | } 91 | 92 | ReturnResultT VisionManager::on_deactivate(const rclcpp_lifecycle::State & /*state*/) 93 | { 94 | INFO("Deactivating vision_manager. "); 95 | is_activate_ = false; 96 | DestoryThread(); 97 | ResetAlgo(); 98 | if (!CallService(camera_clinet_, 0, "face-interval=0")) { 99 | ERROR("Close camera stream fail. "); 100 | return ReturnResultT::FAILURE; 101 | } 102 | INFO("Close camera stream success. "); 103 | person_pub_->on_deactivate(); 104 | status_pub_->on_deactivate(); 105 | face_result_pub_->on_deactivate(); 106 | ResetCudaDevs(); 107 | INFO("Deactivate success. "); 108 | return ReturnResultT::SUCCESS; 109 | } 110 | 111 | ReturnResultT VisionManager::on_cleanup(const rclcpp_lifecycle::State & /*state*/) 112 | { 113 | INFO("Cleaning up vision_manager. "); 114 | img_proc_thread_.reset(); 115 | main_manager_thread_.reset(); 116 | depend_manager_thread_.reset(); 117 | body_det_thread_.reset(); 118 | face_thread_.reset(); 119 | focus_thread_.reset(); 120 | gesture_thread_.reset(); 121 | reid_thread_.reset(); 122 | keypoints_thread_.reset(); 123 | person_pub_.reset(); 124 | status_pub_.reset(); 125 | face_result_pub_.reset(); 126 | tracking_service_.reset(); 127 | algo_manager_service_.reset(); 128 | facemanager_service_.reset(); 129 | camera_clinet_.reset(); 130 | INFO("Clean up complated. "); 131 | return ReturnResultT::SUCCESS; 132 | } 133 | 134 | ReturnResultT VisionManager::on_shutdown(const rclcpp_lifecycle::State & /*state*/) 135 | { 136 | INFO("Shutting down vision_manager. "); 137 | return ReturnResultT::SUCCESS; 138 | } 139 | 140 | int VisionManager::Init() 141 | { 142 | if (0 != InitIPC()) { 143 | ERROR("Init shared memory or semaphore fail. "); 144 | return -1; 145 | } 146 | 147 | // Create ROS object 148 | CreateObjectROS(); 149 | 150 | return 0; 151 | } 152 | 153 | int VisionManager::InitIPC() 154 | { 155 | // Create shared memory and get address 156 | if (0 != CreateShm(SHM_PROJ_ID, sizeof(uint64_t) + IMAGE_SIZE, shm_id_)) { 157 | return -1; 158 | } 159 | shm_addr_ = GetShmAddr(shm_id_, sizeof(uint64_t) + IMAGE_SIZE); 160 | if (shm_addr_ == nullptr) { 161 | return -1; 162 | } 163 | 164 | // Create semaphore set, 0-mutex, 1-empty, 2-full 165 | if (0 != CreateSem(SEM_PROJ_ID, 3, sem_set_id_)) { 166 | return -1; 167 | } 168 | 169 | // Set init value of the semaphore 170 | if (0 != SetSemInitVal(sem_set_id_, 0, 1)) { 171 | return -1; 172 | } 173 | 174 | if (0 != SetSemInitVal(sem_set_id_, 1, 1)) { 175 | return -1; 176 | } 177 | 178 | if (0 != SetSemInitVal(sem_set_id_, 2, 0)) { 179 | return -1; 180 | } 181 | 182 | return 0; 183 | } 184 | 185 | void VisionManager::CreateObjectAI() 186 | { 187 | // Create AI object 188 | INFO("Create object start. "); 189 | if (open_body_) { 190 | body_ptr_ = std::make_shared( 191 | kModelPath + std::string("/body_gesture")); 192 | } 193 | 194 | if (open_face_ || open_face_manager_) { 195 | face_ptr_ = std::make_shared( 196 | kModelPath + std::string( 197 | "/face_recognition"), true, true); 198 | } 199 | 200 | if (open_focus_) { 201 | focus_ptr_ = std::make_shared( 202 | kModelPath + std::string("/auto_track")); 203 | } 204 | 205 | if (open_gesture_) { 206 | gesture_ptr_ = std::make_shared( 207 | kModelPath + std::string("/body_gesture")); 208 | } 209 | 210 | if (open_reid_) { 211 | reid_ptr_ = 212 | std::make_shared( 213 | kModelPath + 214 | std::string("/person_reid")); 215 | } 216 | 217 | if (open_keypoints_) { 218 | keypoints_ptr_ = std::make_shared( 219 | kModelPath + std::string("/keypoints_detection")); 220 | } 221 | 222 | INFO("Create AI object complated. "); 223 | } 224 | 225 | void VisionManager::CreateObjectROS() 226 | { 227 | INFO("Create ROS object. "); 228 | // Create service server 229 | tracking_service_ = create_service( 230 | "tracking_object", std::bind( 231 | &VisionManager::TrackingService, this, 232 | std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); 233 | 234 | algo_manager_service_ = create_service( 235 | "algo_manager", std::bind( 236 | &VisionManager::AlgoManagerService, this, 237 | std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); 238 | 239 | facemanager_service_ = create_service( 240 | "facemanager", std::bind( 241 | &VisionManager::FaceManagerService, this, 242 | std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); 243 | 244 | // Create service client 245 | camera_clinet_ = create_client("camera_service"); 246 | 247 | // Create publisher 248 | rclcpp::SensorDataQoS pub_qos; 249 | pub_qos.reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); 250 | person_pub_ = create_publisher("person", pub_qos); 251 | status_pub_ = create_publisher("processing_status", pub_qos); 252 | face_result_pub_ = create_publisher("facemanager/face_result", pub_qos); 253 | } 254 | 255 | void VisionManager::CreateThread() 256 | { 257 | img_proc_thread_ = std::make_shared(&VisionManager::ImageProc, this); 258 | 259 | // Create thread depends algorithm selection 260 | if (!open_face_manager_) { 261 | main_manager_thread_ = std::make_shared(&VisionManager::MainAlgoManager, this); 262 | } 263 | if (open_reid_ || open_gesture_ || open_keypoints_) { 264 | depend_manager_thread_ = std::make_shared(&VisionManager::DependAlgoManager, this); 265 | } 266 | if (open_body_) { 267 | body_det_thread_ = std::make_shared(&VisionManager::BodyDet, this); 268 | } 269 | if (open_face_) { 270 | face_thread_ = std::make_shared(&VisionManager::FaceRecognize, this); 271 | } 272 | if (open_focus_) { 273 | focus_thread_ = std::make_shared(&VisionManager::FocusTrack, this); 274 | } 275 | if (open_gesture_) { 276 | gesture_thread_ = std::make_shared(&VisionManager::GestureRecognize, this); 277 | } 278 | if (open_reid_) { 279 | reid_thread_ = std::make_shared(&VisionManager::ReIDProc, this); 280 | } 281 | if (open_keypoints_) { 282 | keypoints_thread_ = std::make_shared(&VisionManager::KeypointsDet, this); 283 | } 284 | } 285 | 286 | void VisionManager::ImageProc() 287 | { 288 | while (rclcpp::ok()) { 289 | if (!is_activate_) { 290 | INFO("ImageProc: Deactivate to return image thread. "); 291 | return; 292 | } 293 | if (0 != WaitSem(sem_set_id_, 2)) {return;} 294 | if (0 != WaitSem(sem_set_id_, 0)) {return;} 295 | StampedImage simg; 296 | simg.img.create(480, 640, CV_8UC3); 297 | memcpy(simg.img.data, reinterpret_cast(shm_addr_) + sizeof(uint64_t), IMAGE_SIZE); 298 | uint64_t time; 299 | memcpy(&time, reinterpret_cast(shm_addr_), sizeof(uint64_t)); 300 | simg.header.stamp.sec = time / 1000000000; 301 | simg.header.stamp.nanosec = time % 1000000000; 302 | INFO("Received rgb image, ts: %.9d.%.9d", simg.header.stamp.sec, simg.header.stamp.nanosec); 303 | if (0 != SignalSem(sem_set_id_, 0)) {return;} 304 | if (0 != SignalSem(sem_set_id_, 1)) {return;} 305 | 306 | // Save image to buffer, only process with real img 307 | { 308 | std::unique_lock lk(global_img_buf_.mtx); 309 | global_img_buf_.img_buf.clear(); 310 | global_img_buf_.img_buf.push_back(simg); 311 | global_img_buf_.is_filled = true; 312 | global_img_buf_.cond.notify_one(); 313 | INFO("ImageProc: Notify main thread. "); 314 | } 315 | } 316 | } 317 | 318 | void VisionManager::MainAlgoManager() 319 | { 320 | while (rclcpp::ok()) { 321 | { 322 | INFO("MainAlgoManager: Wait to activate main thread. "); 323 | std::unique_lock lk(global_img_buf_.mtx); 324 | global_img_buf_.cond.wait(lk, [this] {return global_img_buf_.is_filled;}); 325 | global_img_buf_.is_filled = false; 326 | INFO("MainAlgoManager: Activate main algo manager thread. "); 327 | } 328 | if (!is_activate_) { 329 | INFO("MainAlgoManager: Deactivate to return main algo thread. "); 330 | main_algo_deactivated_ = true; 331 | return; 332 | } 333 | 334 | if (open_body_) { 335 | std::unique_lock lk_result(body_struct_.mtx); 336 | if (!body_struct_.is_called) { 337 | body_complated_ = false; 338 | reid_complated_ = false; 339 | gesture_complated_ = false; 340 | keypoints_complated_ = false; 341 | body_struct_.is_called = true; 342 | body_struct_.cond.notify_one(); 343 | } 344 | } 345 | 346 | if (open_face_) { 347 | std::unique_lock lk_result(face_struct_.mtx); 348 | if (!face_struct_.is_called) { 349 | face_complated_ = false; 350 | face_struct_.is_called = true; 351 | face_struct_.cond.notify_one(); 352 | } 353 | } 354 | 355 | if (open_focus_) { 356 | std::unique_lock lk_result(focus_struct_.mtx); 357 | if (!focus_struct_.is_called) { 358 | focus_complated_ = false; 359 | focus_struct_.is_called = true; 360 | focus_struct_.cond.notify_one(); 361 | } 362 | } 363 | 364 | // Wait for result to pub 365 | if (open_body_ || open_face_ || open_focus_) { 366 | std::unique_lock lk_proc(algo_proc_.mtx); 367 | INFO("MainAlgoManager: main thread process_complated: %d", algo_proc_.process_complated); 368 | algo_proc_.cond.wait(lk_proc, [this] {return algo_proc_.process_complated;}); 369 | INFO("MainAlgoManager: Main thread wake up to pub. "); 370 | { 371 | std::unique_lock lk_result(result_mtx_); 372 | person_pub_->publish(algo_result_); 373 | INFO( 374 | "MainAlgoManager: Publish det stamp: %d.%d", algo_result_.body_info.header.stamp.sec, 375 | algo_result_.body_info.header.stamp.nanosec); 376 | INFO( 377 | "MainAlgoManager: Publish track stamp: %d.%d", algo_result_.track_res.header.stamp.sec, 378 | algo_result_.track_res.header.stamp.nanosec); 379 | for (size_t i = 0; i < algo_result_.body_info.infos.size(); ++i) { 380 | sensor_msgs::msg::RegionOfInterest rect = algo_result_.body_info.infos[i].roi; 381 | INFO( 382 | "MainAlgoManager: Publish detection %d bbox: %d,%d,%d,%d", i, rect.x_offset, 383 | rect.y_offset, rect.width, rect.height); 384 | } 385 | sensor_msgs::msg::RegionOfInterest rect = algo_result_.track_res.roi; 386 | INFO( 387 | "MainAlgoManager: Publish tracked bbox: %d,%d,%d,%d", rect.x_offset, rect.y_offset, 388 | rect.width, 389 | rect.height); 390 | PersonInfoT person_info; 391 | algo_result_ = person_info; 392 | } 393 | if (open_body_ || open_focus_) { 394 | INFO("MainAlgoManager: Publish processing status: %d", (int)processing_status_.status); 395 | status_pub_->publish(processing_status_); 396 | } 397 | algo_proc_.process_complated = false; 398 | } 399 | INFO("MainAlgoManager: end of main thread ."); 400 | } 401 | } 402 | 403 | void VisionManager::DependAlgoManager() 404 | { 405 | while (rclcpp::ok()) { 406 | { 407 | INFO("DependAlgoManager: Wait to activate depend thread. "); 408 | std::unique_lock lk(body_results_.mtx); 409 | body_results_.cond.wait(lk, [this] {return body_results_.is_filled;}); 410 | body_results_.is_filled = false; 411 | INFO("DependAlgoManager: Activate depend algo manager thread. "); 412 | } 413 | if (!is_activate_) { 414 | INFO("DependAlgoManager: Deactivate to return depend thread. "); 415 | depend_deactivated_ = true; 416 | return; 417 | } 418 | 419 | if (open_reid_) { 420 | std::unique_lock lk_result(reid_struct_.mtx); 421 | if (!reid_struct_.is_called) { 422 | reid_struct_.is_called = true; 423 | reid_struct_.cond.notify_one(); 424 | } 425 | } 426 | 427 | if (open_gesture_) { 428 | std::unique_lock lk_result(gesture_struct_.mtx); 429 | if (!gesture_struct_.is_called) { 430 | gesture_struct_.is_called = true; 431 | gesture_struct_.cond.notify_one(); 432 | } 433 | } 434 | 435 | if (open_keypoints_) { 436 | std::unique_lock lk_result(keypoints_struct_.mtx); 437 | if (!keypoints_struct_.is_called) { 438 | keypoints_struct_.is_called = true; 439 | keypoints_struct_.cond.notify_one(); 440 | } 441 | } 442 | INFO("DependAlgoManager: end of depend thread. "); 443 | } 444 | } 445 | 446 | cv::Rect Convert(const sensor_msgs::msg::RegionOfInterest & roi) 447 | { 448 | cv::Rect bbox = cv::Rect(roi.x_offset, roi.y_offset, roi.width, roi.height); 449 | return bbox; 450 | } 451 | 452 | sensor_msgs::msg::RegionOfInterest Convert(const cv::Rect & bbox) 453 | { 454 | sensor_msgs::msg::RegionOfInterest roi; 455 | roi.x_offset = bbox.x; 456 | roi.y_offset = bbox.y; 457 | roi.width = bbox.width; 458 | roi.height = bbox.height; 459 | return roi; 460 | } 461 | 462 | void Convert(const std_msgs::msg::Header & header, const BodyFrameInfo & from, BodyInfoT & to) 463 | { 464 | to.header = header; 465 | to.count = from.size(); 466 | to.infos.clear(); 467 | for (size_t i = 0; i < from.size(); ++i) { 468 | BodyT body; 469 | body.roi.x_offset = from[i].left; 470 | body.roi.y_offset = from[i].top; 471 | body.roi.width = from[i].width; 472 | body.roi.height = from[i].height; 473 | to.infos.push_back(body); 474 | } 475 | } 476 | 477 | void VisionManager::BodyDet() 478 | { 479 | while (rclcpp::ok()) { 480 | { 481 | INFO("BodyDet: Wait to activate body thread. "); 482 | std::unique_lock lk_struct(body_struct_.mtx); 483 | body_struct_.cond.wait(lk_struct, [this] {return body_struct_.is_called;}); 484 | body_struct_.is_called = false; 485 | INFO("BodyDet: Activate body detect thread. "); 486 | } 487 | if (!is_activate_) { 488 | INFO("BodyDet: Deactivate to return body thread. "); 489 | body_deactivated_ = true; 490 | return; 491 | } 492 | 493 | // Get image and detect body 494 | StampedImage stamped_img; 495 | { 496 | std::unique_lock lk(global_img_buf_.mtx); 497 | stamped_img = global_img_buf_.img_buf.back(); 498 | } 499 | 500 | BodyFrameInfo infos; 501 | { 502 | std::unique_lock lk_body(body_results_.mtx); 503 | if (-1 != body_ptr_->Detect(stamped_img.img, infos)) { 504 | if (body_results_.body_infos.size() >= buf_size_) { 505 | body_results_.body_infos.erase(body_results_.body_infos.begin()); 506 | } 507 | body_results_.body_infos.push_back(infos); 508 | body_results_.detection_img.img = stamped_img.img.clone(); 509 | body_results_.detection_img.header = stamped_img.header; 510 | body_results_.is_filled = true; 511 | body_results_.cond.notify_one(); 512 | INFO("BodyDet: Body thread notify depend thread. "); 513 | 514 | INFO("BodyDet: Body detection num: %d", infos.size()); 515 | for (size_t count = 0; count < infos.size(); ++count) { 516 | INFO( 517 | "BodyDet: Person %d: sim: %f, x: %d", count, infos[count].score, 518 | infos[count].left); 519 | } 520 | } else { 521 | WARN("BodyDet: Body detect fail of current image. "); 522 | } 523 | } 524 | 525 | // Storage body detection result 526 | { 527 | std::lock(algo_proc_.mtx, result_mtx_); 528 | std::unique_lock lk_proc(algo_proc_.mtx, std::adopt_lock); 529 | std::unique_lock lk_result(result_mtx_, std::adopt_lock); 530 | body_complated_ = true; 531 | algo_result_.header = stamped_img.header; 532 | Convert(stamped_img.header, infos, algo_result_.body_info); 533 | INFO( 534 | "BodyDet: Det result-img: %d.%d", stamped_img.header.stamp.sec, 535 | stamped_img.header.stamp.nanosec); 536 | INFO( 537 | "BodyDet: Det result-body: %d.%d", algo_result_.body_info.header.stamp.sec, 538 | algo_result_.body_info.header.stamp.nanosec); 539 | SetThreadState("BodyDet", algo_proc_.process_complated); 540 | INFO("BodyDet: body thread process_complated: %d", algo_proc_.process_complated); 541 | if (algo_proc_.process_complated) { 542 | INFO("BodyDet: Body thread notify to pub. "); 543 | algo_proc_.cond.notify_one(); 544 | } 545 | } 546 | } 547 | } 548 | 549 | void Convert( 550 | const std_msgs::msg::Header & header, const std::vector & from, 551 | FaceInfoT & to) 552 | { 553 | to.header = header; 554 | to.count = from.size(); 555 | to.infos.clear(); 556 | for (size_t i = 0; i < from.size(); ++i) { 557 | FaceT face; 558 | face.roi.x_offset = from[i].rect.left; 559 | face.roi.y_offset = from[i].rect.top; 560 | face.roi.width = from[i].rect.right - from[i].rect.left; 561 | face.roi.height = from[i].rect.bottom - from[i].rect.top; 562 | face.id = from[i].face_id; 563 | face.score = from[i].score; 564 | face.match = from[i].match_score; 565 | face.yaw = from[i].poses[0]; 566 | face.pitch = from[i].poses[1]; 567 | face.row = from[i].poses[2]; 568 | face.age = from[i].ages[0]; 569 | face.emotion = from[i].emotions[0]; 570 | to.infos.push_back(face); 571 | } 572 | } 573 | 574 | void VisionManager::FaceRecognize() 575 | { 576 | while (rclcpp::ok()) { 577 | { 578 | INFO("FaceRecognize: Wait to activate face thread. "); 579 | std::unique_lock lk(face_struct_.mtx); 580 | face_struct_.cond.wait(lk, [this] {return face_struct_.is_called;}); 581 | face_struct_.is_called = false; 582 | INFO("FaceRecognize: Activate face recognition thread. "); 583 | } 584 | if (!is_activate_) { 585 | INFO("FaceRecognize: Deactivate to return face thread. "); 586 | face_deactivated_ = true; 587 | return; 588 | } 589 | 590 | // Get image to proc 591 | StampedImage stamped_img; 592 | { 593 | std::unique_lock lk(global_img_buf_.mtx); 594 | stamped_img = global_img_buf_.img_buf.back(); 595 | } 596 | 597 | // Face recognition and get result 598 | std::vector result; 599 | if (0 != face_ptr_->GetRecognitionResult(stamped_img.img, face_library_, result)) { 600 | WARN("FaceRecognize: Face recognition fail. "); 601 | } 602 | 603 | // Storage face recognition result 604 | { 605 | std::lock(algo_proc_.mtx, result_mtx_); 606 | std::unique_lock lk_proc(algo_proc_.mtx, std::adopt_lock); 607 | std::unique_lock lk_result(result_mtx_, std::adopt_lock); 608 | face_complated_ = true; 609 | algo_result_.header = stamped_img.header; 610 | Convert(stamped_img.header, result, algo_result_.face_info); 611 | SetThreadState("FaceRecognize", algo_proc_.process_complated); 612 | INFO("FaceRecognize: face thread process_complated: %d", algo_proc_.process_complated); 613 | if (algo_proc_.process_complated) { 614 | INFO("FaceRecognize: Face thread notify to pub. "); 615 | algo_proc_.cond.notify_one(); 616 | } 617 | } 618 | } 619 | } 620 | 621 | void Convert( 622 | const std_msgs::msg::Header & header, const cv::Rect & from, TrackResultT & to) 623 | { 624 | to.header = header; 625 | to.roi = Convert(from); 626 | } 627 | 628 | void VisionManager::FocusTrack() 629 | { 630 | while (rclcpp::ok()) { 631 | { 632 | INFO("FocusTrack: Wait to activate focus thread. "); 633 | std::unique_lock lk(focus_struct_.mtx); 634 | focus_struct_.cond.wait(lk, [this] {return focus_struct_.is_called;}); 635 | focus_struct_.is_called = false; 636 | INFO("FocusTrack: Activate focus thread. "); 637 | } 638 | if (!is_activate_) { 639 | INFO("FocusTrack: Deactivate to return focus thread. "); 640 | focus_deactivated_ = true; 641 | return; 642 | } 643 | 644 | // Get image to proc 645 | StampedImage stamped_img; 646 | { 647 | std::unique_lock lk(global_img_buf_.mtx); 648 | stamped_img = global_img_buf_.img_buf.back(); 649 | } 650 | 651 | // Focus track and get result 652 | cv::Rect track_res = cv::Rect(0, 0, 0, 0); 653 | if (!focus_ptr_->Track(stamped_img.img, track_res)) { 654 | WARN("FocusTrack: Auto track fail of crunt frame. "); 655 | } 656 | if (focus_ptr_->GetLostStatus()) { 657 | WARN("FocusTrack: Auto track object lost. "); 658 | processing_status_.status = TrackingStatusT::STATUS_SELECTING; 659 | } 660 | 661 | // Storage foucs track result 662 | { 663 | std::lock(algo_proc_.mtx, result_mtx_); 664 | std::unique_lock lk_proc(algo_proc_.mtx, std::adopt_lock); 665 | std::unique_lock lk_result(result_mtx_, std::adopt_lock); 666 | focus_complated_ = true; 667 | // Convert data to publish 668 | INFO( 669 | "FocusTrack: Focus result %d,%d,%d,%d.", track_res.x, track_res.y, track_res.width, 670 | track_res.height); 671 | algo_result_.header = stamped_img.header; 672 | Convert(stamped_img.header, track_res, algo_result_.track_res); 673 | SetThreadState("FocusTrack", algo_proc_.process_complated); 674 | INFO("FocusTrack: focus thread process_complated: %d", algo_proc_.process_complated); 675 | if (algo_proc_.process_complated) { 676 | INFO("FocusTrack: Focus thread notify to pub. "); 677 | algo_proc_.cond.notify_one(); 678 | } 679 | } 680 | } 681 | } 682 | 683 | double GetIOU(const HumanBodyInfo & b1, const sensor_msgs::msg::RegionOfInterest & b2) 684 | { 685 | int w = 686 | std::max( 687 | std::min((b1.left + b1.width), (b2.x_offset + b2.width)) - std::max( 688 | b1.left, 689 | b2.x_offset), 690 | (uint32_t)0); 691 | int h = 692 | std::max( 693 | std::min((b1.top + b1.height), (b2.y_offset + b2.height)) - std::max( 694 | b1.top, 695 | b2.y_offset), 696 | (uint32_t)0); 697 | 698 | return w * h / static_cast(b1.width * b1.height + 699 | b2.width * b2.height - w * h); 700 | } 701 | 702 | void VisionManager::ReIDProc() 703 | { 704 | while (rclcpp::ok()) { 705 | int person_id = -1; 706 | cv::Rect tracked_bbox = cv::Rect(0, 0, 0, 0); 707 | { 708 | INFO("ReIDProc: Wait to activate reid thread. "); 709 | std::unique_lock lk_reid(reid_struct_.mtx); 710 | reid_struct_.cond.wait(lk_reid, [this] {return reid_struct_.is_called;}); 711 | reid_struct_.is_called = false; 712 | INFO("ReIDProc: Activate reid thread. "); 713 | } 714 | if (!is_activate_) { 715 | INFO("ReIDProc: Deactivate to return reid thread. "); 716 | reid_deactivated_ = true; 717 | return; 718 | } 719 | 720 | // ReID and get result 721 | cv::Mat img_show; 722 | std_msgs::msg::Header img_header; 723 | { 724 | INFO("ReIDProc: Waiting for mutex to reid. "); 725 | std::unique_lock lk_body(body_results_.mtx); 726 | std::vector body_bboxes = BodyConvert(body_results_.body_infos.back()); 727 | img_show = body_results_.detection_img.img.clone(); 728 | img_header = body_results_.detection_img.header; 729 | if (-1 != 730 | reid_ptr_->GetReIDInfo( 731 | body_results_.detection_img.img, body_bboxes, person_id, tracked_bbox) && 732 | -1 != person_id) 733 | { 734 | INFO( 735 | "ReIDProc: Reid result, person id: %d, bbox: %d, %d, %d, %d", person_id, tracked_bbox.x, 736 | tracked_bbox.y, tracked_bbox.width, tracked_bbox.height); 737 | } 738 | if (reid_ptr_->GetLostStatus()) { 739 | processing_status_.status = TrackingStatusT::STATUS_SELECTING; 740 | } 741 | } 742 | 743 | // Storage reid result 744 | { 745 | std::lock(algo_proc_.mtx, result_mtx_); 746 | std::unique_lock lk_proc(algo_proc_.mtx, std::adopt_lock); 747 | std::unique_lock lk_result(result_mtx_, std::adopt_lock); 748 | reid_complated_ = true; 749 | Convert(img_header, tracked_bbox, algo_result_.track_res); 750 | SetThreadState("ReIDProc", algo_proc_.process_complated); 751 | INFO("ReIDProc: reid thread process_complated: %d", algo_proc_.process_complated); 752 | if (algo_proc_.process_complated) { 753 | INFO("ReIDProc: Reid thread notify to pub. "); 754 | algo_proc_.cond.notify_one(); 755 | } 756 | } 757 | } 758 | } 759 | 760 | void Convert(const std::vector & from, BodyInfoT & to) 761 | { 762 | for (size_t i = 0; i < from.size(); ++i) { 763 | to.infos[i].gesture.roi = Convert(from[i].rect); 764 | to.infos[i].gesture.cls = from[i].label; 765 | } 766 | } 767 | 768 | void VisionManager::GestureRecognize() 769 | { 770 | while (rclcpp::ok()) { 771 | { 772 | INFO("GestureRecognize: Wait to activate gesture thread. "); 773 | std::unique_lock lk(gesture_struct_.mtx); 774 | gesture_struct_.cond.wait(lk, [this] {return gesture_struct_.is_called;}); 775 | gesture_struct_.is_called = false; 776 | INFO("GestureRecognize: Activate gesture recognition thread. "); 777 | } 778 | if (!is_activate_) { 779 | INFO("GestureRecognize: Deactivate to return gesture thread. "); 780 | gesture_deactivated_ = true; 781 | return; 782 | } 783 | 784 | // Gesture recognition and get result 785 | bool is_success = false; 786 | std::vector infos; 787 | { 788 | std::unique_lock lk_body(body_results_.mtx); 789 | std::vector body_bboxes = BodyConvert(body_results_.body_infos.back()); 790 | if (-1 != gesture_ptr_->GetGestureInfo(body_results_.detection_img.img, body_bboxes, infos)) { 791 | is_success = true; 792 | } 793 | } 794 | 795 | // Storage gesture recognition result 796 | { 797 | std::lock(algo_proc_.mtx, result_mtx_); 798 | std::unique_lock lk_proc(algo_proc_.mtx, std::adopt_lock); 799 | std::unique_lock lk_result(result_mtx_, std::adopt_lock); 800 | gesture_complated_ = true; 801 | // Convert data to publish 802 | if (is_success) { 803 | Convert(infos, algo_result_.body_info); 804 | } 805 | SetThreadState("GestureRecognize", algo_proc_.process_complated); 806 | INFO("GestureRecognize: gesture thread process_complated: %d", algo_proc_.process_complated); 807 | if (algo_proc_.process_complated) { 808 | INFO("GestureRecognize: Gesture thread notify to pub. "); 809 | algo_proc_.cond.notify_one(); 810 | } 811 | } 812 | } 813 | } 814 | 815 | void Convert(const std::vector> & from, BodyInfoT & to) 816 | { 817 | for (size_t i = 0; i < from.size(); ++i) { 818 | to.infos[i].keypoints.clear(); 819 | for (size_t num = 0; num < from[i].size(); ++num) { 820 | KeypointT keypoint; 821 | keypoint.x = from[i][num].x; 822 | keypoint.y = from[i][num].y; 823 | to.infos[i].keypoints.push_back(keypoint); 824 | } 825 | } 826 | } 827 | 828 | void VisionManager::KeypointsDet() 829 | { 830 | while (rclcpp::ok()) { 831 | { 832 | INFO("KeypointsDet: Wait to activate keypoints thread. "); 833 | std::unique_lock lk(keypoints_struct_.mtx); 834 | keypoints_struct_.cond.wait(lk, [this] {return keypoints_struct_.is_called;}); 835 | keypoints_struct_.is_called = false; 836 | INFO("KeypointsDet: Activate keypoints detection thread. "); 837 | } 838 | if (!is_activate_) { 839 | INFO("KeypointsDet: Deactivate to return keypoints thread. "); 840 | keypoints_deactivated_ = true; 841 | return; 842 | } 843 | 844 | // Keypoints detection and get result 845 | std::vector> bodies_keypoints; 846 | { 847 | std::unique_lock lk_body(body_results_.mtx); 848 | std::vector body_bboxes = BodyConvert(body_results_.body_infos.back()); 849 | keypoints_ptr_->GetKeypointsInfo( 850 | body_results_.detection_img.img, body_bboxes, 851 | bodies_keypoints); 852 | } 853 | 854 | // Storage keypoints detection result 855 | { 856 | std::lock(algo_proc_.mtx, result_mtx_); 857 | std::unique_lock lk_proc(algo_proc_.mtx, std::adopt_lock); 858 | std::unique_lock lk_result(result_mtx_, std::adopt_lock); 859 | keypoints_complated_ = true; 860 | // Convert data to publish 861 | Convert(bodies_keypoints, algo_result_.body_info); 862 | SetThreadState("KeypointsDet", algo_proc_.process_complated); 863 | INFO("KeypointsDet: keypoints thread process_complated: %d", algo_proc_.process_complated); 864 | if (algo_proc_.process_complated) { 865 | INFO("KeypointsDet: Keypoints thread notify to pub. "); 866 | algo_proc_.cond.notify_one(); 867 | } 868 | } 869 | } 870 | } 871 | 872 | int VisionManager::LoadFaceLibrary(std::map> & library) 873 | { 874 | cv::FileStorage fs(std::string(kLibraryPath), cv::FileStorage::READ); 875 | if (!fs.isOpened()) { 876 | ERROR("Open the face library file fail! "); 877 | return -1; 878 | } 879 | 880 | library.erase(library.begin(), library.end()); 881 | cv::FileNode node = fs["UserFaceInfo"]; 882 | cv::FileNodeIterator it = node.begin(); 883 | for (; it != node.end(); ++it) { 884 | std::string name = (string)(*it)["name"]; 885 | cv::FileNode feat = (*it)["feature"]; 886 | cv::FileNodeIterator jt = feat.begin(); 887 | std::vector face_feat; 888 | for (; jt != feat.end(); ++jt) { 889 | face_feat.push_back(static_cast(*jt)); 890 | } 891 | library.insert(std::pair>(name, face_feat)); 892 | } 893 | 894 | return 0; 895 | } 896 | 897 | int VisionManager::GetMatchBody(const sensor_msgs::msg::RegionOfInterest & roi) 898 | { 899 | cv::Mat track_img; 900 | cv::Rect track_rect; 901 | bool is_found = false; 902 | for (size_t i = body_results_.body_infos.size() - 1; i > 0 && !is_found; --i) { 903 | double max_score = 0; 904 | for (size_t j = 0; j < body_results_.body_infos[i].size(); ++j) { 905 | double score = GetIOU(body_results_.body_infos[i][j], roi); 906 | if (score > max_score && score > 0.5) { 907 | max_score = score; 908 | track_rect = cv::Rect( 909 | body_results_.body_infos[i][j].left, 910 | body_results_.body_infos[i][j].top, 911 | body_results_.body_infos[i][j].width, 912 | body_results_.body_infos[i][j].height); 913 | } 914 | } 915 | if (max_score > 0.5) { 916 | is_found = true; 917 | track_img = body_results_.detection_img.img; 918 | } 919 | } 920 | if (is_found) { 921 | std::vector reid_feat; 922 | if (0 != reid_ptr_->SetTracker(track_img, track_rect, reid_feat)) { 923 | WARN("Set reid tracker fail. "); 924 | return -1; 925 | } 926 | } else { 927 | WARN("Can not find match body. "); 928 | return -1; 929 | } 930 | return 0; 931 | } 932 | 933 | void VisionManager::SetAlgoState(const AlgoListT & algo_list, const bool & value) 934 | { 935 | INFO("Algo type: %d", (int)algo_list.algo_module); 936 | switch (algo_list.algo_module) { 937 | case AlgoListT::ALGO_FACE: 938 | open_face_ = value; 939 | if (value) { 940 | LoadFaceLibrary(face_library_); 941 | } 942 | break; 943 | case AlgoListT::ALGO_BODY: 944 | open_body_ = value; 945 | break; 946 | case AlgoListT::ALGO_GESTURE: 947 | open_gesture_ = value; 948 | break; 949 | case AlgoListT::ALGO_KEYPOINTS: 950 | open_keypoints_ = value; 951 | break; 952 | case AlgoListT::ALGO_REID: 953 | open_reid_ = value; 954 | break; 955 | case AlgoListT::ALGO_FOCUS: 956 | open_focus_ = value; 957 | break; 958 | case AlgoListT::FACE_MANAGER: 959 | open_face_manager_ = value; 960 | if (value) { 961 | LoadFaceLibrary(face_library_); 962 | } 963 | default: 964 | break; 965 | } 966 | } 967 | 968 | void VisionManager::TrackingService( 969 | const std::shared_ptr, 970 | const std::shared_ptr req, 971 | std::shared_ptr res) 972 | { 973 | INFO( 974 | "Received tracking object from app: %d, %d, %d, %d", 975 | req->roi.x_offset, req->roi.y_offset, req->roi.width, req->roi.height); 976 | 977 | if (open_reid_) { 978 | std::unique_lock lk(body_results_.mtx); 979 | if (0 != GetMatchBody(req->roi)) { 980 | res->success = false; 981 | } else { 982 | res->success = true; 983 | processing_status_.status = TrackingStatusT::STATUS_TRACKING; 984 | } 985 | } 986 | 987 | if (open_focus_) { 988 | StampedImage stamped_img; 989 | { 990 | std::unique_lock lk(global_img_buf_.mtx); 991 | stamped_img = global_img_buf_.img_buf.back(); 992 | } 993 | cv::Rect rect = Convert(req->roi); 994 | if (!focus_ptr_->SetTracker(stamped_img.img, rect)) { 995 | WARN("Set focus tracker fail. "); 996 | res->success = false; 997 | } else { 998 | res->success = true; 999 | processing_status_.status = TrackingStatusT::STATUS_TRACKING; 1000 | } 1001 | } 1002 | } 1003 | 1004 | void VisionManager::AlgoManagerService( 1005 | const std::shared_ptr, 1006 | const std::shared_ptr req, 1007 | std::shared_ptr res) 1008 | { 1009 | INFO("Received algo request. "); 1010 | if (open_face_ || open_body_ || open_gesture_ || open_keypoints_ || open_reid_ || open_focus_ || 1011 | open_face_manager_) 1012 | { 1013 | WARN("Algo used was not reset, cannot select new algo."); 1014 | res->result_enable = AlgoManagerT::Response::ENABLE_FAIL; 1015 | res->result_disable = AlgoManagerT::Response::DISABLE_FAIL; 1016 | return; 1017 | } 1018 | 1019 | for (size_t i = 0; i < req->algo_enable.size(); ++i) { 1020 | SetAlgoState(req->algo_enable[i], true); 1021 | } 1022 | for (size_t i = 0; i < req->algo_disable.size(); ++i) { 1023 | SetAlgoState(req->algo_disable[i], false); 1024 | } 1025 | 1026 | res->result_enable = AlgoManagerT::Response::ENABLE_SUCCESS; 1027 | res->result_disable = AlgoManagerT::Response::DISABLE_SUCCESS; 1028 | } 1029 | 1030 | bool VisionManager::CallService( 1031 | rclcpp::Client::SharedPtr & client, 1032 | const uint8_t & cmd, const std::string & args) 1033 | { 1034 | auto req = std::make_shared(); 1035 | req->command = cmd; 1036 | req->args = args; 1037 | 1038 | std::chrono::seconds timeout = std::chrono::seconds(10); 1039 | if (!client->wait_for_service(timeout)) { 1040 | if (!rclcpp::ok()) { 1041 | ERROR("Interrupted while waiting for the service. Exiting."); 1042 | return false; 1043 | } 1044 | INFO("Service not available..."); 1045 | return false; 1046 | } 1047 | 1048 | auto client_cb = [timeout](rclcpp::Client::SharedFuture future) { 1049 | std::future_status status = future.wait_for(timeout); 1050 | 1051 | if (status == std::future_status::ready) { 1052 | if (0 != future.get()->result) { 1053 | return false; 1054 | } else { 1055 | return true; 1056 | } 1057 | } else { 1058 | return false; 1059 | } 1060 | }; 1061 | 1062 | auto result = client->async_send_request(req, client_cb); 1063 | return true; 1064 | } 1065 | 1066 | void VisionManager::FaceManagerService( 1067 | const std::shared_ptr, 1068 | const std::shared_ptr request, 1069 | std::shared_ptr response) 1070 | { 1071 | INFO( 1072 | "face service received command %d, argument '%s'", 1073 | request->command, request->args.c_str()); 1074 | 1075 | switch (request->command) { 1076 | case FaceManagerT::Request::ADD_FACE: 1077 | INFO( 1078 | "addFaceInfo: %s, is_host: %d", request->username.c_str(), 1079 | static_cast(request->ishost)); 1080 | if (request->username.length() == 0) { 1081 | response->result = -1; 1082 | } else { 1083 | FaceManager::getInstance()->addFaceIDCacheInfo(request->username, request->ishost); 1084 | face_detect_ = true; 1085 | std::thread faceDet = std::thread(&VisionManager::FaceDetProc, this, request->username); 1086 | faceDet.detach(); 1087 | response->result = 0; 1088 | } 1089 | break; 1090 | case FaceManagerT::Request::CANCLE_ADD_FACE: 1091 | INFO("cancelAddFace"); 1092 | face_detect_ = false; 1093 | response->result = FaceManager::getInstance()->cancelAddFace(); 1094 | break; 1095 | case FaceManagerT::Request::CONFIRM_LAST_FACE: 1096 | INFO( 1097 | "confirmFace username : %s, is_host: %d", request->username.c_str(), 1098 | static_cast(request->ishost)); 1099 | if (request->username.length() == 0) { 1100 | response->result = -1; 1101 | } else { 1102 | response->result = FaceManager::getInstance()->confirmFace( 1103 | request->username, 1104 | request->ishost); 1105 | } 1106 | break; 1107 | case FaceManagerT::Request::UPDATE_FACE_ID: 1108 | INFO( 1109 | "updateFaceId username : %s, is_host: %d", request->username.c_str(), 1110 | static_cast(request->ishost)); 1111 | if (request->username.length() == 0 || request->oriname.length() == 0) { 1112 | response->result = -1; 1113 | } else { 1114 | response->result = FaceManager::getInstance()->updateFaceId( 1115 | request->oriname, 1116 | request->username); 1117 | } 1118 | break; 1119 | case FaceManagerT::Request::DELETE_FACE: 1120 | INFO("deleteFace username: %s", request->username.c_str()); 1121 | if (request->username.length() == 0) { 1122 | response->result = -1; 1123 | } else { 1124 | response->result = FaceManager::getInstance()->deleteFace(request->username); 1125 | } 1126 | break; 1127 | case FaceManagerT::Request::GET_ALL_FACES: 1128 | response->msg = FaceManager::getInstance()->getAllFaces(); 1129 | INFO("getAllFaces : %s", response->msg.c_str()); 1130 | response->result = 0; 1131 | break; 1132 | default: 1133 | ERROR("service unsupport command %d", request->command); 1134 | response->result = FaceManagerT::Response::RESULT_INVALID_ARGS; 1135 | } 1136 | } 1137 | 1138 | void VisionManager::publishFaceResult( 1139 | int result, std::string & face_msg) 1140 | { 1141 | INFO("Publish face result. "); 1142 | auto face_result_msg = std::make_unique(); 1143 | face_result_msg->result = result; 1144 | face_result_msg->msg = face_msg; 1145 | face_result_pub_->publish(std::move(face_result_msg)); 1146 | } 1147 | 1148 | void VisionManager::FaceDetProc(std::string face_name) 1149 | { 1150 | std::map> endlib_feats; 1151 | std::vector match_info; 1152 | cv::Mat mat_tmp; 1153 | bool get_face_timeout = true; 1154 | std::string checkFacePose_Msg; 1155 | int checkFacePose_ret; 1156 | endlib_feats = FaceManager::getInstance()->getFeatures(); 1157 | std::time_t cur_time = std::time(NULL); 1158 | 1159 | while (face_ptr_ != nullptr && std::difftime(std::time(NULL), cur_time) < 40 && face_detect_) { 1160 | get_face_timeout = false; 1161 | std::unique_lock lk_img(global_img_buf_.mtx); 1162 | global_img_buf_.cond.wait(lk_img, [this] {return global_img_buf_.is_filled;}); 1163 | global_img_buf_.is_filled = false; 1164 | 1165 | std::vector faces_info; 1166 | mat_tmp = global_img_buf_.img_buf[0].img.clone(); 1167 | 1168 | face_ptr_->GetFaceInfo(mat_tmp, faces_info); 1169 | #if 0 1170 | // debug - visualization 1171 | for (unsigned int i = 0; i < faces_info.size(); i++) { 1172 | cv::rectangle( 1173 | mat_tmp, 1174 | cv::Rect( 1175 | faces_info[i].rect.left, faces_info[i].rect.top, 1176 | (faces_info[i].rect.right - faces_info[i].rect.left), 1177 | (faces_info[i].rect.bottom - faces_info[i].rect.top)), 1178 | cv::Scalar(0, 0, 255)); 1179 | } 1180 | cv::imshow("face", mat_tmp); 1181 | cv::waitKey(10); 1182 | #endif 1183 | checkFacePose_ret = FaceManager::getInstance()->checkFacePose(faces_info, checkFacePose_Msg); 1184 | if (checkFacePose_ret == 0) { 1185 | /*check if face feature already in endlib_feats*/ 1186 | face_ptr_->GetRecognitionResult(mat_tmp, endlib_feats, match_info); 1187 | if (match_info.size() > 0 && match_info[0].match_score > 0.65) { 1188 | checkFacePose_ret = 17; 1189 | face_name = match_info[0].face_id; 1190 | checkFacePose_Msg = "face already in endlib"; 1191 | ERROR( 1192 | "%s face already in endlib current score:%f", 1193 | face_name.c_str(), match_info[0].match_score); 1194 | } else { 1195 | FaceManager::getInstance()->addFaceFeatureCacheInfo(faces_info); 1196 | } 1197 | } 1198 | publishFaceResult(checkFacePose_ret, checkFacePose_Msg); 1199 | if (checkFacePose_ret == 0 || checkFacePose_ret == 17) { 1200 | break; 1201 | } 1202 | get_face_timeout = true; 1203 | } 1204 | 1205 | /*it time out publish error*/ 1206 | if (face_ptr_ != nullptr && get_face_timeout && face_detect_) { 1207 | checkFacePose_Msg = "timeout"; 1208 | publishFaceResult(3, checkFacePose_Msg); 1209 | } 1210 | } 1211 | 1212 | void VisionManager::SetThreadState(const std::string & thread_flag, bool & state) 1213 | { 1214 | INFO("%s: Face: %d", thread_flag.c_str(), !open_face_ || face_complated_); 1215 | INFO("%s: Body: %d", thread_flag.c_str(), !open_body_ || body_complated_); 1216 | INFO("%s: Focus: %d", thread_flag.c_str(), !open_focus_ || focus_complated_); 1217 | INFO("%s: Keypoints: %d", thread_flag.c_str(), !open_keypoints_ || keypoints_complated_); 1218 | INFO("%s: Gesture: %d", thread_flag.c_str(), !open_gesture_ || gesture_complated_); 1219 | INFO("%s: ReID: %d", thread_flag.c_str(), !open_reid_ || reid_complated_); 1220 | state = (!open_face_ || face_complated_) && (!open_body_ || body_complated_) && 1221 | (!open_gesture_ || gesture_complated_) && (!open_keypoints_ || keypoints_complated_) && 1222 | (!open_reid_ || reid_complated_) && (!open_focus_ || focus_complated_); 1223 | } 1224 | 1225 | void VisionManager::WakeThread(AlgoStruct & algo) 1226 | { 1227 | std::unique_lock lk(algo.mtx); 1228 | if (!algo.is_called) { 1229 | algo.is_called = true; 1230 | algo.cond.notify_one(); 1231 | } 1232 | } 1233 | 1234 | void VisionManager::ResetThread(AlgoStruct & algo) 1235 | { 1236 | std::unique_lock lk(algo.mtx); 1237 | algo.is_called = false; 1238 | } 1239 | 1240 | void VisionManager::ResetAlgo() 1241 | { 1242 | if (open_focus_) { 1243 | focus_ptr_->ResetTracker(); 1244 | } 1245 | if (open_reid_) { 1246 | reid_ptr_->ResetTracker(); 1247 | } 1248 | open_face_ = false; 1249 | open_body_ = false; 1250 | open_gesture_ = false; 1251 | open_keypoints_ = false; 1252 | open_reid_ = false; 1253 | open_focus_ = false; 1254 | open_face_manager_ = false; 1255 | main_algo_deactivated_ = false; 1256 | depend_deactivated_ = false; 1257 | body_deactivated_ = false; 1258 | face_deactivated_ = false; 1259 | focus_deactivated_ = false; 1260 | reid_deactivated_ = false; 1261 | gesture_deactivated_ = false; 1262 | keypoints_deactivated_ = false; 1263 | face_complated_ = false; 1264 | body_complated_ = false; 1265 | gesture_complated_ = false; 1266 | keypoints_complated_ = false; 1267 | reid_complated_ = false; 1268 | focus_complated_ = false; 1269 | ResetThread(body_struct_); 1270 | ResetThread(face_struct_); 1271 | ResetThread(focus_struct_); 1272 | ResetThread(reid_struct_); 1273 | ResetThread(gesture_struct_); 1274 | ResetThread(keypoints_struct_); 1275 | { 1276 | std::unique_lock lk_body(body_results_.mtx); 1277 | body_results_.is_filled = false; 1278 | } 1279 | { 1280 | std::unique_lock lk(global_img_buf_.mtx); 1281 | global_img_buf_.is_filled = false; 1282 | } 1283 | { 1284 | std::unique_lock lk_proc(algo_proc_.mtx); 1285 | algo_proc_.process_complated = false; 1286 | } 1287 | } 1288 | 1289 | void VisionManager::ResetCudaDevs() 1290 | { 1291 | body_ptr_.reset(); 1292 | face_ptr_.reset(); 1293 | focus_ptr_.reset(); 1294 | gesture_ptr_.reset(); 1295 | reid_ptr_.reset(); 1296 | keypoints_ptr_.reset(); 1297 | int dev_count = 0; 1298 | cudaSetDevice(dev_count); 1299 | cudaDeviceReset(); 1300 | INFO("Cuda device reset complated. "); 1301 | malloc_trim(0); 1302 | INFO("Malloc trim complated. "); 1303 | } 1304 | 1305 | void VisionManager::DestoryThread() 1306 | { 1307 | if (img_proc_thread_->joinable()) { 1308 | img_proc_thread_->join(); 1309 | INFO("img_proc_thread_ joined. "); 1310 | } 1311 | 1312 | if (open_gesture_ && gesture_thread_->joinable()) { 1313 | if (!gesture_deactivated_) { 1314 | WakeThread(gesture_struct_); 1315 | } 1316 | gesture_thread_->join(); 1317 | INFO("gesture_thread_ joined. "); 1318 | } 1319 | 1320 | if (open_reid_ && reid_thread_->joinable()) { 1321 | if (!reid_deactivated_) { 1322 | WakeThread(reid_struct_); 1323 | } 1324 | reid_thread_->join(); 1325 | INFO("reid_thread_ joined. "); 1326 | } 1327 | 1328 | if (open_keypoints_ && keypoints_thread_->joinable()) { 1329 | if (!keypoints_deactivated_) { 1330 | WakeThread(keypoints_struct_); 1331 | } 1332 | keypoints_thread_->join(); 1333 | INFO("keypoints_thread_ joined. "); 1334 | } 1335 | 1336 | if (open_face_ && face_thread_->joinable()) { 1337 | if (!face_deactivated_) { 1338 | WakeThread(face_struct_); 1339 | } 1340 | face_thread_->join(); 1341 | INFO("face_thread_ joined. "); 1342 | } 1343 | 1344 | if (open_focus_ && focus_thread_->joinable()) { 1345 | if (!focus_deactivated_) { 1346 | WakeThread(focus_struct_); 1347 | } 1348 | focus_thread_->join(); 1349 | INFO("focus_thread_ joined. "); 1350 | } 1351 | 1352 | if (open_body_ && body_det_thread_->joinable()) { 1353 | if (!body_deactivated_) { 1354 | WakeThread(body_struct_); 1355 | } 1356 | body_det_thread_->join(); 1357 | INFO("body_det_thread_ joined. "); 1358 | } 1359 | 1360 | if (open_reid_ || open_gesture_ || open_keypoints_) { 1361 | { 1362 | std::unique_lock lk_proc(algo_proc_.mtx); 1363 | algo_proc_.process_complated = true; 1364 | algo_proc_.cond.notify_one(); 1365 | INFO("Destory notify to pub. "); 1366 | } 1367 | 1368 | if (depend_manager_thread_->joinable()) { 1369 | if (!depend_deactivated_) { 1370 | std::unique_lock lk_body(body_results_.mtx); 1371 | if (!body_results_.is_filled) { 1372 | body_results_.is_filled = true; 1373 | body_results_.cond.notify_one(); 1374 | INFO("Destory notify depend thread. "); 1375 | } 1376 | } 1377 | depend_manager_thread_->join(); 1378 | INFO("depend_manager_thread_ joined. "); 1379 | } 1380 | } 1381 | 1382 | if (!open_face_manager_ && main_manager_thread_->joinable()) { 1383 | if (!main_algo_deactivated_) { 1384 | std::lock(global_img_buf_.mtx, algo_proc_.mtx); 1385 | std::unique_lock lk_img(global_img_buf_.mtx, std::adopt_lock); 1386 | std::unique_lock lk_proc(algo_proc_.mtx, std::adopt_lock); 1387 | if (!global_img_buf_.is_filled) { 1388 | global_img_buf_.is_filled = true; 1389 | global_img_buf_.cond.notify_one(); 1390 | INFO("Destory notify main thread. "); 1391 | } 1392 | if (!algo_proc_.process_complated) { 1393 | algo_proc_.process_complated = true; 1394 | algo_proc_.cond.notify_one(); 1395 | INFO("Destory notify main thread to pub. "); 1396 | } 1397 | } 1398 | main_manager_thread_->join(); 1399 | INFO("main_manager_thread_ joined. "); 1400 | } 1401 | INFO("Destory thread complated. "); 1402 | } 1403 | 1404 | VisionManager::~VisionManager() 1405 | { 1406 | DestoryThread(); 1407 | 1408 | if (0 == DetachShm(shm_addr_)) { 1409 | DelShm(shm_id_); 1410 | } 1411 | } 1412 | 1413 | } // namespace cyberdog_vision 1414 | --------------------------------------------------------------------------------