From 0a99fe0d7b1e03c19e8aab3d2e528b29aa3566da Mon Sep 17 00:00:00 2001 From: Victor Hogeweij Date: Fri, 31 Jan 2025 23:44:21 +0100 Subject: [PATCH] -add ros2 support to face mesh --- .github/workflows/code_compiles.yml | 31 +++++++ .github/workflows/ros2_code_compiles.yml | 38 ++++++++ CMakeLists.txt | 43 ++++++--- README.md | 53 ++++++++++- bindings/ros2/face_mesh_node.cpp | 63 ++++++------- example/ros2/face_mesh_viewer.cpp | 109 +++++++++++++++++++++++ example/ros2/launch.py | 12 +++ 7 files changed, 301 insertions(+), 48 deletions(-) create mode 100644 .github/workflows/code_compiles.yml create mode 100644 .github/workflows/ros2_code_compiles.yml create mode 100644 example/ros2/face_mesh_viewer.cpp create mode 100644 example/ros2/launch.py diff --git a/.github/workflows/code_compiles.yml b/.github/workflows/code_compiles.yml new file mode 100644 index 0000000..963c63b --- /dev/null +++ b/.github/workflows/code_compiles.yml @@ -0,0 +1,31 @@ +name: code_compiles + +on: + push: + branches: ["main"] + pull_request: + branches: ["main"] + workflow_dispatch: {} + +jobs: + build: + # The CMake configure and build commands are platform agnostic and should work equally well on Windows or Mac. + # You can convert this to a matrix build if you need cross-platform coverage. + # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix + runs-on: ubuntu-24.04 + + steps: + - uses: actions/checkout@v3 + + - name: Install prerequired packages + # To compile and run the code we require cmake, ninja and opencv + run: sudo apt-get update && sudo apt-get install build-essential cmake ninja-build libopencv-dev + + - name: Configure CMake + # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make. + # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type + run: cmake -B ${{github.workspace}}/build -G Ninja + + - name: Build + # Build your program with the given configuration + run: cmake --build ${{github.workspace}}/build diff --git a/.github/workflows/ros2_code_compiles.yml b/.github/workflows/ros2_code_compiles.yml new file mode 100644 index 0000000..b0ce685 --- /dev/null +++ b/.github/workflows/ros2_code_compiles.yml @@ -0,0 +1,38 @@ +name: ros_code_compiles + +on: + push: + branches: ["main"] + pull_request: + branches: ["main"] + workflow_dispatch: {} + +jobs: + build: + # The CMake configure and build commands are platform agnostic and should work equally well on Windows or Mac. + # You can convert this to a matrix build if you need cross-platform coverage. + # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix + runs-on: ubuntu-24.04 + + steps: + - uses: actions/checkout@v3 + - uses: ros-tooling/setup-ros@v0.7 + with: + required-ros-distributions: jazzy + + - name: Install prerequired packages + # To compile and run the code we require cmake, ninja and opencv + run: sudo apt-get update && sudo apt-get install build-essential cmake ninja-build libopencv-dev + + - name: Install dependencies + working-directory: ${{ github.workspace }} + run: | + sudo rosdep init || true + rosdep update + rosdep install --from-paths src -y --ignore-src + + - name: Colcon build + working-directory: ${{ github.workspace }} + run: | + source /opt/ros/jazzy/setup.bash + colcon build --packages-select face_detector diff --git a/CMakeLists.txt b/CMakeLists.txt index 2da8369..41676dc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,7 +8,7 @@ set(CLFML_FACE_MESH_MAIN_PROJECT OFF) option(CLFML_FACE_MESH_BUILD_EXAMPLE_PROJECTS "Build example projects" ON) # Optionally enable ROS2 package build (Requires ROS2 jazzy to be installed!) -option(CLFML_ROS2_PACKAGE_BUILD "Build a ROS2 package" OFF) +option(CLFML_ROS2_PACKAGE_BUILD "Build a ROS2 package" ON) if (CMAKE_CURRENT_SOURCE_DIR STREQUAL CMAKE_SOURCE_DIR) set(CMAKE_CXX_STANDARD 17) @@ -111,18 +111,6 @@ target_compile_definitions(${PROJECT_NAME} PUBLIC -DCLFML_FACE_MESH_CPU_MODEL_PA set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 17) -if(CLFML_FACE_MESH_BUILD_EXAMPLE_PROJECTS) - FetchContent_Declare( - face_detector.cpp - GIT_REPOSITORY https://github.com/CLFML/Face_Detector.Cpp.git - GIT_TAG main - ) - FetchContent_MakeAvailable(face_detector.cpp) - add_executable(face_mesh_demo ${CMAKE_CURRENT_LIST_DIR}/example/face_mesh_demo/demo.cpp) - target_link_libraries(face_mesh_demo PUBLIC CLFML::${PROJECT_NAME} CLFML::face_detector) - -endif() - if (CLFML_ROS2_PACKAGE_BUILD) find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) @@ -148,6 +136,35 @@ if (CLFML_ROS2_PACKAGE_BUILD) DESTINATION lib/${PROJECT_NAME}) endif() + +if(CLFML_FACE_MESH_BUILD_EXAMPLE_PROJECTS) +FetchContent_Declare( + face_detector.cpp + GIT_REPOSITORY https://github.com/CLFML/Face_Detector.Cpp.git + GIT_TAG main +) +FetchContent_MakeAvailable(face_detector.cpp) +if (CLFML_ROS2_PACKAGE_BUILD) + add_executable(face_mesh_viewer ${CMAKE_CURRENT_LIST_DIR}/example/ros2/face_mesh_viewer.cpp) + ament_target_dependencies(face_mesh_viewer + rclcpp + sensor_msgs + cv_bridge + std_msgs + geometry_msgs + std_srvs + OpenCV + ) + install(TARGETS + face_mesh_viewer + DESTINATION lib/${PROJECT_NAME}) + else () + add_executable(face_mesh_demo ${CMAKE_CURRENT_LIST_DIR}/example/face_mesh_demo/demo.cpp) + target_link_libraries(face_mesh_demo PUBLIC CLFML::${PROJECT_NAME} CLFML::face_detector) + endif() +endif() + + if (CLFML_ROS2_PACKAGE_BUILD) install(FILES bindings/ros2/set_ld_path.sh DESTINATION lib/${PROJECT_NAME}) ament_package() diff --git a/README.md b/README.md index 32e0719..e7c50a2 100644 --- a/README.md +++ b/README.md @@ -77,6 +77,58 @@ add_subdirectory(Face_Mesh.Cpp) ... target_link_libraries(YOUR_EXECUTABLE CLFML::face_mesh) ``` + +## Building a ROS2 package with Colcon +Before using this library you will need the following packages installed: +- OpenCV +- ROS2 +- ROS CV bridge +- Working C++ compiler (GCC or Clang) +- CMake + +### Running the examples (Ubuntu, CPU) + +1. Clone this repo: +``` +git clone https://github.com/CLFML/Face_Mesh.Cpp.git +``` + +2. Source your ROS2 installation: + +```bash +source /opt/ros/jazzy/setup.bash +``` + +3. Install the dependencies: +```bash +rosdep install --from-paths src -y --ignore-src +``` + +4. Build the package: + +```bash +colcon build --packages-select face_mesh +``` + +5. Set up the environment: + +```bash +source install/setup.bash +``` + +6. Run the camera node: + +```bash +ros2 run v4l2_camera v4l2_camera_node +``` + +7. In another terminal, run the nodes + +```bash +ros2 launch example/ros2/launch.py +``` + + ## Aditional documentation See our [wiki](https://clfml.github.io/Face_Mesh.Cpp/)... @@ -84,7 +136,6 @@ See our [wiki](https://clfml.github.io/Face_Mesh.Cpp/)... - Add language bindings for Python, C# and Java - Add support for MakeFiles and Bazel - Add Unit-tests -- Add ROS2 package support - Add support for the [Face Mesh V2 model](https://storage.googleapis.com/mediapipe-assets/Model%20Card%20MediaPipe%20Face%20Mesh%20V2.pdf) ## License diff --git a/bindings/ros2/face_mesh_node.cpp b/bindings/ros2/face_mesh_node.cpp index 867fcae..f28d514 100644 --- a/bindings/ros2/face_mesh_node.cpp +++ b/bindings/ros2/face_mesh_node.cpp @@ -20,12 +20,12 @@ * */ #include "rclcpp/rclcpp.hpp" -#include "sensor_msgs/msg/image.hpp" -#include "sensor_msgs/msg/region_of_interest.hpp" #include "cv_bridge/cv_bridge.hpp" #include "std_msgs/msg/int32.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" #include "sensor_msgs/point_cloud2_iterator.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "sensor_msgs/msg/region_of_interest.hpp" #include "std_srvs/srv/set_bool.hpp" #include "face_mesh.hpp" @@ -43,38 +43,27 @@ class FaceMeshNode : public rclcpp::Node private: void declare_and_get_parameters() { - declare_parameter("run_on_demand", false); - declare_parameter("trigger_topic", "/face_detected"); + declare_parameter("roi_topic", "/face_roi"); declare_parameter("camera_topic", "/image_raw"); declare_parameter("face_mesh_landmarks_topic", "/face_mesh_landmarks"); camera_topic_ = get_parameter("camera_topic").as_string(); face_landmarks_topic_ = get_parameter("face_mesh_landmarks_topic").as_string(); - on_demand_topic_ = get_parameter("trigger_topic").as_string(); - run_continuous_ = !get_parameter("run_on_demand").as_bool(); + roi_topic_ = get_parameter("roi_topic").as_string(); } void setup_communication() { - if (run_continuous_) - { - // Link process image straight to the incoming image callback - image_sub_ = create_subscription( - camera_topic_, rclcpp::SensorDataQoS(), - std::bind(&FaceMeshNode::image_callback, this, std::placeholders::_1)); - } - else - { - // Subscribe to the on-demand trigger topic - on_demand_sub_ = create_subscription( - on_demand_topic_, 10, - std::bind(&FaceMeshNode::on_demand_callback, this, std::placeholders::_1)); - - // Subscribe to the image topic but only store the latest image - image_sub_ = create_subscription( - camera_topic_, rclcpp::SensorDataQoS(), - std::bind(&FaceMeshNode::store_latest_image, this, std::placeholders::_1)); - } + // Subscribe to the on-demand trigger topic + on_demand_sub_ = create_subscription( + roi_topic_, 10, + std::bind(&FaceMeshNode::on_demand_callback, this, std::placeholders::_1)); + + // Subscribe to the image topic but only store the latest image + image_sub_ = create_subscription( + camera_topic_, rclcpp::SensorDataQoS(), + std::bind(&FaceMeshNode::store_latest_image, this, std::placeholders::_1)); + face_mesh_landmarks_pub_ = create_publisher(face_landmarks_topic_, 10); toggle_service_ = create_service( @@ -97,7 +86,9 @@ class FaceMeshNode : public rclcpp::Node RCLCPP_ERROR(get_logger(), "cv_bridge exception: %s", e.what()); return; } - det_.load_image(cv_ptr->image); + + cv::Mat cropped_image_to_roi = cv_ptr->image(roi_); + det_.load_image(cropped_image_to_roi, roi_); publish_face_mesh_landmarks(); } @@ -106,16 +97,20 @@ class FaceMeshNode : public rclcpp::Node last_image_msg_ = msg; // Store the latest image in memory } - void on_demand_callback(const std_msgs::msg::Int32::SharedPtr msg) + void on_demand_callback(const sensor_msgs::msg::RegionOfInterest::SharedPtr msg) { - if (!detection_enabled_ || msg->data == 0) + if (!detection_enabled_ || msg->width == 0 || msg->height == 0) return; - RCLCPP_INFO(get_logger(), "Trigger received! Processing latest image..."); - if (last_image_msg_) { - image_callback(last_image_msg_); // Process the last stored image + roi_ = cv::Rect(msg->x_offset, msg->y_offset, msg->width, msg->height); + if (roi_.x >= 0 && roi_.y >= 0 && + roi_.x + roi_.width <= last_image_msg_->width && + roi_.y + roi_.height <= last_image_msg_->height) + { + image_callback(last_image_msg_); // Process the last stored image + } } else { @@ -179,7 +174,7 @@ class FaceMeshNode : public rclcpp::Node } rclcpp::Subscription::SharedPtr image_sub_; - rclcpp::Subscription::SharedPtr on_demand_sub_; + rclcpp::Subscription::SharedPtr on_demand_sub_; rclcpp::Publisher::SharedPtr face_mesh_landmarks_pub_; rclcpp::Service::SharedPtr toggle_service_; @@ -188,9 +183,9 @@ class FaceMeshNode : public rclcpp::Node CLFML::FaceMesh::FaceMesh det_; std::string camera_topic_; std::string face_landmarks_topic_; - std::string on_demand_topic_; + std::string roi_topic_; + cv::Rect roi_ = cv::Rect(0, 0, 0, 0); bool detection_enabled_; - bool run_continuous_; }; int main(int argc, char **argv) diff --git a/example/ros2/face_mesh_viewer.cpp b/example/ros2/face_mesh_viewer.cpp new file mode 100644 index 0000000..d854359 --- /dev/null +++ b/example/ros2/face_mesh_viewer.cpp @@ -0,0 +1,109 @@ +#include "rclcpp/rclcpp.hpp" +#include "cv_bridge/cv_bridge.hpp" +#include "std_msgs/msg/int32.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "sensor_msgs/point_cloud2_iterator.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "std_srvs/srv/set_bool.hpp" +#include + +class FaceMeshViewer : public rclcpp::Node +{ +public: + FaceMeshViewer() : Node("face_mesh_viewer") + { + // Declare parameters for topic names + this->declare_parameter("camera_topic", "/image_raw"); + this->declare_parameter("face_landmarks_topic", "/face_mesh_landmarks"); + + // Get parameter values + std::string camera_topic = this->get_parameter("camera_topic").as_string(); + std::string face_landmarks_topic = this->get_parameter("face_landmarks_topic").as_string(); + + // Create subscriptions + image_sub_ = this->create_subscription( + camera_topic, 10, + std::bind(&FaceMeshViewer::image_callback, this, std::placeholders::_1)); + + face_landmarks_sub_ = this->create_subscription( + face_landmarks_topic, 10, + std::bind(&FaceMeshViewer::face_landmarks_callback, this, std::placeholders::_1)); + + // Create OpenCV window + cv::namedWindow("Face Mesh Viewer", cv::WINDOW_NORMAL); + cv::resizeWindow("Face Mesh Viewer", 800, 600); + } + + ~FaceMeshViewer() + { + cv::destroyAllWindows(); + } + +private: + void image_callback(const sensor_msgs::msg::Image::SharedPtr msg) + { + try + { + current_frame_ = cv_bridge::toCvShare(msg, "bgr8")->image.clone(); + update_display(); + } + catch (cv_bridge::Exception &e) + { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + } + } + + void face_landmarks_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) + { + if (msg->width * msg->height != face_landmarks_.size()) + { + RCLCPP_ERROR(this->get_logger(), "Unexpected face landmarks size: %d", msg->width * msg->height); + return; + } + + // Create iterators for PointCloud2 + sensor_msgs::PointCloud2ConstIterator iter_x(*msg, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(*msg, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(*msg, "z"); + + // Extract landmark points + for (size_t i = 0; i < face_landmarks_.size(); ++i, ++iter_x, ++iter_y, ++iter_z) + { + face_landmarks_[i] = cv::Point3f(*iter_x, *iter_y, *iter_z); + } + + update_display(); + } + + void update_display() + { + if (current_frame_.empty()) + return; + + cv::Mat display_frame = current_frame_.clone(); + + // Draw face landmarks + for (cv::Point3f keypoint : face_landmarks_) + { + cv::circle(display_frame, cv::Point(keypoint.x, keypoint.y), 2, cv::Scalar(0, 255, 0), -1); + } + + // Display the frame + cv::imshow("Face Mesh Viewer", display_frame); + cv::waitKey(1); + } + + rclcpp::Subscription::SharedPtr image_sub_; + rclcpp::Subscription::SharedPtr face_landmarks_sub_; + + cv::Mat current_frame_; + std::array face_landmarks_; +}; + +int main(int argc, char *argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/example/ros2/launch.py b/example/ros2/launch.py new file mode 100644 index 0000000..5704207 --- /dev/null +++ b/example/ros2/launch.py @@ -0,0 +1,12 @@ +from launch import LaunchDescription +import launch_ros.actions + +def generate_launch_description(): + return LaunchDescription([ + launch_ros.actions.Node( + namespace= "face_detector", package='face_detector', executable='face_detector_node', output='screen'), + launch_ros.actions.Node( + namespace= "face_mesh", package='face_mesh', executable='face_mesh_node', output='screen'), + launch_ros.actions.Node( + namespace= "face_mesh", package='face_mesh', executable='face_mesh_viewer', output='screen'), + ]) \ No newline at end of file