Feature/object detector with aruco marker

.pre-commit-config.yaml

@@ -1,159 +1,62 @@
-# Config based on:
-# https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/blob/humble/.pre-commit-config.yaml
-# To use:
-#
-#     pre-commit run -a
-#
-# Or:
-#
-#     pre-commit install  # (runs every time you commit in git)
-#
-# To update this file:
-#
-#     pre-commit autoupdate
-#
-# See https://github.com/pre-commit/pre-commit
-
 repos:
-  # Standard hooks
-  - repo: https://github.com/pre-commit/pre-commit-hooks
-    rev: v4.5.0
-    hooks:
-      - id: check-added-large-files
-      - id: check-ast
-      - id: check-byte-order-marker # Forbid UTF-8 byte-order markers
-      - id: check-case-conflict
-      - id: check-docstring-first
-      - id: check-merge-conflict
-      - id: check-symlinks
-      - id: check-xml
-      - id: check-yaml
-      - id: debug-statements
-      - id: destroyed-symlinks
-      - id: detect-private-key
-      - id: end-of-file-fixer
-      - id: mixed-line-ending
-      - id: trailing-whitespace
-
-  # Python hooks
-  - repo: https://github.com/asottile/pyupgrade
-    rev: v3.15.2
-    hooks:
-      - id: pyupgrade
-        args: [--py310-plus]
-
-  - repo: https://github.com/psf/black
-    rev: 22.3.0
-    hooks:
-      - id: black
-        args: ["--line-length=100"]
-
-  - repo: https://github.com/pycqa/pydocstyle
-    rev: 6.1.1
-    hooks:
-      - id: pydocstyle
-        args:
-          [
-            "--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D401,D404",
-          ]
-
-  - repo: https://github.com/pycqa/flake8
-    rev: 3.9.0
-    hooks:
-      - id: flake8
-        args: ["--ignore=E501,W503"]
-
-  # CPP hooks
-  # The same options as in ament_cppcheck are used, but its not working...
-  #- repo: https://github.com/pocc/pre-commit-hooks
-  #rev: v1.1.1
-  #hooks:
-  #- id: cppcheck
-  #args: ['--error-exitcode=1', '-f', '--inline-suppr', '-q', '-rp', '--suppress=internalAstError', '--suppress=unknownMacro', '--verbose']
-
-  - repo: local
-    hooks:
-      - id: ament_cppcheck
-        name: ament_cppcheck
-        description: Static code analysis of C/C++ files.
-        stages: [commit]
-        entry: env AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=1 ament_cppcheck
-        language: system
-        files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$
-
-  # Maybe use https://github.com/cpplint/cpplint instead
-  # TODO(macale) run it before release
-  # - repo: local
-  #   hooks:
-  #     - id: ament_cpplint
-  #       name: ament_cpplint
-  #       description: Static code analysis of C/C++ files.
-  #       stages: [commit]
-  #       entry: ament_cpplint
-  #       language: system
-  #       files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$
-  #       args: ["--linelength=120"]
-
-  - repo: local
-    hooks:
-      - id: clang-format
-        name: clang-format
-        description: Format files with ClangFormat.
-        entry: clang-format-14
-        language: system
-        files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$
-        args: ["-fallback-style=none", "-i"]
-
-  # Cmake hooks
-  - repo: https://github.com/cheshirekow/cmake-format-precommit
-    rev: v0.6.10
-    hooks:
-    # Note: Do not use format - the wrapping function is bugged
-    # https://github.com/cheshirekow/cmake_format/issues/180
-    - id: cmake-lint
-      args: ["--disable", "C0103"]
-  - repo: local
-    hooks:
-      - id: ament_lint_cmake
-        name: ament_lint_cmake
-        description: Check format of CMakeLists.txt files.
-        stages: [commit]
-        entry: ament_lint_cmake
-        args: ["--filter=-readability/wonkycase"]
-        language: system
-        files: CMakeLists.txt$
-
-  # Copyright
-  # TODO: turn on on public code release
-  # - repo: local
-  #   hooks:
-  #     - id: ament_copyright
-  #       name: ament_copyright
-  #       description: Check if copyright notice is available in all files.
-  #       stages: [commit]
-  #       entry: ament_copyright
-  #       language: system
-  #       args: ['--exclude', 'ur_robot_driver/doc/conf.py']
-
-  # Docs - RestructuredText hooks
-  - repo: https://github.com/PyCQA/doc8
-    rev: 0.9.0a1
-    hooks:
-      - id: doc8
-        args: ["--max-line-length=100", "--ignore=D001"]
-
-  - repo: https://github.com/pre-commit/pygrep-hooks
-    rev: v1.8.0
-    hooks:
-      - id: rst-backticks
-      - id: rst-directive-colons
-      - id: rst-inline-touching-normal
-
-  # Spellcheck in comments and docs
-  # skipping of *.svg files is not working...
-  - repo: https://github.com/codespell-project/codespell
-    rev: v2.0.0
-    hooks:
-      - id: codespell
-        args: ["--write-changes"]
-        exclude: \.(svg|pyc|drawio)$
+-   repo: https://gitlab.com/vojko.pribudic.foss/pre-commit-update
+    rev: v0.9.0
+    hooks:
+    -   id: pre-commit-update
+
+-   repo: https://github.com/pre-commit/pre-commit-hooks
+    rev: v6.0.0
+    hooks:
+    -   id: check-added-large-files
+    -   id: check-ast
+    -   id: check-case-conflict
+    -   id: check-executables-have-shebangs
+    -   id: check-json
+    -   id: check-merge-conflict
+    -   id: check-symlinks
+    -   id: check-toml
+    -   id: check-xml
+    -   id: debug-statements
+    -   id: destroyed-symlinks
+    -   id: detect-private-key
+    -   id: end-of-file-fixer
+    -   id: fix-byte-order-marker
+    -   id: mixed-line-ending
+    -   id: trailing-whitespace
+    -   id: check-yaml
+        exclude: joint_limits.yaml     # uses custom macro for deg<->rad transformation
+    -   id: pretty-format-json
+        args: ["--autofix", "--no-sort-keys", "--indent", "2"]
+
+-   repo: https://github.com/codespell-project/codespell
+    rev: v2.4.2
+    hooks:
+    -   id: codespell
+        args: [-w]
+
+-   repo: https://github.com/astral-sh/ruff-pre-commit
+    rev: v0.15.7
+    hooks:
+    -   id: ruff-check
+        args: [--fix, --exit-non-zero-on-fix]
+    -   id: ruff-format
+
+-   repo: https://github.com/cheshirekow/cmake-format-precommit
+    rev: v0.6.13
+    hooks:
+    -   id: cmake-format
+    -   id: cmake-lint
+
+-   repo: https://github.com/pre-commit/mirrors-clang-format
+    rev: v22.1.1
+    hooks:
+    -   id: clang-format
+        types_or: [c++, proto]
+
+-   repo: https://github.com/tier4/pre-commit-hooks-ros
+    rev: v0.10.2
+    hooks:
+    -   id: prettier-xacro
+    -   id: prettier-package-xml
+    -   id: ros-include-guard
+    -   id: sort-package-xml

CMakeLists.txt

@@ -7,57 +7,66 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 endif()
 
 find_package(ament_cmake REQUIRED)
+find_package(ament_index_cpp REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(moveit_msgs REQUIRED)
 find_package(moveit_ros_planning_interface REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(yaml-cpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(cv_bridge REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+
 include(FetchContent)
-FetchContent_Declare(
-  argparse
-  GIT_REPOSITORY https://github.com/p-ranav/argparse.git
-)
+FetchContent_Declare(argparse
+                     GIT_REPOSITORY https://github.com/p-ranav/argparse.git)
 FetchContent_MakeAvailable(argparse)
 
-set(
-  SOURCE_FILES
-  src/args_parser.cpp
-  src/scene_objects_manager.cpp
-  src/spawner.cpp
-  src/yaml_parser.cpp
-)
+set(SOURCE_FILES
+    src/args_parser.cpp src/scene_objects_manager.cpp src/spawner.cpp
+    src/yaml_parser.cpp src/object_pose_detector.cpp)
+
+rosidl_generate_interfaces(${PROJECT_NAME} "srv/DetectBlocksPoses.srv"
+                           DEPENDENCIES geometry_msgs)
+
+add_executable(${PROJECT_NAME}_node ${SOURCE_FILES})
+
+rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME}
+                              "rosidl_typesupport_cpp")
+
+target_link_libraries(${PROJECT_NAME}_node ${cpp_typesupport_target})
 
-add_executable(${PROJECT_NAME} ${SOURCE_FILES})
 ament_target_dependencies(
-  ${PROJECT_NAME}
+  ${PROJECT_NAME}_node
+  ament_index_cpp
   moveit_ros_planning_interface
   rclcpp
   std_msgs
-)
+  rclcpp
+  sensor_msgs
+  geometry_msgs
+  cv_bridge
+  tf2
+  tf2_ros
+  tf2_geometry_msgs)
 
 target_include_directories(
-  ${PROJECT_NAME} PUBLIC
-  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  ${argparse_SOURCE_DIR}/argparse/include
-)
-
-target_link_libraries(
-  ${PROJECT_NAME}
-  argparse
-  yaml-cpp
-)
-
-install(
-  TARGETS ${PROJECT_NAME}
-  DESTINATION lib/${PROJECT_NAME}
-)
-
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  set(ament_cmake_copyright_FOUND TRUE)
-  set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
-endif()
+  ${PROJECT_NAME}_node
+  PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+         "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+         ${argparse_SOURCE_DIR}/argparse/include)
+
+target_link_libraries(${PROJECT_NAME}_node argparse yaml-cpp
+                      ${OpenCV_LIBRARIES})
+
+install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
+
+install(TARGETS ${PROJECT_NAME}_node DESTINATION lib/${PROJECT_NAME})
 
+ament_export_dependencies(rosidl_default_runtime)
 ament_package()

README.md

@@ -17,5 +17,25 @@ Check the following messages definitions:
 | `shape_msgs/msg/SolidPrimitive.msg`      | [link](https://github.com/ros2/common_interfaces/blob/humble/shape_msgs/msg/SolidPrimitive.msg) |
 
 
+## Block detection service (based on ArUco markers)
+This service detects blocks marked with ArUco markers using scene camera and returns their poses.
+
+### Service description
+The `/detect_blocks_poses` service provides the poses of all detected blocks in the scene.
+- Service name: `detect_blocks_poses`
+- Service type: `scene_objects_manager/srv/DetectBlocksPoses`
+- Response: `geometry_msgs/msg/PoseArray`
+
+The response contains a list of poses (`PoseArray`), where each pose corresponds to a detected block in the camera frame.
+
+### Example usage:
+```bash
+ros2 service call /detect_blocks_poses scene_objects_manager/srv/DetectBlocksPoses "{detect: true}"
+```
+
 ## Libraries used
 - [argparse](https://github.com/p-ranav/argparse) (MIT License)
+
+## Tests
+- `detect_all_markers.py` - is publishing static transform to the tf of detected blocks.
+- `go_above_markers.py` - is moving the robot above the detected markers.

config/object_pose_detector_config.yaml

@@ -0,0 +1,7 @@
+object_pose_detector:
+  ros__parameters:
+    aruco_size: 0.02
+    image_topic: "/cam_scene/rgb/image_rect"
+    cam_info_topic: "/cam_scene/rgb/camera_info"
+    output_frame: "base_link"
+    cam_frame: "cam_scene_rgb_camera_optical_frame_cal"