Feature/object detector with aruco marker

.clang-format

@@ -1,72 +1,3 @@
-# Config based on:
-# https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/blob/humble/.clang-format
-
----
-BasedOnStyle: Google
-AccessModifierOffset: -2
-ConstructorInitializerIndentWidth: 2
-AlignAfterOpenBracket: AlwaysBreak
-AlignEscapedNewlinesLeft: true
-AlignTrailingComments: true
-AllowAllArgumentsOnNextLine: false
-AllowAllParametersOfDeclarationOnNextLine: false
-PackConstructorInitializers: NextLine
-AllowShortFunctionsOnASingleLine: None
-AllowShortIfStatementsOnASingleLine: false
-AllowShortLoopsOnASingleLine: false
-AlwaysBreakBeforeMultilineStrings: false
-AlwaysBreakTemplateDeclarations: true
-BinPackArguments: false
-BinPackParameters: false
-BreakBeforeBinaryOperators: false
-BreakBeforeTernaryOperators: false
-BreakConstructorInitializersBeforeComma: true
+BasedOnStyle: Chromium
+SortIncludes: false
 ColumnLimit: 120
-ConstructorInitializerAllOnOneLineOrOnePerLine: true
-DerivePointerBinding: false
-PointerBindsToType: true
-ExperimentalAutoDetectBinPacking: false
-IndentCaseLabels: true
-MaxEmptyLinesToKeep: 1
-NamespaceIndentation: None
-ObjCSpaceBeforeProtocolList: true
-PenaltyBreakBeforeFirstCallParameter: 19
-PenaltyBreakComment: 60
-PenaltyBreakString: 1
-PenaltyBreakFirstLessLess: 1000
-PenaltyExcessCharacter: 1000
-PenaltyReturnTypeOnItsOwnLine: 90
-SpacesBeforeTrailingComments: 2
-Cpp11BracedListStyle: false
-Standard: Auto
-IndentWidth: 2
-TabWidth: 2
-UseTab: Never
-IndentFunctionDeclarationAfterType: false
-SpacesInParentheses: false
-SpacesInAngles: false
-SpaceInEmptyParentheses: false
-SpacesInCStyleCastParentheses: false
-SpacesInContainerLiterals: false
-SpaceAfterControlStatementKeyword: true
-SpaceBeforeAssignmentOperators: true
-ContinuationIndentWidth: 4
-SortIncludes: true
-SpaceAfterCStyleCast: false
-
-# Configure each individual brace in BraceWrapping
-BreakBeforeBraces: Custom
-
-# Control of individual brace wrapping cases
-BraceWrapping:
-  AfterCaseLabel: "false"
-  AfterClass: "false"
-  AfterControlStatement: Never
-  AfterEnum: "false"
-  AfterFunction: "false"
-  AfterNamespace: "false"
-  AfterStruct: "false"
-  AfterUnion: "false"
-  BeforeCatch: "false"
-  BeforeElse: "false"
-  IndentBraces: "false"

.pre-commit-config.yaml

@@ -47,7 +47,7 @@ repos:
     -   id: cmake-lint
 
 -   repo: https://github.com/pre-commit/mirrors-clang-format
-    rev: v22.1.1
+    rev: v22.1.2
     hooks:
     -   id: clang-format
         types_or: [c++, proto]

CHANGELOG.md

@@ -0,0 +1,18 @@
+# Changelog
+
+All notable changes to the `scene_objects_manager` package will be documented in this file.
+
+The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.1.0/),
+and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0.html).
+
+## [Unreleased]
+
+### Added
+
+* [PR-5](https://github.com/AGH-CEAI/scene_objects_manager/pull/5) - Add a service to get the pose of marked objects in the scene.
+
+### Changed
+### Deprecated
+### Removed
+### Fixed
+### Security

CMakeLists.txt

@@ -6,32 +6,71 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
+set(HW_IF_INCLUDE_DEPENDS
+    cv_bridge
+    geometry_msgs
+    moveit_msgs
+    moveit_ros_planning_interface
+    OpenCV
+    rosidl_default_generators
+    std_msgs
+    sensor_msgs
+    tf2
+    tf2_ros
+    tf2_geometry_msgs
+    yaml-cpp)
+
 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)
+
+foreach(dependency IN ITEMS ${HW_IF_INCLUDE_DEPENDS})
+  find_package(${dependency} REQUIRED)
+endforeach()
+
 include(FetchContent)
 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} moveit_ros_planning_interface rclcpp
-                          std_msgs)
+ament_target_dependencies(
+  ${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}
+  ${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} argparse yaml-cpp)
+target_link_libraries(${PROJECT_NAME}_node argparse yaml-cpp
+                      ${OpenCV_LIBRARIES})
 
-install(TARGETS ${PROJECT_NAME} DESTINATION lib/${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.

include/scene_objects_manager/args_parser.hpp

@@ -14,10 +14,9 @@ struct LaunchArguments {
 
 bool validate_launch_arguments(const LaunchArguments& args);
 
-const LaunchArguments parse_args(
-    const std::string& program_name,
-    const std::string& program_version,
-    const std::vector<std::string>& input_args);
+const LaunchArguments parse_args(const std::string& program_name,
+                                 const std::string& program_version,
+                                 const std::vector<std::string>& input_args);
 
 }  // namespace sobjmanager
 #endif  // SCENE_OBJECTS_MANAGER__ARGS_PARSER_HPP_

include/scene_objects_manager/object_pose_detector.hpp

@@ -0,0 +1,73 @@
+#ifndef SCENE_OBJECTS_MANAGER__OBJECT_POSE_DETECTOR_HPP_
+#define SCENE_OBJECTS_MANAGER__OBJECT_POSE_DETECTOR_HPP_
+
+#include <cmath>
+#include <fstream>
+#include <mutex>
+#include <optional>
+#include <stdexcept>
+#include <vector>
+
+#include <opencv2/aruco.hpp>
+#include <opencv2/core.hpp>
+#include <opencv2/opencv.hpp>
+#include <yaml-cpp/yaml.h>
+
+#include <cv_bridge/cv_bridge.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <ament_index_cpp/get_package_share_directory.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/pose_array.hpp>
+#include <scene_objects_manager/srv/detect_blocks_poses.hpp>
+#include <sensor_msgs/msg/camera_info.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+using DetectBlocksPosesSrv = scene_objects_manager::srv::DetectBlocksPoses;
+
+namespace sobjmanager {
+
+class ObjectPoseDetectorNode : public rclcpp::Node {
+ public:
+  ObjectPoseDetectorNode();
+
+ private:
+  void image_cb(const sensor_msgs::msg::Image::SharedPtr msg);
+
+  void on_detect(const std::shared_ptr<DetectBlocksPosesSrv::Request>,
+                 std::shared_ptr<DetectBlocksPosesSrv::Response> res);
+
+  void camera_info_cb(const sensor_msgs::msg::CameraInfo::SharedPtr msg);
+
+  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
+  rclcpp::Service<scene_objects_manager::srv::DetectBlocksPoses>::SharedPtr detect_blocks_srv_;
+  rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr cam_info_sub_;
+
+  std::mutex mtx_;
+  std::optional<cv::Mat> last_rgb_;
+  rclcpp::Time last_rgb_stamp_;
+  bool camera_info_received_;
+  float z_offset_of_block_;
+
+  std::string image_topic_;
+  std::string cam_info_topic_;
+  std::string cam_frame_;
+  std::string output_frame_;
+  double aruco_size_;
+
+  cv::Mat camera_matrix_;
+  cv::Mat dist_coeffs_;
+
+  std::vector<cv::Vec3d> rvecs_;
+  std::vector<cv::Vec3d> tvecs_;
+
+  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+};
+
+}  // namespace sobjmanager
+#endif  // SCENE_OBJECTS_MANAGER__OBJECT_POSE_DETECTOR_HPP_

include/scene_objects_manager/scene_object.hpp

@@ -25,8 +25,8 @@ struct SceneObject {
   Pose pose;
 
   static const inline std::map<std::string, uint8_t> PRIMITIVE_STR_MAP = {
-    { "box", SolidPrimitive::BOX },   { "sphere", SolidPrimitive::SPHERE }, { "cylinder", SolidPrimitive::CYLINDER },
-    { "cone", SolidPrimitive::CONE }, { "prism", SolidPrimitive::PRISM },
+      {"box", SolidPrimitive::BOX},   {"sphere", SolidPrimitive::SPHERE}, {"cylinder", SolidPrimitive::CYLINDER},
+      {"cone", SolidPrimitive::CONE}, {"prism", SolidPrimitive::PRISM},
   };
   static const inline std::map<uint8_t, std::string> PRIMITIVE_UINT_MAP = reverse_map(PRIMITIVE_STR_MAP);
 };

include/scene_objects_manager/scene_objects_manager.hpp

@@ -12,8 +12,8 @@
 namespace sobjmanager {
 
 class SceneObjectsManager : public rclcpp::Node {
-public:
-  SceneObjectsManager() : Node("SceneObjectsManager") {};
+ public:
+  SceneObjectsManager() : Node("scene_objects_manager") {};
 };
 
 }  // namespace sobjmanager

include/scene_objects_manager/spawner.hpp

@@ -11,12 +11,12 @@ using CollisionObject = moveit_msgs::msg::CollisionObject;
 namespace sobjmanager {
 
 class Spawner {
-public:
+ public:
   Spawner(std::string planning_frame_id);
 
   void spawn_object(const SceneObject& obj);
 
-private:
+ private:
   static CollisionObject make_collision_object(const SceneObject& raw_obj);
   void spawn(const CollisionObject& object);
 

package.xml

@@ -5,16 +5,28 @@
   <version>0.0.0</version>
   <description>Node for managing objects within MoveIt2 PlanningSceneInterface.</description>
   <maintainer email="mac.aleksandrowicz@gmail.com">Maciej Aleksandrowicz</maintainer>
+  <maintainer email="antonio.radominski@gmail.com">Antoni Radomiński-Lasek</maintainer>
   <license>Apache-2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>cv_bridge</depend>
+  <depend>geometry_msgs</depend>
   <depend>moveit_msgs</depend>
   <depend>moveit_ros_planning_interface</depend>
   <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
   <depend>yaml-cpp</depend>
 
+  <build_depend>rosidl_default_generators</build_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

src/args_parser.cpp

@@ -13,10 +13,9 @@ bool validate_launch_arguments(const LaunchArguments& args) {
   return true;
 }
 
-const LaunchArguments parse_args(
-    const std::string& program_name,
-    const std::string& program_version,
-    const std::vector<std::string>& input_args) {
+const LaunchArguments parse_args(const std::string& program_name,
+                                 const std::string& program_version,
+                                 const std::vector<std::string>& input_args) {
   argparse::ArgumentParser program(program_name, program_version, argparse::default_arguments::help);
   LaunchArguments args;