From 0f37fa3f4fb0142974a50df2381af5df7d95d4e2 Mon Sep 17 00:00:00 2001 From: antrad Date: Fri, 27 Feb 2026 16:54:19 +0100 Subject: [PATCH 01/35] service setup --- .pre-commit-config.yaml | 73 ++++++++++--------- CMakeLists.txt | 37 ++++++++-- .../object_pose_detector.hpp | 43 +++++++++++ .../scene_objects_manager.hpp | 2 +- package.xml | 7 ++ src/object_pose_detector.cpp | 52 +++++++++++++ src/scene_objects_manager.cpp | 19 +++-- srv/DetectBlocksPoses.srv | 6 ++ 8 files changed, 193 insertions(+), 46 deletions(-) create mode 100644 include/scene_objects_manager/object_pose_detector.hpp create mode 100644 src/object_pose_detector.cpp create mode 100644 srv/DetectBlocksPoses.srv diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index fc71b55..30d52c2 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -71,15 +71,16 @@ repos: #- 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)$ +# todo make it alaive + # - repo: local + # hooks: + # - id: ament_cppcheck + # name: ament_cppcheck + # description: Static code analysis of C/C++ files. + # stages: [pre-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 @@ -94,34 +95,36 @@ repos: # 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"] +# todo make it alaive + # - 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"] +# make it alaive # 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$ + # - 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: [pre-commit] + # entry: ament_lint_cmake + # args: ["--filter=-readability/wonkycase"] + # language: system + # files: CMakeLists.txt$ # Copyright # TODO: turn on on public code release diff --git a/CMakeLists.txt b/CMakeLists.txt index 8a87fb8..eecd7be 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,6 +12,12 @@ 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) + include(FetchContent) FetchContent_Declare( argparse @@ -25,31 +31,51 @@ set( 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 moveit_ros_planning_interface rclcpp std_msgs + rclcpp + sensor_msgs + geometry_msgs + cv_bridge ) target_include_directories( - ${PROJECT_NAME} PUBLIC + ${PROJECT_NAME}_node PUBLIC "$" "$" ${argparse_SOURCE_DIR}/argparse/include ) target_link_libraries( - ${PROJECT_NAME} + ${PROJECT_NAME}_node argparse yaml-cpp + ${OpenCV_LIBRARIES} ) install( - TARGETS ${PROJECT_NAME} + TARGETS ${PROJECT_NAME}_node DESTINATION lib/${PROJECT_NAME} ) @@ -60,4 +86,5 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() +ament_export_dependencies(rosidl_default_runtime) ament_package() diff --git a/include/scene_objects_manager/object_pose_detector.hpp b/include/scene_objects_manager/object_pose_detector.hpp new file mode 100644 index 0000000..b58b06e --- /dev/null +++ b/include/scene_objects_manager/object_pose_detector.hpp @@ -0,0 +1,43 @@ +#ifndef OBJECT_POSE_DETECTOR_NODE_HPP_INCLUDED +#define OBJECT_POSE_DETECTOR_NODE_HPP_INCLUDED + +#include +#include + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "geometry_msgs/msg/pose_array.hpp" + +#include "scene_objects_manager/srv/detect_blocks_poses.hpp" + + +namespace sobjmanager { + +class ObjectPoseDetectorNode : public rclcpp::Node +{ +public: + ObjectPoseDetectorNode(); + +private: + void imageCb(const sensor_msgs::msg::Image::SharedPtr msg); + + void onDetect( + const std::shared_ptr req, + std::shared_ptr res); + + std::mutex mtx_; + std::optional last_bgr_; + rclcpp::Time last_stamp_; + + rclcpp::Subscription::SharedPtr sub_; + rclcpp::Service::SharedPtr srv_; + + std::string image_topic_; + std::string output_frame_; +}; + +} // namespace sobjmanager +#endif // OBJECT_POSE_DETECTOR_NODE_HPP_INCLUDED diff --git a/include/scene_objects_manager/scene_objects_manager.hpp b/include/scene_objects_manager/scene_objects_manager.hpp index 9a1b461..84b9da7 100644 --- a/include/scene_objects_manager/scene_objects_manager.hpp +++ b/include/scene_objects_manager/scene_objects_manager.hpp @@ -13,7 +13,7 @@ namespace sobjmanager { class SceneObjectsManager : public rclcpp::Node { public: - SceneObjectsManager() : Node("SceneObjectsManager"){}; + SceneObjectsManager() : Node("scene_objects_manager"){}; }; } // namespace sobjmanager diff --git a/package.xml b/package.xml index fd4680e..ac8de43 100644 --- a/package.xml +++ b/package.xml @@ -14,7 +14,14 @@ rclcpp std_msgs yaml-cpp + sensor_msgs + geometry_msgs + cv_bridge + rosidl_default_generators + rosidl_default_runtime + + rosidl_interface_packages ament_lint_auto ament_lint_common diff --git a/src/object_pose_detector.cpp b/src/object_pose_detector.cpp new file mode 100644 index 0000000..2eda21a --- /dev/null +++ b/src/object_pose_detector.cpp @@ -0,0 +1,52 @@ +#include "scene_objects_manager/object_pose_detector.hpp" + +namespace sobjmanager { + +ObjectPoseDetectorNode::ObjectPoseDetectorNode() +: rclcpp::Node("object_pose_detector") +{ + image_topic_ = this->declare_parameter("image_topic", "/cam_scene/rgb/image_raw"); + output_frame_ = this->declare_parameter("output_frame", "camera_frame"); + + sub_ = this->create_subscription( + image_topic_, + rclcpp::SensorDataQoS(), + std::bind(&ObjectPoseDetectorNode::imageCb, this, std::placeholders::_1) + ); + + srv_ = this->create_service( + "detect_blocs_poses", + std::bind(&ObjectPoseDetectorNode::onDetect, this, std::placeholders::_1, std::placeholders::_2) + ); + + RCLCPP_INFO(this->get_logger(), "Detector node started. Subscriptionto: %s", image_topic_.c_str()); + RCLCPP_INFO(this->get_logger(), "Service ready: /detect_blocs_poses"); +} + +void ObjectPoseDetectorNode::imageCb(const sensor_msgs::msg::Image::SharedPtr msg) +{ + v_bridge::CvImageConstPtr cv_ptr; + try { + cv_ptr = cv_bridge::toCvShare(msg, "bgr8"); + } catch (const cv_bridge::Exception & e) { + RCLCPP_WARN(this->get_logger(), "cv_bridge exception: %s", e.what()); + return; + } + + { + std::lock_guard lock(mtx_); + last_bgr_ = cv_ptr->image.clone(); + last_stamp_ = msg->header.stamp; + } + RCLCPP_DEBUG(this->get_logger(), "Stored image %dx%d", cv_ptr->image.cols, cv_ptr->image.rows); +} + +void ObjectPoseDetectorNode::onDetect( + const std::shared_ptr, + std::shared_ptr res +) +{ + RCLCPP_INFO(this->get_logger(), "make the request"); +} + +} // namespace sobjmanager diff --git a/src/scene_objects_manager.cpp b/src/scene_objects_manager.cpp index 7c57eba..83a7c4d 100644 --- a/src/scene_objects_manager.cpp +++ b/src/scene_objects_manager.cpp @@ -1,4 +1,5 @@ #include "scene_objects_manager/scene_objects_manager.hpp" +#include "scene_objects_manager/object_pose_detector.hpp" #include "scene_objects_manager/args_parser.hpp" #include "scene_objects_manager/spawner.hpp" @@ -11,11 +12,14 @@ int main(int argc, char* argv[]) { std::vector args = rclcpp::init_and_remove_ros_arguments(argc, argv); return parse_args("SceneObjectsSpawner", "0.0.0", args); }(); - auto const node = std::make_shared(); + + // auto const node = std::make_shared(); + auto manager_node = std::make_shared(); + auto detector_node = std::make_shared(); auto scene_objs = load_scene_objects_from_yaml(launch_args.cfg_path); RCLCPP_INFO( - node->get_logger(), + manager_node->get_logger(), "Loaded %d object(s) from the configuration file.", static_cast(scene_objs.size())); @@ -23,14 +27,19 @@ int main(int argc, char* argv[]) { for (const auto& obj : scene_objs) { spawner.spawn_object(obj); RCLCPP_INFO( - node->get_logger(), + manager_node->get_logger(), "Spawned new scene object \"%s\" (unique_id: %s).", obj.pretty_name.c_str(), obj.unique_id.c_str()); } - RCLCPP_INFO(node->get_logger(), "Spawned all objects."); + RCLCPP_INFO(manager_node->get_logger(), "Spawned all objects."); + + rclcpp::executors::MultiThreadedExecutor executor_; + executor_.add_node(manager_node); + executor_.add_node(detector_node); + executor_.spin(); - rclcpp::spin(node); + // rclcpp::spin(node); rclcpp::shutdown(); return 0; } diff --git a/srv/DetectBlocksPoses.srv b/srv/DetectBlocksPoses.srv new file mode 100644 index 0000000..1d06b30 --- /dev/null +++ b/srv/DetectBlocksPoses.srv @@ -0,0 +1,6 @@ +# Request +bool start_detection + +--- +# Response +geometry_msgs/PoseArray poses From 23dd19f575fbbe7eb20436f1ed29f1622bb1f35a Mon Sep 17 00:00:00 2001 From: antrad Date: Tue, 3 Mar 2026 12:25:03 +0100 Subject: [PATCH 02/35] updated pre-commit --- .pre-commit-config.yaml | 222 +++++++++++----------------------------- 1 file changed, 61 insertions(+), 161 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 30d52c2..4cbc075 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,162 +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'] - -# todo make it alaive - # - repo: local - # hooks: - # - id: ament_cppcheck - # name: ament_cppcheck - # description: Static code analysis of C/C++ files. - # stages: [pre-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"] - -# todo make it alaive - # - 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"] - -# make it alaive - # 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: [pre-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.1 + hooks: + - id: codespell + args: [-w] + +- repo: https://github.com/astral-sh/ruff-pre-commit + rev: v0.15.4 + 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.0 + 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 From caca4583bae7afa009817825429e8b1b894370ef Mon Sep 17 00:00:00 2001 From: antrad Date: Tue, 3 Mar 2026 12:29:35 +0100 Subject: [PATCH 03/35] get pose --- CMakeLists.txt | 67 +++--- .../object_pose_detector.hpp | 70 ++++--- package.xml | 9 +- src/object_pose_detector.cpp | 196 ++++++++++++++---- srv/DetectBlocksPoses.srv | 2 +- test/go_above_markers.py | 88 ++++++++ 6 files changed, 324 insertions(+), 108 deletions(-) create mode 100644 test/go_above_markers.py diff --git a/CMakeLists.txt b/CMakeLists.txt index eecd7be..36d758d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,6 +7,7 @@ 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) @@ -17,40 +18,32 @@ 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 - src/object_pose_detector.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 -) +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" -) +rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} + "rosidl_typesupport_cpp") -target_link_libraries(${PROJECT_NAME}_node - ${cpp_typesupport_target} -) +target_link_libraries(${PROJECT_NAME}_node ${cpp_typesupport_target}) ament_target_dependencies( ${PROJECT_NAME}_node + ament_index_cpp moveit_ros_planning_interface rclcpp std_msgs @@ -58,31 +51,25 @@ ament_target_dependencies( sensor_msgs geometry_msgs cv_bridge -) + tf2 + tf2_ros + tf2_geometry_msgs) target_include_directories( - ${PROJECT_NAME}_node PUBLIC - "$" - "$" - ${argparse_SOURCE_DIR}/argparse/include -) - -target_link_libraries( ${PROJECT_NAME}_node - argparse - yaml-cpp - ${OpenCV_LIBRARIES} -) + PUBLIC "$" + "$" + ${argparse_SOURCE_DIR}/argparse/include) + +target_link_libraries(${PROJECT_NAME}_node argparse yaml-cpp + ${OpenCV_LIBRARIES}) -install( - TARGETS ${PROJECT_NAME}_node - DESTINATION lib/${PROJECT_NAME} -) +install(TARGETS ${PROJECT_NAME}_node 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) + set(_ament_cmake_copyright_found TRUE) + set(_ament_cmake_cpplint_found TRUE) ament_lint_auto_find_test_dependencies() endif() diff --git a/include/scene_objects_manager/object_pose_detector.hpp b/include/scene_objects_manager/object_pose_detector.hpp index b58b06e..a7f3dd4 100644 --- a/include/scene_objects_manager/object_pose_detector.hpp +++ b/include/scene_objects_manager/object_pose_detector.hpp @@ -1,43 +1,63 @@ -#ifndef OBJECT_POSE_DETECTOR_NODE_HPP_INCLUDED -#define OBJECT_POSE_DETECTOR_NODE_HPP_INCLUDED +#ifndef SCENE_OBJECTS_MANAGER__OBJECT_POSE_DETECTOR_HPP_ +#define SCENE_OBJECTS_MANAGER__OBJECT_POSE_DETECTOR_HPP_ -#include -#include +#include +#include +#include +#include +#include +#include +#include #include -#include +#include +#include +#include +#include -#include "rclcpp/rclcpp.hpp" -#include "sensor_msgs/msg/image.hpp" +#include "ament_index_cpp/get_package_share_directory.hpp" #include "geometry_msgs/msg/pose_array.hpp" - +#include "rclcpp/rclcpp.hpp" #include "scene_objects_manager/srv/detect_blocks_poses.hpp" - +#include "sensor_msgs/msg/image.hpp" namespace sobjmanager { -class ObjectPoseDetectorNode : public rclcpp::Node -{ +class ObjectPoseDetectorNode : public rclcpp::Node { public: - ObjectPoseDetectorNode(); + ObjectPoseDetectorNode(); private: - void imageCb(const sensor_msgs::msg::Image::SharedPtr msg); + void imageCb(const sensor_msgs::msg::Image::SharedPtr msg); + + void onDetect( + const std::shared_ptr req, + std::shared_ptr res); + + void readCamCalib(); + + std::mutex mtx_; + std::optional last_rgb_; + rclcpp::Time last_rgb_stamp_; + + rclcpp::Subscription::SharedPtr sub_; + rclcpp::Service::SharedPtr srv_; - void onDetect( - const std::shared_ptr req, - std::shared_ptr res); + std::string image_topic_; + std::string output_frame_; - std::mutex mtx_; - std::optional last_bgr_; - rclcpp::Time last_stamp_; + std::vector rvecs, tvecs; + std::string camera_info_path_; + cv::Mat camera_matrix_; + cv::Mat dist_coeffs_; + double aruco_size_{ 0.0198 }; - rclcpp::Subscription::SharedPtr sub_; - rclcpp::Service::SharedPtr srv_; + std::unique_ptr tf_buffer_; + std::shared_ptr tf_listener_; - std::string image_topic_; - std::string output_frame_; + std::string camera_frame_; + std::string target_frame_; }; -} // namespace sobjmanager -#endif // OBJECT_POSE_DETECTOR_NODE_HPP_INCLUDED +} // namespace sobjmanager +#endif // SCENE_OBJECTS_MANAGER__OBJECT_POSE_DETECTOR_HPP_ diff --git a/package.xml b/package.xml index ac8de43..24343a7 100644 --- a/package.xml +++ b/package.xml @@ -9,14 +9,17 @@ ament_cmake + cv_bridge + geometry_msgs moveit_msgs moveit_ros_planning_interface rclcpp + sensor_msgs std_msgs + tf2 + tf2_geometry_msgs + tf2_ros yaml-cpp - sensor_msgs - geometry_msgs - cv_bridge rosidl_default_generators rosidl_default_runtime diff --git a/src/object_pose_detector.cpp b/src/object_pose_detector.cpp index 2eda21a..d56d348 100644 --- a/src/object_pose_detector.cpp +++ b/src/object_pose_detector.cpp @@ -2,51 +2,169 @@ namespace sobjmanager { -ObjectPoseDetectorNode::ObjectPoseDetectorNode() -: rclcpp::Node("object_pose_detector") -{ - image_topic_ = this->declare_parameter("image_topic", "/cam_scene/rgb/image_raw"); - output_frame_ = this->declare_parameter("output_frame", "camera_frame"); - - sub_ = this->create_subscription( - image_topic_, - rclcpp::SensorDataQoS(), - std::bind(&ObjectPoseDetectorNode::imageCb, this, std::placeholders::_1) - ); - - srv_ = this->create_service( - "detect_blocs_poses", - std::bind(&ObjectPoseDetectorNode::onDetect, this, std::placeholders::_1, std::placeholders::_2) - ); - - RCLCPP_INFO(this->get_logger(), "Detector node started. Subscriptionto: %s", image_topic_.c_str()); - RCLCPP_INFO(this->get_logger(), "Service ready: /detect_blocs_poses"); +static std::vector readDataVector(const YAML::Node& root, const std::string& key) { + if (!root[key] || !root[key]["data"]) { + throw std::runtime_error("Missing key '" + key + ".data' in calibration YAML"); + } + return root[key]["data"].as>(); } -void ObjectPoseDetectorNode::imageCb(const sensor_msgs::msg::Image::SharedPtr msg) -{ - v_bridge::CvImageConstPtr cv_ptr; +ObjectPoseDetectorNode::ObjectPoseDetectorNode() : rclcpp::Node("object_pose_detector") { + const auto share = ament_index_cpp::get_package_share_directory("aegis_utils"); + camera_info_path_ = this->declare_parameter("camera_info_path", share + "/config/scene_intrinsics.yaml"); + aruco_size_ = this->declare_parameter("aruco_size", aruco_size_); + + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + camera_frame_ = "cam_scene_rgb_camera_optical_frame"; + target_frame_ = "base_link"; + + // TODO(#xx): Read data from ROS2 topic /cam_scene/rgb/camera_info, but for now it contains data befoure calibration + readCamCalib(); + + image_topic_ = this->declare_parameter("image_topic", "/cam_scene/rgb/image_raw"); + output_frame_ = this->declare_parameter("output_frame", "base_link"); + + sub_ = this->create_subscription( + image_topic_, + rclcpp::SensorDataQoS(), + std::bind(&ObjectPoseDetectorNode::imageCb, this, std::placeholders::_1)); + + srv_ = this->create_service( + "detect_blocks_poses", + std::bind(&ObjectPoseDetectorNode::onDetect, this, std::placeholders::_1, std::placeholders::_2)); + + RCLCPP_INFO(this->get_logger(), "Detector node started. Subscriptionto: %s", image_topic_.c_str()); + RCLCPP_INFO(this->get_logger(), "Service ready: /detect_blocks_poses"); +} + +void ObjectPoseDetectorNode::imageCb(const sensor_msgs::msg::Image::SharedPtr msg) { + cv_bridge::CvImageConstPtr cv_ptr; + try { + cv_ptr = cv_bridge::toCvShare(msg, "bgr8"); + } catch (const cv_bridge::Exception& e) { + RCLCPP_WARN(this->get_logger(), "cv_bridge exception: %s", e.what()); + return; + } + { + std::lock_guard lock(mtx_); + last_rgb_ = cv_ptr->image.clone(); + last_rgb_stamp_ = msg->header.stamp; + } + RCLCPP_DEBUG(this->get_logger(), "Stored image %dx%d", cv_ptr->image.cols, cv_ptr->image.rows); +} + +void ObjectPoseDetectorNode::onDetect( + const std::shared_ptr req, + std::shared_ptr res) { + if (!req->detect) { + RCLCPP_INFO(this->get_logger(), "start_detection=false -> returning empty PoseArray (no detection)."); + return; + } + + res->poses.header.stamp = this->now(); + res->poses.header.frame_id = output_frame_; + res->poses.poses.clear(); + + // TODO:: Read from image topic + std::string path_image; + path_image = "/home/antrad/ceai_ws/getpos_data/scene_4_blocks.png"; + cv::Mat img = cv::imread(path_image, cv::IMREAD_COLOR); + if (img.empty()) { + RCLCPP_ERROR(this->get_logger(), "cv::imread failed"); + return; + } + + // cv::Mat img = last_rgb_; + // if (img.empty()) { + // RCLCPP_ERROR(this->get_logger(), "cv::imread failed"); + // return; + // } + + // cv::imshow("get_pose", img); + // cv::waitKey(0); + // cv::destroyAllWindows(); + + std::vector markerIds; + std::vector> markerCorners, rejectedCandidates; + cv::Ptr parameters = cv::aruco::DetectorParameters::create(); + cv::Ptr dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250); + cv::aruco::detectMarkers(img, dictionary, markerCorners, markerIds, parameters, rejectedCandidates); + + cv::Mat output_image = img.clone(); + cv::aruco::drawDetectedMarkers(output_image, markerCorners, markerIds); + + cv::imshow("output", output_image); + cv::waitKey(0); + cv::destroyAllWindows(); + + cv::aruco::estimatePoseSingleMarkers(markerCorners, aruco_size_, camera_matrix_, dist_coeffs_, rvecs, tvecs); + + for (size_t i = 0; i < markerIds.size(); i++) { + geometry_msgs::msg::PoseStamped pose_cam_; + pose_cam_.header.stamp = this->now(); + pose_cam_.header.frame_id = camera_frame_; + pose_cam_.pose.position.x = tvecs[i][0]; + pose_cam_.pose.position.y = tvecs[i][1]; + pose_cam_.pose.position.z = tvecs[i][2]; + cv::Mat R; + cv::Rodrigues(rvecs[i], R); + tf2::Matrix3x3 tf3d( + R.at(0, 0), + R.at(0, 1), + R.at(0, 2), + R.at(1, 0), + R.at(1, 1), + R.at(1, 2), + R.at(2, 0), + R.at(2, 1), + R.at(2, 2)); + tf2::Quaternion q; + tf3d.getRotation(q); + q.normalize(); + pose_cam_.pose.orientation = tf2::toMsg(q); + + geometry_msgs::msg::PoseStamped pose_target; try { - cv_ptr = cv_bridge::toCvShare(msg, "bgr8"); - } catch (const cv_bridge::Exception & e) { - RCLCPP_WARN(this->get_logger(), "cv_bridge exception: %s", e.what()); - return; + pose_target = tf_buffer_->transform(pose_cam_, target_frame_, tf2::durationFromSec(0.1)); + } catch (const tf2::TransformException& ex) { + RCLCPP_WARN(this->get_logger(), "TF transforms failed: %s", ex.what()); + continue; } + res->poses.poses.push_back(pose_target.pose); + } +} + +void ObjectPoseDetectorNode::readCamCalib() { + YAML::Node calib; + try { + calib = YAML::LoadFile(camera_info_path_); + } catch (const std::exception& e) { + throw std::runtime_error( + std::string("Failed to open/parse camera calib YAML '") + camera_info_path_ + "': " + e.what()); + } + const auto K = readDataVector(calib, "camera_matrix"); + const auto D = readDataVector(calib, "distortion_coefficients"); - { - std::lock_guard lock(mtx_); - last_bgr_ = cv_ptr->image.clone(); - last_stamp_ = msg->header.stamp; + if (K.size() != 9) { + throw std::runtime_error("cmera_matrix.data must contains 9 elements"); + } + if (D.empty()) { + throw std::runtime_error("distortion_coefficients.data must not be empty"); + } + + camera_matrix_ = cv::Mat(3, 3, CV_64F); + for (int r = 0; r < 3; ++r) { + for (int c = 0; c < 3; ++c) { + camera_matrix_.at(r, c) = K[r * 3 + c]; } - RCLCPP_DEBUG(this->get_logger(), "Stored image %dx%d", cv_ptr->image.cols, cv_ptr->image.rows); -} + } -void ObjectPoseDetectorNode::onDetect( - const std::shared_ptr, - std::shared_ptr res -) -{ - RCLCPP_INFO(this->get_logger(), "make the request"); + dist_coeffs_ = cv::Mat(1, static_cast(D.size()), CV_64F); + for (size_t i = 0; i < D.size(); ++i) { + dist_coeffs_.at(0, static_cast(i)) = D[i]; + } } -} // namespace sobjmanager +} // namespace sobjmanager diff --git a/srv/DetectBlocksPoses.srv b/srv/DetectBlocksPoses.srv index 1d06b30..90d8772 100644 --- a/srv/DetectBlocksPoses.srv +++ b/srv/DetectBlocksPoses.srv @@ -1,5 +1,5 @@ # Request -bool start_detection +bool detect --- # Response diff --git a/test/go_above_markers.py b/test/go_above_markers.py new file mode 100644 index 0000000..1d3e3fb --- /dev/null +++ b/test/go_above_markers.py @@ -0,0 +1,88 @@ +#!/usr/bin/env python3 +import time + +import rclpy +from geometry_msgs.msg import PoseStamped + +from scene_objects_manager.srv import DetectBlocksPoses +from aegis_director.robot_director import RobotDirector + + +def main(): + rclpy.init() + + director = RobotDirector(synchronous=True) + + service_name = "/detect_blocks_poses" + target_frame_fallback = "base_link" + z_offset = 0.10 + wait_at_target_s = 3.0 + + cli = director.node.create_client(DetectBlocksPoses, service_name) + director.node.get_logger().info(f"Waiting for service {service_name} ...") + if not cli.wait_for_service(timeout_sec=10.0): + director.node.get_logger().error(f"Service {service_name} not available.") + director.node.destroy_node() + rclpy.shutdown() + return + + req = DetectBlocksPoses.Request() + req.detect = True + + future = cli.call_async(req) + rclpy.spin_until_future_complete(director.node, future, timeout_sec=10.0) + + resp = future.result() + if resp is None: + director.node.get_logger().error("Service call failed / timed out.") + director.node.destroy_node() + rclpy.shutdown() + return + + pose_array = resp.poses + frame_id = pose_array.header.frame_id or target_frame_fallback + + director.node.get_logger().info( + f"Got {len(pose_array.poses)} pose(s) in frame '{frame_id}'." + ) + + targets: list[PoseStamped] = [] + for i, p in enumerate(pose_array.poses): + target = PoseStamped() + target.header.stamp = director.node.get_clock().now().to_msg() + target.header.frame_id = frame_id + target.pose = p + target.pose.position.z += z_offset + + target.pose.orientation.x = 1.0 + target.pose.orientation.y = 0.000651933 + target.pose.orientation.z = 0.000017838 + target.pose.orientation.w = 0.000445749 + + targets.append(target) + + director.node.get_logger().info( + f"[{i + 1}] above: x={target.pose.position.x:.3f}, " + f"y={target.pose.position.y:.3f}, z={target.pose.position.z:.3f}" + ) + + for i, t in enumerate(targets): + director.node.get_logger().info(f"Moving to target {i + 1}/{len(targets)} ...") + director.pose_move( + pose=t, + cartesian=False, + max_vel=0.2, + max_accel=0.2, + ) + director.node.get_logger().info( + f"Reached target {i + 1}. Waiting {wait_at_target_s:.1f}s ..." + ) + time.sleep(wait_at_target_s) + + director.node.get_logger().info("Done with all targets.") + director.node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() From a46581c2a272f0b522b5204651a7b9dce7236b5d Mon Sep 17 00:00:00 2001 From: antrad Date: Tue, 3 Mar 2026 12:29:53 +0100 Subject: [PATCH 04/35] get pose --- CMakeLists.txt | 7 ------- 1 file changed, 7 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 36d758d..d29c480 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -66,12 +66,5 @@ target_link_libraries(${PROJECT_NAME}_node argparse yaml-cpp install(TARGETS ${PROJECT_NAME}_node 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() - ament_export_dependencies(rosidl_default_runtime) ament_package() From f19bfab04249c1be0c34d8179987ea9863141f58 Mon Sep 17 00:00:00 2001 From: AntoniRL Date: Thu, 5 Mar 2026 13:24:49 +0100 Subject: [PATCH 05/35] wip get camera config from ros topic --- .../object_pose_detector.hpp | 15 +++++++++++---- src/object_pose_detector.cpp | 8 +++++++- 2 files changed, 18 insertions(+), 5 deletions(-) diff --git a/include/scene_objects_manager/object_pose_detector.hpp b/include/scene_objects_manager/object_pose_detector.hpp index a7f3dd4..7cf46d8 100644 --- a/include/scene_objects_manager/object_pose_detector.hpp +++ b/include/scene_objects_manager/object_pose_detector.hpp @@ -36,20 +36,27 @@ class ObjectPoseDetectorNode : public rclcpp::Node { void readCamCalib(); + // void cameraInfoCb(const sensor_msgs::msg::CameraInfo::SharedPtr msg); + + rclcpp::Subscription::SharedPtr sub_; + rclcpp::Service::SharedPtr srv_; + // rclcpp::Subscription::SharedPtr cam_info_sub_; + std::mutex mtx_; std::optional last_rgb_; rclcpp::Time last_rgb_stamp_; - rclcpp::Subscription::SharedPtr sub_; - rclcpp::Service::SharedPtr srv_; + // std::mutex calib_mutex_; + // bool have_calib{false}; std::string image_topic_; std::string output_frame_; - std::vector rvecs, tvecs; - std::string camera_info_path_; cv::Mat camera_matrix_; cv::Mat dist_coeffs_; + + std::vector rvecs, tvecs; + std::string camera_info_path_; double aruco_size_{ 0.0198 }; std::unique_ptr tf_buffer_; diff --git a/src/object_pose_detector.cpp b/src/object_pose_detector.cpp index d56d348..8aedb3b 100644 --- a/src/object_pose_detector.cpp +++ b/src/object_pose_detector.cpp @@ -20,7 +20,7 @@ ObjectPoseDetectorNode::ObjectPoseDetectorNode() : rclcpp::Node("object_pose_det camera_frame_ = "cam_scene_rgb_camera_optical_frame"; target_frame_ = "base_link"; - // TODO(#xx): Read data from ROS2 topic /cam_scene/rgb/camera_info, but for now it contains data befoure calibration + // TODO(#xx): Read data from ROS2 topic /cam_scene/rgb/camera_info, it contains data from befoure the calib readCamCalib(); image_topic_ = this->declare_parameter("image_topic", "/cam_scene/rgb/image_raw"); @@ -35,6 +35,12 @@ ObjectPoseDetectorNode::ObjectPoseDetectorNode() : rclcpp::Node("object_pose_det "detect_blocks_poses", std::bind(&ObjectPoseDetectorNode::onDetect, this, std::placeholders::_1, std::placeholders::_2)); + // cam_info_sub_ = this->create_subscription( + // "/cam_scene/rgb/camera_info", + // rclcpp::SensorDataQoS(), + // std::bind(&ObjectPoseDetectorNode::cameraInfoCb, this, std::placeholders::_1) + // ) + RCLCPP_INFO(this->get_logger(), "Detector node started. Subscriptionto: %s", image_topic_.c_str()); RCLCPP_INFO(this->get_logger(), "Service ready: /detect_blocks_poses"); } From cc42ad2d5fb9010d116f79af4042dbb52b4f0393 Mon Sep 17 00:00:00 2001 From: AntoniRL Date: Thu, 5 Mar 2026 17:22:47 +0100 Subject: [PATCH 06/35] update params --- .../object_pose_detector.hpp | 2 +- src/object_pose_detector.cpp | 24 +++++++++---------- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/include/scene_objects_manager/object_pose_detector.hpp b/include/scene_objects_manager/object_pose_detector.hpp index 7cf46d8..568ee92 100644 --- a/include/scene_objects_manager/object_pose_detector.hpp +++ b/include/scene_objects_manager/object_pose_detector.hpp @@ -57,7 +57,7 @@ class ObjectPoseDetectorNode : public rclcpp::Node { std::vector rvecs, tvecs; std::string camera_info_path_; - double aruco_size_{ 0.0198 }; + double aruco_size_{ 0.02 }; std::unique_ptr tf_buffer_; std::shared_ptr tf_listener_; diff --git a/src/object_pose_detector.cpp b/src/object_pose_detector.cpp index 8aedb3b..c7499a2 100644 --- a/src/object_pose_detector.cpp +++ b/src/object_pose_detector.cpp @@ -17,7 +17,7 @@ ObjectPoseDetectorNode::ObjectPoseDetectorNode() : rclcpp::Node("object_pose_det tf_buffer_ = std::make_unique(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); - camera_frame_ = "cam_scene_rgb_camera_optical_frame"; + camera_frame_ = "cam_scene_rgb_camera_optical_frame_cal"; target_frame_ = "base_link"; // TODO(#xx): Read data from ROS2 topic /cam_scene/rgb/camera_info, it contains data from befoure the calib @@ -74,20 +74,20 @@ void ObjectPoseDetectorNode::onDetect( res->poses.poses.clear(); // TODO:: Read from image topic - std::string path_image; - path_image = "/home/antrad/ceai_ws/getpos_data/scene_4_blocks.png"; - cv::Mat img = cv::imread(path_image, cv::IMREAD_COLOR); +// std::string path_image; +// path_image = "/home/antrad/ceai_ws/getpos_data/scene_4_blocks.png"; +// cv::Mat img = cv::imread(path_image, cv::IMREAD_COLOR); +// if (img.empty()) { +// RCLCPP_ERROR(this->get_logger(), "cv::imread failed"); +// return; +// } + + const cv::Mat& img = *last_rgb_; if (img.empty()) { - RCLCPP_ERROR(this->get_logger(), "cv::imread failed"); - return; + RCLCPP_ERROR(this->get_logger(), "cv::imread failed"); + return; } - // cv::Mat img = last_rgb_; - // if (img.empty()) { - // RCLCPP_ERROR(this->get_logger(), "cv::imread failed"); - // return; - // } - // cv::imshow("get_pose", img); // cv::waitKey(0); // cv::destroyAllWindows(); From 0d4e9780787637f83f4e77a895a9245885f2e3d9 Mon Sep 17 00:00:00 2001 From: AntoniRL Date: Thu, 5 Mar 2026 17:29:28 +0100 Subject: [PATCH 07/35] update params --- .pre-commit-config.yaml | 2 +- include/scene_objects_manager/object_pose_detector.hpp | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 4cbc075..689148e 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -29,7 +29,7 @@ repos: args: ["--autofix", "--no-sort-keys", "--indent", "2"] - repo: https://github.com/codespell-project/codespell - rev: v2.4.1 + rev: v2.4.2 hooks: - id: codespell args: [-w] diff --git a/include/scene_objects_manager/object_pose_detector.hpp b/include/scene_objects_manager/object_pose_detector.hpp index 568ee92..416d8d2 100644 --- a/include/scene_objects_manager/object_pose_detector.hpp +++ b/include/scene_objects_manager/object_pose_detector.hpp @@ -68,3 +68,5 @@ class ObjectPoseDetectorNode : public rclcpp::Node { } // namespace sobjmanager #endif // SCENE_OBJECTS_MANAGER__OBJECT_POSE_DETECTOR_HPP_ + +// test From 7f3be5eca64e9d6bd46ce9d11ecadf74406498f7 Mon Sep 17 00:00:00 2001 From: AntoniRL Date: Tue, 10 Mar 2026 14:12:07 +0100 Subject: [PATCH 08/35] camera config from topic --- .pre-commit-config.yaml | 2 +- .../object_pose_detector.hpp | 14 +- src/object_pose_detector.cpp | 152 +++++++++++------- 3 files changed, 99 insertions(+), 69 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 689148e..9850177 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -35,7 +35,7 @@ repos: args: [-w] - repo: https://github.com/astral-sh/ruff-pre-commit - rev: v0.15.4 + rev: v0.15.5 hooks: - id: ruff-check args: [--fix, --exit-non-zero-on-fix] diff --git a/include/scene_objects_manager/object_pose_detector.hpp b/include/scene_objects_manager/object_pose_detector.hpp index 416d8d2..3abc7c6 100644 --- a/include/scene_objects_manager/object_pose_detector.hpp +++ b/include/scene_objects_manager/object_pose_detector.hpp @@ -19,6 +19,7 @@ #include "geometry_msgs/msg/pose_array.hpp" #include "rclcpp/rclcpp.hpp" #include "scene_objects_manager/srv/detect_blocks_poses.hpp" +#include "sensor_msgs/msg/camera_info.hpp" #include "sensor_msgs/msg/image.hpp" namespace sobjmanager { @@ -36,18 +37,17 @@ class ObjectPoseDetectorNode : public rclcpp::Node { void readCamCalib(); - // void cameraInfoCb(const sensor_msgs::msg::CameraInfo::SharedPtr msg); + void cameraInfoCb(const sensor_msgs::msg::CameraInfo::SharedPtr msg); - rclcpp::Subscription::SharedPtr sub_; - rclcpp::Service::SharedPtr srv_; - // rclcpp::Subscription::SharedPtr cam_info_sub_; + rclcpp::Subscription::SharedPtr image_sub_; + rclcpp::Service::SharedPtr detect_blocks_srv_; + rclcpp::Subscription::SharedPtr cam_info_sub_; std::mutex mtx_; std::optional last_rgb_; rclcpp::Time last_rgb_stamp_; - // std::mutex calib_mutex_; - // bool have_calib{false}; + bool camera_info_received_{ false }; std::string image_topic_; std::string output_frame_; @@ -64,6 +64,8 @@ class ObjectPoseDetectorNode : public rclcpp::Node { std::string camera_frame_; std::string target_frame_; + std::string cam_info_topic_; + std::string cam_frame_; }; } // namespace sobjmanager diff --git a/src/object_pose_detector.cpp b/src/object_pose_detector.cpp index c7499a2..75b3dee 100644 --- a/src/object_pose_detector.cpp +++ b/src/object_pose_detector.cpp @@ -2,45 +2,44 @@ namespace sobjmanager { -static std::vector readDataVector(const YAML::Node& root, const std::string& key) { - if (!root[key] || !root[key]["data"]) { - throw std::runtime_error("Missing key '" + key + ".data' in calibration YAML"); - } - return root[key]["data"].as>(); -} +// static std::vector readDataVector(const YAML::Node& root, const std::string& key) { +// if (!root[key] || !root[key]["data"]) { +// throw std::runtime_error("Missing key '" + key + ".data' in calibration YAML"); +// } +// return root[key]["data"].as>(); +// } ObjectPoseDetectorNode::ObjectPoseDetectorNode() : rclcpp::Node("object_pose_detector") { - const auto share = ament_index_cpp::get_package_share_directory("aegis_utils"); - camera_info_path_ = this->declare_parameter("camera_info_path", share + "/config/scene_intrinsics.yaml"); - aruco_size_ = this->declare_parameter("aruco_size", aruco_size_); - - tf_buffer_ = std::make_unique(this->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); - - camera_frame_ = "cam_scene_rgb_camera_optical_frame_cal"; - target_frame_ = "base_link"; + // const auto share = ament_index_cpp::get_package_share_directory("aegis_utils"); + // camera_info_path_ = this->declare_parameter("camera_info_path", share + + // "/config/scene_intrinsics.yaml"); // TODO: cancel // TODO(#xx): Read data from ROS2 topic /cam_scene/rgb/camera_info, it contains data from befoure the calib - readCamCalib(); + // readCamCalib(); + aruco_size_ = this->declare_parameter("aruco_size", aruco_size_); image_topic_ = this->declare_parameter("image_topic", "/cam_scene/rgb/image_raw"); + cam_info_topic_ = this->declare_parameter("cam_info_topic", "/cam_scene/rgb/camera_info"); output_frame_ = this->declare_parameter("output_frame", "base_link"); + cam_frame_ = this->declare_parameter("cam_scene_rgb_camera_optical_frame_cal"); - sub_ = this->create_subscription( + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + image_sub_ = this->create_subscription( image_topic_, rclcpp::SensorDataQoS(), std::bind(&ObjectPoseDetectorNode::imageCb, this, std::placeholders::_1)); - srv_ = this->create_service( + cam_info_sub_ = this->create_subscription( + cam_info_topic_, + rclcpp::SensorDataQoS(), + std::bind(&ObjectPoseDetectorNode::cameraInfoCb, this, std::placeholders::_1)); + + detect_blocks_srv_ = this->create_service( "detect_blocks_poses", std::bind(&ObjectPoseDetectorNode::onDetect, this, std::placeholders::_1, std::placeholders::_2)); - // cam_info_sub_ = this->create_subscription( - // "/cam_scene/rgb/camera_info", - // rclcpp::SensorDataQoS(), - // std::bind(&ObjectPoseDetectorNode::cameraInfoCb, this, std::placeholders::_1) - // ) - RCLCPP_INFO(this->get_logger(), "Detector node started. Subscriptionto: %s", image_topic_.c_str()); RCLCPP_INFO(this->get_logger(), "Service ready: /detect_blocks_poses"); } @@ -58,7 +57,6 @@ void ObjectPoseDetectorNode::imageCb(const sensor_msgs::msg::Image::SharedPtr ms last_rgb_ = cv_ptr->image.clone(); last_rgb_stamp_ = msg->header.stamp; } - RCLCPP_DEBUG(this->get_logger(), "Stored image %dx%d", cv_ptr->image.cols, cv_ptr->image.rows); } void ObjectPoseDetectorNode::onDetect( @@ -68,29 +66,34 @@ void ObjectPoseDetectorNode::onDetect( RCLCPP_INFO(this->get_logger(), "start_detection=false -> returning empty PoseArray (no detection)."); return; } + if (!camera_info_received_) { + RCLCPP_INFO(this->get_logger(), "Camera info not received yet."); + return; + } res->poses.header.stamp = this->now(); res->poses.header.frame_id = output_frame_; res->poses.poses.clear(); - // TODO:: Read from image topic -// std::string path_image; -// path_image = "/home/antrad/ceai_ws/getpos_data/scene_4_blocks.png"; -// cv::Mat img = cv::imread(path_image, cv::IMREAD_COLOR); -// if (img.empty()) { -// RCLCPP_ERROR(this->get_logger(), "cv::imread failed"); -// return; -// } + // Read from folder - testing + // std::string path_image; + // path_image = "/home/antrad/ceai_ws/getpos_data/scene_4_blocks.png"; + // cv::Mat img = cv::imread(path_image, cv::IMREAD_COLOR); + // if (img.empty()) { + // RCLCPP_ERROR(this->get_logger(), "cv::imread failed"); + // return; + // } - const cv::Mat& img = *last_rgb_; - if (img.empty()) { - RCLCPP_ERROR(this->get_logger(), "cv::imread failed"); - return; + cv::Mat img; + { + std::lock_guard lock(mtx_); + img = last_rgb_.value().clone(); } - // cv::imshow("get_pose", img); - // cv::waitKey(0); - // cv::destroyAllWindows(); + if (img.empty()) { + RCLCPP_ERROR(this->get_logger(), "cv::imread failed"); + return; + } std::vector markerIds; std::vector> markerCorners, rejectedCandidates; @@ -110,7 +113,7 @@ void ObjectPoseDetectorNode::onDetect( for (size_t i = 0; i < markerIds.size(); i++) { geometry_msgs::msg::PoseStamped pose_cam_; pose_cam_.header.stamp = this->now(); - pose_cam_.header.frame_id = camera_frame_; + pose_cam_.header.frame_id = cam_frame_; pose_cam_.pose.position.x = tvecs[i][0]; pose_cam_.pose.position.y = tvecs[i][1]; pose_cam_.pose.position.z = tvecs[i][2]; @@ -133,7 +136,7 @@ void ObjectPoseDetectorNode::onDetect( geometry_msgs::msg::PoseStamped pose_target; try { - pose_target = tf_buffer_->transform(pose_cam_, target_frame_, tf2::durationFromSec(0.1)); + pose_target = tf_buffer_->transform(pose_cam_, output_frame_, tf2::durationFromSec(0.1)); } catch (const tf2::TransformException& ex) { RCLCPP_WARN(this->get_logger(), "TF transforms failed: %s", ex.what()); continue; @@ -142,35 +145,60 @@ void ObjectPoseDetectorNode::onDetect( } } -void ObjectPoseDetectorNode::readCamCalib() { - YAML::Node calib; - try { - calib = YAML::LoadFile(camera_info_path_); - } catch (const std::exception& e) { - throw std::runtime_error( - std::string("Failed to open/parse camera calib YAML '") + camera_info_path_ + "': " + e.what()); - } - const auto K = readDataVector(calib, "camera_matrix"); - const auto D = readDataVector(calib, "distortion_coefficients"); - - if (K.size() != 9) { - throw std::runtime_error("cmera_matrix.data must contains 9 elements"); - } - if (D.empty()) { - throw std::runtime_error("distortion_coefficients.data must not be empty"); +void ObjectPoseDetectorNode::cameraInfoCb(const sensor_msgs::msg::CameraInfo::SharedPtr msg) { + if (camera_info_received_) { + return; } camera_matrix_ = cv::Mat(3, 3, CV_64F); for (int r = 0; r < 3; ++r) { for (int c = 0; c < 3; ++c) { - camera_matrix_.at(r, c) = K[r * 3 + c]; + camera_matrix_.at(r, c) = msg->k[r * 3 + c]; } } - dist_coeffs_ = cv::Mat(1, static_cast(D.size()), CV_64F); - for (size_t i = 0; i < D.size(); ++i) { - dist_coeffs_.at(0, static_cast(i)) = D[i]; + dist_coeffs_ = cv::Mat(1, static_cast(msg->d.size()), CV_64F); + for (size_t i = 0; i < msg->d.size(); ++i) { + dist_coeffs_.at(0, static_cast(i)) = msg->d[i]; } + + camera_frame_ = msg->header.frame_id; + camera_info_received_ = true; + + RCLCPP_INFO(this->get_logger(), "Received camera info from topic."); + + cam_info_sub_.reset(); } +// void ObjectPoseDetectorNode::readCamCalib() { +// YAML::Node calib; +// try { +// calib = YAML::LoadFile(camera_info_path_); +// } catch (const std::exception& e) { +// throw std::runtime_error( +// std::string("Failed to open/parse camera calib YAML '") + camera_info_path_ + "': " + e.what()); +// } +// const auto K = readDataVector(calib, "camera_matrix"); +// const auto D = readDataVector(calib, "distortion_coefficients"); + +// if (K.size() != 9) { +// throw std::runtime_error("cmera_matrix.data must contains 9 elements"); +// } +// if (D.empty()) { +// throw std::runtime_error("distortion_coefficients.data must not be empty"); +// } + +// camera_matrix_ = cv::Mat(3, 3, CV_64F); +// for (int r = 0; r < 3; ++r) { +// for (int c = 0; c < 3; ++c) { +// camera_matrix_.at(r, c) = K[r * 3 + c]; +// } +// } + +// dist_coeffs_ = cv::Mat(1, static_cast(D.size()), CV_64F); +// for (size_t i = 0; i < D.size(); ++i) { +// dist_coeffs_.at(0, static_cast(i)) = D[i]; +// } +// } + } // namespace sobjmanager From 2e48488d3ed302071c4cdcec7d829d8c2003eaa0 Mon Sep 17 00:00:00 2001 From: AntoniRL Date: Tue, 10 Mar 2026 14:59:03 +0100 Subject: [PATCH 09/35] update the camera_frame --- src/object_pose_detector.cpp | 2 +- test/go_above_markers.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/object_pose_detector.cpp b/src/object_pose_detector.cpp index 75b3dee..beeacc9 100644 --- a/src/object_pose_detector.cpp +++ b/src/object_pose_detector.cpp @@ -21,7 +21,7 @@ ObjectPoseDetectorNode::ObjectPoseDetectorNode() : rclcpp::Node("object_pose_det image_topic_ = this->declare_parameter("image_topic", "/cam_scene/rgb/image_raw"); cam_info_topic_ = this->declare_parameter("cam_info_topic", "/cam_scene/rgb/camera_info"); output_frame_ = this->declare_parameter("output_frame", "base_link"); - cam_frame_ = this->declare_parameter("cam_scene_rgb_camera_optical_frame_cal"); + cam_frame_ = this->declare_parameter("cam_frame", "cam_scene_rgb_camera_optical_frame_cal"); tf_buffer_ = std::make_unique(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); diff --git a/test/go_above_markers.py b/test/go_above_markers.py index 1d3e3fb..bee5c9a 100644 --- a/test/go_above_markers.py +++ b/test/go_above_markers.py @@ -15,7 +15,7 @@ def main(): service_name = "/detect_blocks_poses" target_frame_fallback = "base_link" - z_offset = 0.10 + z_offset = 0.12 wait_at_target_s = 3.0 cli = director.node.create_client(DetectBlocksPoses, service_name) From cac854aa1325beef50d2e02531c227e7708f5abf Mon Sep 17 00:00:00 2001 From: AntoniRL Date: Tue, 10 Mar 2026 15:21:01 +0100 Subject: [PATCH 10/35] change the testing program --- src/object_pose_detector.cpp | 2 +- test/go_above_markers.py | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/object_pose_detector.cpp b/src/object_pose_detector.cpp index beeacc9..6a7006b 100644 --- a/src/object_pose_detector.cpp +++ b/src/object_pose_detector.cpp @@ -105,7 +105,7 @@ void ObjectPoseDetectorNode::onDetect( cv::aruco::drawDetectedMarkers(output_image, markerCorners, markerIds); cv::imshow("output", output_image); - cv::waitKey(0); + cv::waitKey(1); cv::destroyAllWindows(); cv::aruco::estimatePoseSingleMarkers(markerCorners, aruco_size_, camera_matrix_, dist_coeffs_, rvecs, tvecs); diff --git a/test/go_above_markers.py b/test/go_above_markers.py index bee5c9a..2116f2d 100644 --- a/test/go_above_markers.py +++ b/test/go_above_markers.py @@ -52,6 +52,7 @@ def main(): target.header.stamp = director.node.get_clock().now().to_msg() target.header.frame_id = frame_id target.pose = p + print(f"Target z: {target.pose.position.z}") # TODO: testing target.pose.position.z += z_offset target.pose.orientation.x = 1.0 From adab15402fb930dadbbb3326192440cc8b6821c3 Mon Sep 17 00:00:00 2001 From: AntoniRL Date: Tue, 10 Mar 2026 15:49:11 +0100 Subject: [PATCH 11/35] update tests - draw pointer --- test/go_above_markers.py | 31 +++++++++++++++++++++++++++++++ 1 file changed, 31 insertions(+) diff --git a/test/go_above_markers.py b/test/go_above_markers.py index 2116f2d..bbe53c6 100644 --- a/test/go_above_markers.py +++ b/test/go_above_markers.py @@ -2,22 +2,47 @@ import time import rclpy +from std_msgs.msg import ColorRGBA from geometry_msgs.msg import PoseStamped +from visualization_msgs.msg import Marker, MarkerArray from scene_objects_manager.srv import DetectBlocksPoses from aegis_director.robot_director import RobotDirector +def make_marker(idx: int, pose: PoseStamped, ns: str = "detected_blocks") -> Marker: + msg = Marker() + msg.header.stamp = pose.header.stamp + msg.header.frame_id = pose.header.frame_id + + msg.ns = ns + msg.id = idx + msg.type = Marker.SPHERE + msg.action = Marker.ADD + + msg.pose = pose.pose + + msg.scale.x = 0.04 + msg.scale.y = 0.04 + msg.scale.z = 0.04 + + msg.color = ColorRGBA(r=1.0, g=0.0, b=0.0, a=1.0) + return msg + + def main(): rclpy.init() director = RobotDirector(synchronous=True) service_name = "/detect_blocks_poses" + marker_topic = "/detected_blocks_markers" target_frame_fallback = "base_link" z_offset = 0.12 wait_at_target_s = 3.0 + marker_pub = director.node.create_publisher(MarkerArray, marker_topic, 10) + cli = director.node.create_client(DetectBlocksPoses, service_name) director.node.get_logger().info(f"Waiting for service {service_name} ...") if not cli.wait_for_service(timeout_sec=10.0): @@ -47,6 +72,8 @@ def main(): ) targets: list[PoseStamped] = [] + marker_array = MarkerArray() + for i, p in enumerate(pose_array.poses): target = PoseStamped() target.header.stamp = director.node.get_clock().now().to_msg() @@ -61,12 +88,16 @@ def main(): target.pose.orientation.w = 0.000445749 targets.append(target) + marker_array.markers.append(make_marker(i, target)) director.node.get_logger().info( f"[{i + 1}] above: x={target.pose.position.x:.3f}, " f"y={target.pose.position.y:.3f}, z={target.pose.position.z:.3f}" ) + marker_pub.publish(marker_array) + time.sleep(0.5) + for i, t in enumerate(targets): director.node.get_logger().info(f"Moving to target {i + 1}/{len(targets)} ...") director.pose_move( From 7861b986e6cba3fd35b3271a550ffd20cb0e5e69 Mon Sep 17 00:00:00 2001 From: AntoniRL Date: Tue, 10 Mar 2026 16:36:20 +0100 Subject: [PATCH 12/35] cleaning coments --- .../object_pose_detector.hpp | 4 -- src/object_pose_detector.cpp | 64 ++++--------------- test/go_above_markers.py | 36 +---------- 3 files changed, 13 insertions(+), 91 deletions(-) diff --git a/include/scene_objects_manager/object_pose_detector.hpp b/include/scene_objects_manager/object_pose_detector.hpp index 3abc7c6..e7a54aa 100644 --- a/include/scene_objects_manager/object_pose_detector.hpp +++ b/include/scene_objects_manager/object_pose_detector.hpp @@ -35,8 +35,6 @@ class ObjectPoseDetectorNode : public rclcpp::Node { const std::shared_ptr req, std::shared_ptr res); - void readCamCalib(); - void cameraInfoCb(const sensor_msgs::msg::CameraInfo::SharedPtr msg); rclcpp::Subscription::SharedPtr image_sub_; @@ -70,5 +68,3 @@ class ObjectPoseDetectorNode : public rclcpp::Node { } // namespace sobjmanager #endif // SCENE_OBJECTS_MANAGER__OBJECT_POSE_DETECTOR_HPP_ - -// test diff --git a/src/object_pose_detector.cpp b/src/object_pose_detector.cpp index 6a7006b..fcb89fe 100644 --- a/src/object_pose_detector.cpp +++ b/src/object_pose_detector.cpp @@ -2,21 +2,7 @@ namespace sobjmanager { -// static std::vector readDataVector(const YAML::Node& root, const std::string& key) { -// if (!root[key] || !root[key]["data"]) { -// throw std::runtime_error("Missing key '" + key + ".data' in calibration YAML"); -// } -// return root[key]["data"].as>(); -// } - ObjectPoseDetectorNode::ObjectPoseDetectorNode() : rclcpp::Node("object_pose_detector") { - // const auto share = ament_index_cpp::get_package_share_directory("aegis_utils"); - // camera_info_path_ = this->declare_parameter("camera_info_path", share + - // "/config/scene_intrinsics.yaml"); // TODO: cancel - - // TODO(#xx): Read data from ROS2 topic /cam_scene/rgb/camera_info, it contains data from befoure the calib - // readCamCalib(); - aruco_size_ = this->declare_parameter("aruco_size", aruco_size_); image_topic_ = this->declare_parameter("image_topic", "/cam_scene/rgb/image_raw"); cam_info_topic_ = this->declare_parameter("cam_info_topic", "/cam_scene/rgb/camera_info"); @@ -76,13 +62,16 @@ void ObjectPoseDetectorNode::onDetect( res->poses.poses.clear(); // Read from folder - testing - // std::string path_image; - // path_image = "/home/antrad/ceai_ws/getpos_data/scene_4_blocks.png"; - // cv::Mat img = cv::imread(path_image, cv::IMREAD_COLOR); - // if (img.empty()) { - // RCLCPP_ERROR(this->get_logger(), "cv::imread failed"); - // return; - // } + // std::string path_image; + // path_image = "/home/antrad/ceai_ws/getpos_data/scene_4_blocks.png"; + // cv::Mat img = cv::imread(path_image, cv::IMREAD_COLOR); + // if (img.empty()) { + // RCLCPP_ERROR(this->get_logger(), "cv::imread failed"); + // return; + // } + // cv::imshow("data", img); + // cv::waitKey(0); + // cv::destroyAllWindows(); cv::Mat img; { @@ -105,7 +94,7 @@ void ObjectPoseDetectorNode::onDetect( cv::aruco::drawDetectedMarkers(output_image, markerCorners, markerIds); cv::imshow("output", output_image); - cv::waitKey(1); + cv::waitKey(0); cv::destroyAllWindows(); cv::aruco::estimatePoseSingleMarkers(markerCorners, aruco_size_, camera_matrix_, dist_coeffs_, rvecs, tvecs); @@ -170,35 +159,4 @@ void ObjectPoseDetectorNode::cameraInfoCb(const sensor_msgs::msg::CameraInfo::Sh cam_info_sub_.reset(); } -// void ObjectPoseDetectorNode::readCamCalib() { -// YAML::Node calib; -// try { -// calib = YAML::LoadFile(camera_info_path_); -// } catch (const std::exception& e) { -// throw std::runtime_error( -// std::string("Failed to open/parse camera calib YAML '") + camera_info_path_ + "': " + e.what()); -// } -// const auto K = readDataVector(calib, "camera_matrix"); -// const auto D = readDataVector(calib, "distortion_coefficients"); - -// if (K.size() != 9) { -// throw std::runtime_error("cmera_matrix.data must contains 9 elements"); -// } -// if (D.empty()) { -// throw std::runtime_error("distortion_coefficients.data must not be empty"); -// } - -// camera_matrix_ = cv::Mat(3, 3, CV_64F); -// for (int r = 0; r < 3; ++r) { -// for (int c = 0; c < 3; ++c) { -// camera_matrix_.at(r, c) = K[r * 3 + c]; -// } -// } - -// dist_coeffs_ = cv::Mat(1, static_cast(D.size()), CV_64F); -// for (size_t i = 0; i < D.size(); ++i) { -// dist_coeffs_.at(0, static_cast(i)) = D[i]; -// } -// } - } // namespace sobjmanager diff --git a/test/go_above_markers.py b/test/go_above_markers.py index bbe53c6..be2bab7 100644 --- a/test/go_above_markers.py +++ b/test/go_above_markers.py @@ -2,47 +2,22 @@ import time import rclpy -from std_msgs.msg import ColorRGBA from geometry_msgs.msg import PoseStamped -from visualization_msgs.msg import Marker, MarkerArray from scene_objects_manager.srv import DetectBlocksPoses from aegis_director.robot_director import RobotDirector -def make_marker(idx: int, pose: PoseStamped, ns: str = "detected_blocks") -> Marker: - msg = Marker() - msg.header.stamp = pose.header.stamp - msg.header.frame_id = pose.header.frame_id - - msg.ns = ns - msg.id = idx - msg.type = Marker.SPHERE - msg.action = Marker.ADD - - msg.pose = pose.pose - - msg.scale.x = 0.04 - msg.scale.y = 0.04 - msg.scale.z = 0.04 - - msg.color = ColorRGBA(r=1.0, g=0.0, b=0.0, a=1.0) - return msg - - def main(): rclpy.init() director = RobotDirector(synchronous=True) service_name = "/detect_blocks_poses" - marker_topic = "/detected_blocks_markers" target_frame_fallback = "base_link" - z_offset = 0.12 + z_offset = 0.10 wait_at_target_s = 3.0 - marker_pub = director.node.create_publisher(MarkerArray, marker_topic, 10) - cli = director.node.create_client(DetectBlocksPoses, service_name) director.node.get_logger().info(f"Waiting for service {service_name} ...") if not cli.wait_for_service(timeout_sec=10.0): @@ -72,15 +47,12 @@ def main(): ) targets: list[PoseStamped] = [] - marker_array = MarkerArray() - for i, p in enumerate(pose_array.poses): target = PoseStamped() target.header.stamp = director.node.get_clock().now().to_msg() target.header.frame_id = frame_id target.pose = p - print(f"Target z: {target.pose.position.z}") # TODO: testing - target.pose.position.z += z_offset + target.pose.position.z = 10 + z_offset target.pose.orientation.x = 1.0 target.pose.orientation.y = 0.000651933 @@ -88,16 +60,12 @@ def main(): target.pose.orientation.w = 0.000445749 targets.append(target) - marker_array.markers.append(make_marker(i, target)) director.node.get_logger().info( f"[{i + 1}] above: x={target.pose.position.x:.3f}, " f"y={target.pose.position.y:.3f}, z={target.pose.position.z:.3f}" ) - marker_pub.publish(marker_array) - time.sleep(0.5) - for i, t in enumerate(targets): director.node.get_logger().info(f"Moving to target {i + 1}/{len(targets)} ...") director.pose_move( From 77dfb172395735c0df0926332e46a88faf56fb83 Mon Sep 17 00:00:00 2001 From: AntoniRL Date: Thu, 12 Mar 2026 12:21:27 +0100 Subject: [PATCH 13/35] get rotation in z axis --- .pre-commit-config.yaml | 2 +- .../object_pose_detector.hpp | 4 +- src/object_pose_detector.cpp | 139 ++++++++++++------ test/detect_all_markers.py | 102 +++++++++++++ test/go_above_markers.py | 10 +- 5 files changed, 209 insertions(+), 48 deletions(-) create mode 100644 test/detect_all_markers.py diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 9850177..4cedd67 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -48,7 +48,7 @@ repos: - id: cmake-lint - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v22.1.0 + rev: v22.1.1 hooks: - id: clang-format types_or: [c++, proto] diff --git a/include/scene_objects_manager/object_pose_detector.hpp b/include/scene_objects_manager/object_pose_detector.hpp index e7a54aa..aa1f4a8 100644 --- a/include/scene_objects_manager/object_pose_detector.hpp +++ b/include/scene_objects_manager/object_pose_detector.hpp @@ -7,6 +7,7 @@ #include #include +#include #include #include #include @@ -36,10 +37,11 @@ class ObjectPoseDetectorNode : public rclcpp::Node { std::shared_ptr res); void cameraInfoCb(const sensor_msgs::msg::CameraInfo::SharedPtr msg); + void readCamCalib(); rclcpp::Subscription::SharedPtr image_sub_; rclcpp::Service::SharedPtr detect_blocks_srv_; - rclcpp::Subscription::SharedPtr cam_info_sub_; + // rclcpp::Subscription::SharedPtr cam_info_sub_; std::mutex mtx_; std::optional last_rgb_; diff --git a/src/object_pose_detector.cpp b/src/object_pose_detector.cpp index fcb89fe..9bbffc7 100644 --- a/src/object_pose_detector.cpp +++ b/src/object_pose_detector.cpp @@ -2,6 +2,14 @@ namespace sobjmanager { +// testing method for running in the computer +static std::vector readDataVector(const YAML::Node& root, const std::string& key) { + if (!root[key] || !root[key]["data"]) { + throw std::runtime_error("Missing key '" + key + ".data' in calibration YAML"); + } + return root[key]["data"].as>(); +} + ObjectPoseDetectorNode::ObjectPoseDetectorNode() : rclcpp::Node("object_pose_detector") { aruco_size_ = this->declare_parameter("aruco_size", aruco_size_); image_topic_ = this->declare_parameter("image_topic", "/cam_scene/rgb/image_raw"); @@ -9,6 +17,11 @@ ObjectPoseDetectorNode::ObjectPoseDetectorNode() : rclcpp::Node("object_pose_det output_frame_ = this->declare_parameter("output_frame", "base_link"); cam_frame_ = this->declare_parameter("cam_frame", "cam_scene_rgb_camera_optical_frame_cal"); + // testing + const auto share = ament_index_cpp::get_package_share_directory("aegis_utils"); + camera_info_path_ = this->declare_parameter("camera_info_path", share + "/config/scene_intrinsics.yaml"); + readCamCalib(); + tf_buffer_ = std::make_unique(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); @@ -17,10 +30,11 @@ ObjectPoseDetectorNode::ObjectPoseDetectorNode() : rclcpp::Node("object_pose_det rclcpp::SensorDataQoS(), std::bind(&ObjectPoseDetectorNode::imageCb, this, std::placeholders::_1)); - cam_info_sub_ = this->create_subscription( - cam_info_topic_, - rclcpp::SensorDataQoS(), - std::bind(&ObjectPoseDetectorNode::cameraInfoCb, this, std::placeholders::_1)); + // testing + // cam_info_sub_ = this->create_subscription( + // cam_info_topic_, + // rclcpp::SensorDataQoS(), + // std::bind(&ObjectPoseDetectorNode::cameraInfoCb, this, std::placeholders::_1)); detect_blocks_srv_ = this->create_service( "detect_blocks_poses", @@ -52,33 +66,30 @@ void ObjectPoseDetectorNode::onDetect( RCLCPP_INFO(this->get_logger(), "start_detection=false -> returning empty PoseArray (no detection)."); return; } - if (!camera_info_received_) { - RCLCPP_INFO(this->get_logger(), "Camera info not received yet."); - return; - } + // if (!camera_info_received_) { + // RCLCPP_INFO(this->get_logger(), "Camera info not received yet."); + // return; + // } res->poses.header.stamp = this->now(); res->poses.header.frame_id = output_frame_; res->poses.poses.clear(); // Read from folder - testing - // std::string path_image; - // path_image = "/home/antrad/ceai_ws/getpos_data/scene_4_blocks.png"; - // cv::Mat img = cv::imread(path_image, cv::IMREAD_COLOR); - // if (img.empty()) { - // RCLCPP_ERROR(this->get_logger(), "cv::imread failed"); - // return; - // } - // cv::imshow("data", img); - // cv::waitKey(0); - // cv::destroyAllWindows(); - - cv::Mat img; - { - std::lock_guard lock(mtx_); - img = last_rgb_.value().clone(); + std::string path_image; + path_image = "/home/antrad/ceai_ws/getpos_data/scene_4_blocks.png"; + cv::Mat img = cv::imread(path_image, cv::IMREAD_COLOR); + if (img.empty()) { + RCLCPP_ERROR(this->get_logger(), "cv::imread failed"); + return; } + // cv::Mat img; + // { + // std::lock_guard lock(mtx_); + // img = last_rgb_.value().clone(); + // } + if (img.empty()) { RCLCPP_ERROR(this->get_logger(), "cv::imread failed"); return; @@ -118,10 +129,22 @@ void ObjectPoseDetectorNode::onDetect( R.at(2, 0), R.at(2, 1), R.at(2, 2)); - tf2::Quaternion q; - tf3d.getRotation(q); - q.normalize(); - pose_cam_.pose.orientation = tf2::toMsg(q); + + double roll, pitch, yaw; + tf3d.getRPY(roll, pitch, yaw); + + double step = M_PI / 2.0; + yaw -= step * std::round(yaw / step) - M_PI; + + tf2::Quaternion q_yaw_only; + q_yaw_only.setRPY(0.0, 0.0, yaw); + q_yaw_only.normalize(); + pose_cam_.pose.orientation = tf2::toMsg(q_yaw_only); + + // tf2::Quaternion q; + // tf3d.getRotation(q); + // q.normalize(); + // pose_cam_.pose.orientation = tf2::toMsg(q); geometry_msgs::msg::PoseStamped pose_target; try { @@ -130,33 +153,67 @@ void ObjectPoseDetectorNode::onDetect( RCLCPP_WARN(this->get_logger(), "TF transforms failed: %s", ex.what()); continue; } + + pose_target.pose.position.z = 0.10; + res->poses.poses.push_back(pose_target.pose); } } -void ObjectPoseDetectorNode::cameraInfoCb(const sensor_msgs::msg::CameraInfo::SharedPtr msg) { - if (camera_info_received_) { - return; +// void ObjectPoseDetectorNode::cameraInfoCb(const sensor_msgs::msg::CameraInfo::SharedPtr msg) { +// if (camera_info_received_) { +// return; +// } + +// camera_matrix_ = cv::Mat(3, 3, CV_64F); +// for (int r = 0; r < 3; ++r) { +// for (int c = 0; c < 3; ++c) { +// camera_matrix_.at(r, c) = msg->k[r * 3 + c]; +// } +// } + +// dist_coeffs_ = cv::Mat(1, static_cast(msg->d.size()), CV_64F); +// for (size_t i = 0; i < msg->d.size(); ++i) { +// dist_coeffs_.at(0, static_cast(i)) = msg->d[i]; +// } + +// camera_frame_ = msg->header.frame_id; +// camera_info_received_ = true; + +// RCLCPP_INFO(this->get_logger(), "Received camera info from topic."); + +// cam_info_sub_.reset(); +// } + +void ObjectPoseDetectorNode::readCamCalib() { + YAML::Node calib; + try { + calib = YAML::LoadFile(camera_info_path_); + } catch (const std::exception& e) { + throw std::runtime_error( + std::string("Failed to open/parse camera calib YAML '") + camera_info_path_ + "': " + e.what()); + } + const auto K = readDataVector(calib, "camera_matrix"); + const auto D = readDataVector(calib, "distortion_coefficients"); + + if (K.size() != 9) { + throw std::runtime_error("cmera_matrix.data must contains 9 elements"); + } + if (D.empty()) { + throw std::runtime_error("distortion_coefficients.data must not be empty"); } camera_matrix_ = cv::Mat(3, 3, CV_64F); for (int r = 0; r < 3; ++r) { for (int c = 0; c < 3; ++c) { - camera_matrix_.at(r, c) = msg->k[r * 3 + c]; + camera_matrix_.at(r, c) = K[r * 3 + c]; } } - dist_coeffs_ = cv::Mat(1, static_cast(msg->d.size()), CV_64F); - for (size_t i = 0; i < msg->d.size(); ++i) { - dist_coeffs_.at(0, static_cast(i)) = msg->d[i]; + dist_coeffs_ = cv::Mat(1, static_cast(D.size()), CV_64F); + for (size_t i = 0; i < D.size(); ++i) { + dist_coeffs_.at(0, static_cast(i)) = D[i]; } - - camera_frame_ = msg->header.frame_id; - camera_info_received_ = true; - - RCLCPP_INFO(this->get_logger(), "Received camera info from topic."); - - cam_info_sub_.reset(); } } // namespace sobjmanager diff --git a/test/detect_all_markers.py b/test/detect_all_markers.py new file mode 100644 index 0000000..037e504 --- /dev/null +++ b/test/detect_all_markers.py @@ -0,0 +1,102 @@ +#!/usr/bin/env python3 +import rclpy + +from geometry_msgs.msg import PoseStamped, TransformStamped +from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster + +from scene_objects_manager.srv import DetectBlocksPoses +from aegis_director.robot_director import RobotDirector + + +def main(): + rclpy.init() + + director = RobotDirector(synchronous=True) + + service_name = "/detect_blocks_poses" + target_frame_fallback = "base_link" + z_offset = 0.10 + + cli = director.node.create_client(DetectBlocksPoses, service_name) + static_broadcaster = StaticTransformBroadcaster(director.node) + + director.node.get_logger().info(f"Waiting for service {service_name} ...") + if not cli.wait_for_service(timeout_sec=10.0): + director.node.get_logger().error(f"Service {service_name} not available.") + director.node.destroy_node() + rclpy.shutdown() + return + + req = DetectBlocksPoses.Request() + req.detect = True + + future = cli.call_async(req) + rclpy.spin_until_future_complete(director.node, future, timeout_sec=10.0) + + resp = future.result() + if resp is None: + director.node.get_logger().error("Service call failed / timed out.") + director.node.destroy_node() + rclpy.shutdown() + return + + pose_array = resp.poses + frame_id = pose_array.header.frame_id or target_frame_fallback + + director.node.get_logger().info( + f"Got {len(pose_array.poses)} pose(s) in frame '{frame_id}'." + ) + + targets: list[PoseStamped] = [] + static_transforms: list[TransformStamped] = [] + + now = director.node.get_clock().now().to_msg() + + for i, p in enumerate(pose_array.poses): + target = PoseStamped() + target.header.stamp = now + target.header.frame_id = frame_id + target.pose = p + target.pose.position.z += z_offset + + # target.pose.orientation.x = 1.0 + # target.pose.orientation.y = 0.000651933 + # target.pose.orientation.z = 0.000017838 + # target.pose.orientation.w = 0.000445749 + + targets.append(target) + + director.node.get_logger().info( + f"[{i + 1}] above: x={target.pose.position.x:.3f}, " + f"y={target.pose.position.y:.3f}, z={target.pose.position.z:.3f}" + ) + + tf_msg = TransformStamped() + tf_msg.header.stamp = now + tf_msg.header.frame_id = frame_id + tf_msg.child_frame_id = f"detected_block_{i + 1}" + + tf_msg.transform.translation.x = target.pose.position.x + tf_msg.transform.translation.y = target.pose.position.y + tf_msg.transform.translation.z = target.pose.position.z + + tf_msg.transform.rotation = target.pose.orientation + + static_transforms.append(tf_msg) + + if static_transforms: + static_broadcaster.sendTransform(static_transforms) + director.node.get_logger().info( + f"Published {len(static_transforms)} static TF frame(s)." + ) + + director.node.get_logger().info("Done with all targets.") + + rclpy.spin_once(director.node, timeout_sec=0.5) + + director.node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/test/go_above_markers.py b/test/go_above_markers.py index be2bab7..a6d2828 100644 --- a/test/go_above_markers.py +++ b/test/go_above_markers.py @@ -52,12 +52,12 @@ def main(): target.header.stamp = director.node.get_clock().now().to_msg() target.header.frame_id = frame_id target.pose = p - target.pose.position.z = 10 + z_offset + target.pose.position.z += z_offset - target.pose.orientation.x = 1.0 - target.pose.orientation.y = 0.000651933 - target.pose.orientation.z = 0.000017838 - target.pose.orientation.w = 0.000445749 + # target.pose.orientation.x = 1.0 + # target.pose.orientation.y = 0.000651933 + # target.pose.orientation.z = 0.000017838 + # target.pose.orientation.w = 0.000445749 targets.append(target) From e8b76f18fa525dd1dc735b5bf0478dec8dfcc73a Mon Sep 17 00:00:00 2001 From: AntoniRL Date: Fri, 13 Mar 2026 10:13:48 +0100 Subject: [PATCH 14/35] colect data of aruco center --- .pre-commit-config.yaml | 2 +- CMakeLists.txt | 2 + config/object_pose_detector_config.yaml | 10 +++ .../object_pose_detector.hpp | 4 +- src/object_pose_detector.cpp | 69 ++++++++++++++++--- 5 files changed, 76 insertions(+), 11 deletions(-) create mode 100644 config/object_pose_detector_config.yaml diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 4cedd67..208ca70 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -35,7 +35,7 @@ repos: args: [-w] - repo: https://github.com/astral-sh/ruff-pre-commit - rev: v0.15.5 + rev: v0.15.6 hooks: - id: ruff-check args: [--fix, --exit-non-zero-on-fix] diff --git a/CMakeLists.txt b/CMakeLists.txt index d29c480..1a5e0e3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -64,6 +64,8 @@ target_include_directories( 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) diff --git a/config/object_pose_detector_config.yaml b/config/object_pose_detector_config.yaml new file mode 100644 index 0000000..7af68be --- /dev/null +++ b/config/object_pose_detector_config.yaml @@ -0,0 +1,10 @@ +object_pose_detector: + ros__parameters: + aruco_dict: DICT_6X6_250 + squares_x: 6 + squares_y: 6 + aruco_size: 0.02 + image_topic: "/cam_scene/rgb/image_raw" + cam_info_topic: "/cam_scene/rgb/camera_info" + output_frame: "base_link" + cam_frame: "cam_scene_rgb_camera_optical_frame_cal" diff --git a/include/scene_objects_manager/object_pose_detector.hpp b/include/scene_objects_manager/object_pose_detector.hpp index aa1f4a8..0b98017 100644 --- a/include/scene_objects_manager/object_pose_detector.hpp +++ b/include/scene_objects_manager/object_pose_detector.hpp @@ -8,8 +8,10 @@ #include #include +#include #include #include +#include #include #include #include @@ -57,7 +59,7 @@ class ObjectPoseDetectorNode : public rclcpp::Node { std::vector rvecs, tvecs; std::string camera_info_path_; - double aruco_size_{ 0.02 }; + double aruco_size_; std::unique_ptr tf_buffer_; std::shared_ptr tf_listener_; diff --git a/src/object_pose_detector.cpp b/src/object_pose_detector.cpp index 9bbffc7..08cc4d4 100644 --- a/src/object_pose_detector.cpp +++ b/src/object_pose_detector.cpp @@ -2,7 +2,7 @@ namespace sobjmanager { -// testing method for running in the computer +// testing method for running at the computer static std::vector readDataVector(const YAML::Node& root, const std::string& key) { if (!root[key] || !root[key]["data"]) { throw std::runtime_error("Missing key '" + key + ".data' in calibration YAML"); @@ -10,8 +10,49 @@ static std::vector readDataVector(const YAML::Node& root, const std::str return root[key]["data"].as>(); } +// testing save results to the file +void appendMarkerToYaml(const std::string& filename, int marker_id, const std::vector& corners) { + std::ofstream file(filename, std::ios::app); + if (!file.is_open()) { + throw std::runtime_error("Could not open file: " + filename); + } + + file << " - id: " << marker_id << "\n"; + file << " corners:\n"; + + for (const auto& pt : corners) { + file << " - [" << pt.x << ", " << pt.y << "]\n"; + } + + file.close(); +} + +void appendTvecToYaml(const std::string& filename, const std::vector tvec) { + std::ofstream file(filename, std::ios::app); + if (!file.is_open()) { + throw std::runtime_error("Could not open file: " + filename); + } + file << " tvec:\n"; + + for (const auto& t : tvec) { + file << " - [" << t[0] << ", " << t[1] << ", " << t[2] << "]\n"; + } + + file.close(); +} + +void initYamlFile(const std::string& filename) { + std::ofstream file(filename); + if (!file.is_open()) { + throw std::runtime_error("Could not create file: " + filename); + } + + file << "markers:\n"; + file.close(); +} + ObjectPoseDetectorNode::ObjectPoseDetectorNode() : rclcpp::Node("object_pose_detector") { - aruco_size_ = this->declare_parameter("aruco_size", aruco_size_); + aruco_size_ = this->declare_parameter("aruco_size", 0.02); image_topic_ = this->declare_parameter("image_topic", "/cam_scene/rgb/image_raw"); cam_info_topic_ = this->declare_parameter("cam_info_topic", "/cam_scene/rgb/camera_info"); output_frame_ = this->declare_parameter("output_frame", "base_link"); @@ -36,6 +77,10 @@ ObjectPoseDetectorNode::ObjectPoseDetectorNode() : rclcpp::Node("object_pose_det // rclcpp::SensorDataQoS(), // std::bind(&ObjectPoseDetectorNode::cameraInfoCb, this, std::placeholders::_1)); + // testing + initYamlFile("corners.yaml"); + initYamlFile("tvecs.yaml"); + detect_blocks_srv_ = this->create_service( "detect_blocks_poses", std::bind(&ObjectPoseDetectorNode::onDetect, this, std::placeholders::_1, std::placeholders::_2)); @@ -101,15 +146,25 @@ void ObjectPoseDetectorNode::onDetect( cv::Ptr dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250); cv::aruco::detectMarkers(img, dictionary, markerCorners, markerIds, parameters, rejectedCandidates); + // testing + if (!markerCorners.empty()) { + appendMarkerToYaml("corners.yaml", markerIds[0], markerCorners[0]); + } + + // testing cv::Mat output_image = img.clone(); cv::aruco::drawDetectedMarkers(output_image, markerCorners, markerIds); - cv::imshow("output", output_image); cv::waitKey(0); cv::destroyAllWindows(); cv::aruco::estimatePoseSingleMarkers(markerCorners, aruco_size_, camera_matrix_, dist_coeffs_, rvecs, tvecs); + // testing + if (!tvecs.empty()) { + appendTvecToYaml("tvecs.yaml", tvecs); + } + for (size_t i = 0; i < markerIds.size(); i++) { geometry_msgs::msg::PoseStamped pose_cam_; pose_cam_.header.stamp = this->now(); @@ -134,18 +189,13 @@ void ObjectPoseDetectorNode::onDetect( tf3d.getRPY(roll, pitch, yaw); double step = M_PI / 2.0; - yaw -= step * std::round(yaw / step) - M_PI; + yaw -= step * std::round(yaw / step) - M_PI / 2; tf2::Quaternion q_yaw_only; q_yaw_only.setRPY(0.0, 0.0, yaw); q_yaw_only.normalize(); pose_cam_.pose.orientation = tf2::toMsg(q_yaw_only); - // tf2::Quaternion q; - // tf3d.getRotation(q); - // q.normalize(); - // pose_cam_.pose.orientation = tf2::toMsg(q); - geometry_msgs::msg::PoseStamped pose_target; try { pose_target = tf_buffer_->transform(pose_cam_, output_frame_, tf2::durationFromSec(0.1)); @@ -185,6 +235,7 @@ void ObjectPoseDetectorNode::onDetect( // cam_info_sub_.reset(); // } +// testing void ObjectPoseDetectorNode::readCamCalib() { YAML::Node calib; try { From 1356858b4a11622898a3eadd92ef520f0d8ab046 Mon Sep 17 00:00:00 2001 From: AntoniRL Date: Fri, 13 Mar 2026 17:31:48 +0100 Subject: [PATCH 15/35] bug te error in get pose --- config/object_pose_detector_config.yaml | 2 +- .../object_pose_detector.hpp | 2 +- src/object_pose_detector.cpp | 184 +++++++++--------- test/detect_all_markers.py | 2 +- test/go_above_markers.py | 142 ++++++++------ 5 files changed, 178 insertions(+), 154 deletions(-) diff --git a/config/object_pose_detector_config.yaml b/config/object_pose_detector_config.yaml index 7af68be..a4bbe7a 100644 --- a/config/object_pose_detector_config.yaml +++ b/config/object_pose_detector_config.yaml @@ -4,7 +4,7 @@ object_pose_detector: squares_x: 6 squares_y: 6 aruco_size: 0.02 - image_topic: "/cam_scene/rgb/image_raw" + 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" diff --git a/include/scene_objects_manager/object_pose_detector.hpp b/include/scene_objects_manager/object_pose_detector.hpp index 0b98017..37a8eaa 100644 --- a/include/scene_objects_manager/object_pose_detector.hpp +++ b/include/scene_objects_manager/object_pose_detector.hpp @@ -43,7 +43,7 @@ class ObjectPoseDetectorNode : public rclcpp::Node { rclcpp::Subscription::SharedPtr image_sub_; rclcpp::Service::SharedPtr detect_blocks_srv_; - // rclcpp::Subscription::SharedPtr cam_info_sub_; + rclcpp::Subscription::SharedPtr cam_info_sub_; std::mutex mtx_; std::optional last_rgb_; diff --git a/src/object_pose_detector.cpp b/src/object_pose_detector.cpp index 08cc4d4..5c9a8c4 100644 --- a/src/object_pose_detector.cpp +++ b/src/object_pose_detector.cpp @@ -3,25 +3,29 @@ namespace sobjmanager { // testing method for running at the computer -static std::vector readDataVector(const YAML::Node& root, const std::string& key) { - if (!root[key] || !root[key]["data"]) { - throw std::runtime_error("Missing key '" + key + ".data' in calibration YAML"); - } - return root[key]["data"].as>(); -} +// static std::vector readDataVector(const YAML::Node& root, const std::string& key) { +// if (!root[key] || !root[key]["data"]) { +// throw std::runtime_error("Missing key '" + key + ".data' in calibration YAML"); +// } +// return root[key]["data"].as>(); +// } // testing save results to the file -void appendMarkerToYaml(const std::string& filename, int marker_id, const std::vector& corners) { +void appendMarkerToYaml( + const std::string& filename, + std::vector marker_id, + const std::vector>& corners) { std::ofstream file(filename, std::ios::app); if (!file.is_open()) { throw std::runtime_error("Could not open file: " + filename); } + for (size_t i = 0; i < corners.size(); ++i) { + file << " - id: " << marker_id[i] << "\n"; + file << " corners:\n"; - file << " - id: " << marker_id << "\n"; - file << " corners:\n"; - - for (const auto& pt : corners) { - file << " - [" << pt.x << ", " << pt.y << "]\n"; + for (const auto& pt : corners[i]) { + file << " - [" << pt.x << ", " << pt.y << "]\n"; + } } file.close(); @@ -59,9 +63,9 @@ ObjectPoseDetectorNode::ObjectPoseDetectorNode() : rclcpp::Node("object_pose_det cam_frame_ = this->declare_parameter("cam_frame", "cam_scene_rgb_camera_optical_frame_cal"); // testing - const auto share = ament_index_cpp::get_package_share_directory("aegis_utils"); - camera_info_path_ = this->declare_parameter("camera_info_path", share + "/config/scene_intrinsics.yaml"); - readCamCalib(); + // const auto share = ament_index_cpp::get_package_share_directory("aegis_utils"); + // camera_info_path_ = this->declare_parameter("camera_info_path", share + + // "/config/scene_intrinsics.yaml"); readCamCalib(); tf_buffer_ = std::make_unique(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); @@ -72,10 +76,10 @@ ObjectPoseDetectorNode::ObjectPoseDetectorNode() : rclcpp::Node("object_pose_det std::bind(&ObjectPoseDetectorNode::imageCb, this, std::placeholders::_1)); // testing - // cam_info_sub_ = this->create_subscription( - // cam_info_topic_, - // rclcpp::SensorDataQoS(), - // std::bind(&ObjectPoseDetectorNode::cameraInfoCb, this, std::placeholders::_1)); + cam_info_sub_ = this->create_subscription( + cam_info_topic_, + rclcpp::SensorDataQoS(), + std::bind(&ObjectPoseDetectorNode::cameraInfoCb, this, std::placeholders::_1)); // testing initYamlFile("corners.yaml"); @@ -111,35 +115,35 @@ void ObjectPoseDetectorNode::onDetect( RCLCPP_INFO(this->get_logger(), "start_detection=false -> returning empty PoseArray (no detection)."); return; } - // if (!camera_info_received_) { - // RCLCPP_INFO(this->get_logger(), "Camera info not received yet."); - // return; - // } - - res->poses.header.stamp = this->now(); - res->poses.header.frame_id = output_frame_; - res->poses.poses.clear(); - - // Read from folder - testing - std::string path_image; - path_image = "/home/antrad/ceai_ws/getpos_data/scene_4_blocks.png"; - cv::Mat img = cv::imread(path_image, cv::IMREAD_COLOR); - if (img.empty()) { - RCLCPP_ERROR(this->get_logger(), "cv::imread failed"); + if (!camera_info_received_) { + RCLCPP_INFO(this->get_logger(), "Camera info not received yet."); return; } - // cv::Mat img; - // { - // std::lock_guard lock(mtx_); - // img = last_rgb_.value().clone(); + // Read from folder - testing + // std::string path_image; + // path_image = "/home/antrad/ceai_ws/getpos_data/scene_4_blocks.png"; + // cv::Mat img = cv::imread(path_image, cv::IMREAD_COLOR); + // if (img.empty()) { + // RCLCPP_ERROR(this->get_logger(), "cv::imread failed"); + // return; // } + cv::Mat img; + if (last_rgb_) { + std::lock_guard lock(mtx_); + img = last_rgb_.value().clone(); + } + if (img.empty()) { RCLCPP_ERROR(this->get_logger(), "cv::imread failed"); return; } + res->poses.header.stamp = this->now(); + res->poses.header.frame_id = output_frame_; + res->poses.poses.clear(); + std::vector markerIds; std::vector> markerCorners, rejectedCandidates; cv::Ptr parameters = cv::aruco::DetectorParameters::create(); @@ -148,15 +152,15 @@ void ObjectPoseDetectorNode::onDetect( // testing if (!markerCorners.empty()) { - appendMarkerToYaml("corners.yaml", markerIds[0], markerCorners[0]); + appendMarkerToYaml("corners.yaml", markerIds, markerCorners); } // testing - cv::Mat output_image = img.clone(); - cv::aruco::drawDetectedMarkers(output_image, markerCorners, markerIds); - cv::imshow("output", output_image); - cv::waitKey(0); - cv::destroyAllWindows(); + // cv::Mat output_image = img.clone(); + // cv::aruco::drawDetectedMarkers(output_image, markerCorners, markerIds); + // cv::imshow("output", output_image); + // cv::waitKey(0); + // cv::destroyAllWindows(); cv::aruco::estimatePoseSingleMarkers(markerCorners, aruco_size_, camera_matrix_, dist_coeffs_, rvecs, tvecs); @@ -210,61 +214,61 @@ void ObjectPoseDetectorNode::onDetect( } } -// void ObjectPoseDetectorNode::cameraInfoCb(const sensor_msgs::msg::CameraInfo::SharedPtr msg) { -// if (camera_info_received_) { -// return; -// } - -// camera_matrix_ = cv::Mat(3, 3, CV_64F); -// for (int r = 0; r < 3; ++r) { -// for (int c = 0; c < 3; ++c) { -// camera_matrix_.at(r, c) = msg->k[r * 3 + c]; -// } -// } - -// dist_coeffs_ = cv::Mat(1, static_cast(msg->d.size()), CV_64F); -// for (size_t i = 0; i < msg->d.size(); ++i) { -// dist_coeffs_.at(0, static_cast(i)) = msg->d[i]; -// } - -// camera_frame_ = msg->header.frame_id; -// camera_info_received_ = true; - -// RCLCPP_INFO(this->get_logger(), "Received camera info from topic."); - -// cam_info_sub_.reset(); -// } - -// testing -void ObjectPoseDetectorNode::readCamCalib() { - YAML::Node calib; - try { - calib = YAML::LoadFile(camera_info_path_); - } catch (const std::exception& e) { - throw std::runtime_error( - std::string("Failed to open/parse camera calib YAML '") + camera_info_path_ + "': " + e.what()); - } - const auto K = readDataVector(calib, "camera_matrix"); - const auto D = readDataVector(calib, "distortion_coefficients"); - - if (K.size() != 9) { - throw std::runtime_error("cmera_matrix.data must contains 9 elements"); - } - if (D.empty()) { - throw std::runtime_error("distortion_coefficients.data must not be empty"); +void ObjectPoseDetectorNode::cameraInfoCb(const sensor_msgs::msg::CameraInfo::SharedPtr msg) { + if (camera_info_received_) { + return; } camera_matrix_ = cv::Mat(3, 3, CV_64F); for (int r = 0; r < 3; ++r) { for (int c = 0; c < 3; ++c) { - camera_matrix_.at(r, c) = K[r * 3 + c]; + camera_matrix_.at(r, c) = msg->k[r * 3 + c]; } } - dist_coeffs_ = cv::Mat(1, static_cast(D.size()), CV_64F); - for (size_t i = 0; i < D.size(); ++i) { - dist_coeffs_.at(0, static_cast(i)) = D[i]; + dist_coeffs_ = cv::Mat(1, static_cast(msg->d.size()), CV_64F); + for (size_t i = 0; i < msg->d.size(); ++i) { + dist_coeffs_.at(0, static_cast(i)) = msg->d[i]; } + + camera_frame_ = msg->header.frame_id; + camera_info_received_ = true; + + RCLCPP_INFO(this->get_logger(), "Received camera info from topic."); + + cam_info_sub_.reset(); } +// testing +// void ObjectPoseDetectorNode::readCamCalib() { +// YAML::Node calib; +// try { +// calib = YAML::LoadFile(camera_info_path_); +// } catch (const std::exception& e) { +// throw std::runtime_error( +// std::string("Failed to open/parse camera calib YAML '") + camera_info_path_ + "': " + e.what()); +// } +// const auto K = readDataVector(calib, "camera_matrix"); +// const auto D = readDataVector(calib, "distortion_coefficients"); + +// if (K.size() != 9) { +// throw std::runtime_error("cmera_matrix.data must contains 9 elements"); +// } +// if (D.empty()) { +// throw std::runtime_error("distortion_coefficients.data must not be empty"); +// } + +// camera_matrix_ = cv::Mat(3, 3, CV_64F); +// for (int r = 0; r < 3; ++r) { +// for (int c = 0; c < 3; ++c) { +// camera_matrix_.at(r, c) = K[r * 3 + c]; +// } +// } + +// dist_coeffs_ = cv::Mat(1, static_cast(D.size()), CV_64F); +// for (size_t i = 0; i < D.size(); ++i) { +// dist_coeffs_.at(0, static_cast(i)) = D[i]; +// } +// } + } // namespace sobjmanager diff --git a/test/detect_all_markers.py b/test/detect_all_markers.py index 037e504..fb32636 100644 --- a/test/detect_all_markers.py +++ b/test/detect_all_markers.py @@ -15,7 +15,7 @@ def main(): service_name = "/detect_blocks_poses" target_frame_fallback = "base_link" - z_offset = 0.10 + z_offset = 0.01 cli = director.node.create_client(DetectBlocksPoses, service_name) static_broadcaster = StaticTransformBroadcaster(director.node) diff --git a/test/go_above_markers.py b/test/go_above_markers.py index a6d2828..e6a57ac 100644 --- a/test/go_above_markers.py +++ b/test/go_above_markers.py @@ -13,71 +13,91 @@ def main(): director = RobotDirector(synchronous=True) - service_name = "/detect_blocks_poses" - target_frame_fallback = "base_link" - z_offset = 0.10 - wait_at_target_s = 3.0 - - cli = director.node.create_client(DetectBlocksPoses, service_name) - director.node.get_logger().info(f"Waiting for service {service_name} ...") - if not cli.wait_for_service(timeout_sec=10.0): - director.node.get_logger().error(f"Service {service_name} not available.") - director.node.destroy_node() - rclpy.shutdown() - return - - req = DetectBlocksPoses.Request() - req.detect = True - - future = cli.call_async(req) - rclpy.spin_until_future_complete(director.node, future, timeout_sec=10.0) - - resp = future.result() - if resp is None: - director.node.get_logger().error("Service call failed / timed out.") - director.node.destroy_node() - rclpy.shutdown() - return - - pose_array = resp.poses - frame_id = pose_array.header.frame_id or target_frame_fallback - - director.node.get_logger().info( - f"Got {len(pose_array.poses)} pose(s) in frame '{frame_id}'." - ) - - targets: list[PoseStamped] = [] - for i, p in enumerate(pose_array.poses): - target = PoseStamped() - target.header.stamp = director.node.get_clock().now().to_msg() - target.header.frame_id = frame_id - target.pose = p - target.pose.position.z += z_offset - - # target.pose.orientation.x = 1.0 - # target.pose.orientation.y = 0.000651933 - # target.pose.orientation.z = 0.000017838 - # target.pose.orientation.w = 0.000445749 - - targets.append(target) - - director.node.get_logger().info( - f"[{i + 1}] above: x={target.pose.position.x:.3f}, " - f"y={target.pose.position.y:.3f}, z={target.pose.position.z:.3f}" + NUM_OF_ITER = 1 + + for i in range(NUM_OF_ITER): + director.joint_move( + joint_positions={ + "shoulder_pan_joint": 0.0, + "shoulder_lift_joint": -2.09, + "elbow_joint": 2.09, + "wrist_1_joint": -1.57, + "wrist_2_joint": -1.57, + "wrist_3_joint": 0.0, + }, + max_vel=0.1, + max_accel=0.1, ) - for i, t in enumerate(targets): - director.node.get_logger().info(f"Moving to target {i + 1}/{len(targets)} ...") - director.pose_move( - pose=t, - cartesian=False, - max_vel=0.2, - max_accel=0.2, - ) + time.sleep(2.0) + + service_name = "/detect_blocks_poses" + target_frame_fallback = "base_link" + z_offset = 0.01 + wait_at_target_s = 3.0 + + cli = director.node.create_client(DetectBlocksPoses, service_name) + director.node.get_logger().info(f"Waiting for service {service_name} ...") + if not cli.wait_for_service(timeout_sec=10.0): + director.node.get_logger().error(f"Service {service_name} not available.") + director.node.destroy_node() + rclpy.shutdown() + return + + req = DetectBlocksPoses.Request() + req.detect = True + + future = cli.call_async(req) + rclpy.spin_until_future_complete(director.node, future, timeout_sec=10.0) + + resp = future.result() + if resp is None: + director.node.get_logger().error("Service call failed / timed out.") + director.node.destroy_node() + rclpy.shutdown() + return + + pose_array = resp.poses + frame_id = pose_array.header.frame_id or target_frame_fallback + director.node.get_logger().info( - f"Reached target {i + 1}. Waiting {wait_at_target_s:.1f}s ..." + f"Got {len(pose_array.poses)} pose(s) in frame '{frame_id}'." ) - time.sleep(wait_at_target_s) + + targets: list[PoseStamped] = [] + for i, p in enumerate(pose_array.poses): + target = PoseStamped() + target.header.stamp = director.node.get_clock().now().to_msg() + target.header.frame_id = frame_id + target.pose = p + target.pose.position.z += z_offset + + # target.pose.orientation.x = 1.0 + # target.pose.orientation.y = 0.000651933 + # target.pose.orientation.z = 0.000017838 + # target.pose.orientation.w = 0.000445749 + + targets.append(target) + + director.node.get_logger().info( + f"[{i + 1}] above: x={target.pose.position.x:.3f}, " + f"y={target.pose.position.y:.3f}, z={target.pose.position.z:.3f}" + ) + + for i, t in enumerate(targets): + director.node.get_logger().info( + f"Moving to target {i + 1}/{len(targets)} ..." + ) + director.pose_move( + pose=t, + cartesian=False, + max_vel=0.1, + max_accel=0.1, + ) + director.node.get_logger().info( + f"Reached target {i + 1}. Waiting {wait_at_target_s:.1f}s ..." + ) + time.sleep(wait_at_target_s) director.node.get_logger().info("Done with all targets.") director.node.destroy_node() From 9e2ec76b070d0b682ebfd9e1550b7720f827b7dc Mon Sep 17 00:00:00 2001 From: AntoniRL Date: Mon, 23 Mar 2026 11:34:39 +0100 Subject: [PATCH 16/35] yaml changes --- .pre-commit-config.yaml | 2 +- config/object_pose_detector_config.yaml | 3 --- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 208ca70..907f65b 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -35,7 +35,7 @@ repos: args: [-w] - repo: https://github.com/astral-sh/ruff-pre-commit - rev: v0.15.6 + rev: v0.15.7 hooks: - id: ruff-check args: [--fix, --exit-non-zero-on-fix] diff --git a/config/object_pose_detector_config.yaml b/config/object_pose_detector_config.yaml index a4bbe7a..ada5324 100644 --- a/config/object_pose_detector_config.yaml +++ b/config/object_pose_detector_config.yaml @@ -1,8 +1,5 @@ object_pose_detector: ros__parameters: - aruco_dict: DICT_6X6_250 - squares_x: 6 - squares_y: 6 aruco_size: 0.02 image_topic: "/cam_scene/rgb/image_rect" cam_info_topic: "/cam_scene/rgb/camera_info" From 75e6d52a3aff6855bac9b46fe56d5a1c4bf02f31 Mon Sep 17 00:00:00 2001 From: AntoniRL Date: Mon, 23 Mar 2026 13:06:47 +0100 Subject: [PATCH 17/35] cleaning testing and debbuging lines --- README.md | 20 +++ .../object_pose_detector.hpp | 12 +- package.xml | 1 + src/object_pose_detector.cpp | 123 +----------------- test/detect_all_markers.py | 5 - test/go_above_markers.py | 5 - 6 files changed, 25 insertions(+), 141 deletions(-) diff --git a/README.md b/README.md index ad906d1..19f365e 100644 --- a/README.md +++ b/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. diff --git a/include/scene_objects_manager/object_pose_detector.hpp b/include/scene_objects_manager/object_pose_detector.hpp index 37a8eaa..94c9b0f 100644 --- a/include/scene_objects_manager/object_pose_detector.hpp +++ b/include/scene_objects_manager/object_pose_detector.hpp @@ -39,7 +39,6 @@ class ObjectPoseDetectorNode : public rclcpp::Node { std::shared_ptr res); void cameraInfoCb(const sensor_msgs::msg::CameraInfo::SharedPtr msg); - void readCamCalib(); rclcpp::Subscription::SharedPtr image_sub_; rclcpp::Service::SharedPtr detect_blocks_srv_; @@ -48,26 +47,21 @@ class ObjectPoseDetectorNode : public rclcpp::Node { std::mutex mtx_; std::optional last_rgb_; rclcpp::Time last_rgb_stamp_; - bool camera_info_received_{ false }; 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 rvecs, tvecs; - std::string camera_info_path_; - double aruco_size_; std::unique_ptr tf_buffer_; std::shared_ptr tf_listener_; - - std::string camera_frame_; - std::string target_frame_; - std::string cam_info_topic_; - std::string cam_frame_; }; } // namespace sobjmanager diff --git a/package.xml b/package.xml index 24343a7..b9ace0c 100644 --- a/package.xml +++ b/package.xml @@ -5,6 +5,7 @@ 0.0.0 Node for managing objects within MoveIt2 PlanningSceneInterface. Maciej Aleksandrowicz + Antoni Radomiński-Lasek Apache-2.0 ament_cmake diff --git a/src/object_pose_detector.cpp b/src/object_pose_detector.cpp index 5c9a8c4..4ee5174 100644 --- a/src/object_pose_detector.cpp +++ b/src/object_pose_detector.cpp @@ -2,59 +2,6 @@ namespace sobjmanager { -// testing method for running at the computer -// static std::vector readDataVector(const YAML::Node& root, const std::string& key) { -// if (!root[key] || !root[key]["data"]) { -// throw std::runtime_error("Missing key '" + key + ".data' in calibration YAML"); -// } -// return root[key]["data"].as>(); -// } - -// testing save results to the file -void appendMarkerToYaml( - const std::string& filename, - std::vector marker_id, - const std::vector>& corners) { - std::ofstream file(filename, std::ios::app); - if (!file.is_open()) { - throw std::runtime_error("Could not open file: " + filename); - } - for (size_t i = 0; i < corners.size(); ++i) { - file << " - id: " << marker_id[i] << "\n"; - file << " corners:\n"; - - for (const auto& pt : corners[i]) { - file << " - [" << pt.x << ", " << pt.y << "]\n"; - } - } - - file.close(); -} - -void appendTvecToYaml(const std::string& filename, const std::vector tvec) { - std::ofstream file(filename, std::ios::app); - if (!file.is_open()) { - throw std::runtime_error("Could not open file: " + filename); - } - file << " tvec:\n"; - - for (const auto& t : tvec) { - file << " - [" << t[0] << ", " << t[1] << ", " << t[2] << "]\n"; - } - - file.close(); -} - -void initYamlFile(const std::string& filename) { - std::ofstream file(filename); - if (!file.is_open()) { - throw std::runtime_error("Could not create file: " + filename); - } - - file << "markers:\n"; - file.close(); -} - ObjectPoseDetectorNode::ObjectPoseDetectorNode() : rclcpp::Node("object_pose_detector") { aruco_size_ = this->declare_parameter("aruco_size", 0.02); image_topic_ = this->declare_parameter("image_topic", "/cam_scene/rgb/image_raw"); @@ -62,11 +9,6 @@ ObjectPoseDetectorNode::ObjectPoseDetectorNode() : rclcpp::Node("object_pose_det output_frame_ = this->declare_parameter("output_frame", "base_link"); cam_frame_ = this->declare_parameter("cam_frame", "cam_scene_rgb_camera_optical_frame_cal"); - // testing - // const auto share = ament_index_cpp::get_package_share_directory("aegis_utils"); - // camera_info_path_ = this->declare_parameter("camera_info_path", share + - // "/config/scene_intrinsics.yaml"); readCamCalib(); - tf_buffer_ = std::make_unique(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); @@ -75,16 +17,11 @@ ObjectPoseDetectorNode::ObjectPoseDetectorNode() : rclcpp::Node("object_pose_det rclcpp::SensorDataQoS(), std::bind(&ObjectPoseDetectorNode::imageCb, this, std::placeholders::_1)); - // testing cam_info_sub_ = this->create_subscription( cam_info_topic_, rclcpp::SensorDataQoS(), std::bind(&ObjectPoseDetectorNode::cameraInfoCb, this, std::placeholders::_1)); - // testing - initYamlFile("corners.yaml"); - initYamlFile("tvecs.yaml"); - detect_blocks_srv_ = this->create_service( "detect_blocks_poses", std::bind(&ObjectPoseDetectorNode::onDetect, this, std::placeholders::_1, std::placeholders::_2)); @@ -120,15 +57,6 @@ void ObjectPoseDetectorNode::onDetect( return; } - // Read from folder - testing - // std::string path_image; - // path_image = "/home/antrad/ceai_ws/getpos_data/scene_4_blocks.png"; - // cv::Mat img = cv::imread(path_image, cv::IMREAD_COLOR); - // if (img.empty()) { - // RCLCPP_ERROR(this->get_logger(), "cv::imread failed"); - // return; - // } - cv::Mat img; if (last_rgb_) { std::lock_guard lock(mtx_); @@ -150,25 +78,8 @@ void ObjectPoseDetectorNode::onDetect( cv::Ptr dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250); cv::aruco::detectMarkers(img, dictionary, markerCorners, markerIds, parameters, rejectedCandidates); - // testing - if (!markerCorners.empty()) { - appendMarkerToYaml("corners.yaml", markerIds, markerCorners); - } - - // testing - // cv::Mat output_image = img.clone(); - // cv::aruco::drawDetectedMarkers(output_image, markerCorners, markerIds); - // cv::imshow("output", output_image); - // cv::waitKey(0); - // cv::destroyAllWindows(); - cv::aruco::estimatePoseSingleMarkers(markerCorners, aruco_size_, camera_matrix_, dist_coeffs_, rvecs, tvecs); - // testing - if (!tvecs.empty()) { - appendTvecToYaml("tvecs.yaml", tvecs); - } - for (size_t i = 0; i < markerIds.size(); i++) { geometry_msgs::msg::PoseStamped pose_cam_; pose_cam_.header.stamp = this->now(); @@ -231,7 +142,7 @@ void ObjectPoseDetectorNode::cameraInfoCb(const sensor_msgs::msg::CameraInfo::Sh dist_coeffs_.at(0, static_cast(i)) = msg->d[i]; } - camera_frame_ = msg->header.frame_id; + cam_frame_ = msg->header.frame_id; camera_info_received_ = true; RCLCPP_INFO(this->get_logger(), "Received camera info from topic."); @@ -239,36 +150,4 @@ void ObjectPoseDetectorNode::cameraInfoCb(const sensor_msgs::msg::CameraInfo::Sh cam_info_sub_.reset(); } -// testing -// void ObjectPoseDetectorNode::readCamCalib() { -// YAML::Node calib; -// try { -// calib = YAML::LoadFile(camera_info_path_); -// } catch (const std::exception& e) { -// throw std::runtime_error( -// std::string("Failed to open/parse camera calib YAML '") + camera_info_path_ + "': " + e.what()); -// } -// const auto K = readDataVector(calib, "camera_matrix"); -// const auto D = readDataVector(calib, "distortion_coefficients"); - -// if (K.size() != 9) { -// throw std::runtime_error("cmera_matrix.data must contains 9 elements"); -// } -// if (D.empty()) { -// throw std::runtime_error("distortion_coefficients.data must not be empty"); -// } - -// camera_matrix_ = cv::Mat(3, 3, CV_64F); -// for (int r = 0; r < 3; ++r) { -// for (int c = 0; c < 3; ++c) { -// camera_matrix_.at(r, c) = K[r * 3 + c]; -// } -// } - -// dist_coeffs_ = cv::Mat(1, static_cast(D.size()), CV_64F); -// for (size_t i = 0; i < D.size(); ++i) { -// dist_coeffs_.at(0, static_cast(i)) = D[i]; -// } -// } - } // namespace sobjmanager diff --git a/test/detect_all_markers.py b/test/detect_all_markers.py index fb32636..aab01e6 100644 --- a/test/detect_all_markers.py +++ b/test/detect_all_markers.py @@ -59,11 +59,6 @@ def main(): target.pose = p target.pose.position.z += z_offset - # target.pose.orientation.x = 1.0 - # target.pose.orientation.y = 0.000651933 - # target.pose.orientation.z = 0.000017838 - # target.pose.orientation.w = 0.000445749 - targets.append(target) director.node.get_logger().info( diff --git a/test/go_above_markers.py b/test/go_above_markers.py index e6a57ac..2632b99 100644 --- a/test/go_above_markers.py +++ b/test/go_above_markers.py @@ -72,11 +72,6 @@ def main(): target.pose = p target.pose.position.z += z_offset - # target.pose.orientation.x = 1.0 - # target.pose.orientation.y = 0.000651933 - # target.pose.orientation.z = 0.000017838 - # target.pose.orientation.w = 0.000445749 - targets.append(target) director.node.get_logger().info( From 97822e06da70d12e817633cf6fe3d4eae3b3a04b Mon Sep 17 00:00:00 2001 From: AntoniRL Date: Tue, 24 Mar 2026 12:03:14 +0100 Subject: [PATCH 18/35] correct the test scripts --- src/object_pose_detector.cpp | 5 +---- test/detect_all_markers.py | 2 +- test/go_above_markers.py | 2 +- 3 files changed, 3 insertions(+), 6 deletions(-) diff --git a/src/object_pose_detector.cpp b/src/object_pose_detector.cpp index 4ee5174..24f3621 100644 --- a/src/object_pose_detector.cpp +++ b/src/object_pose_detector.cpp @@ -119,7 +119,7 @@ void ObjectPoseDetectorNode::onDetect( continue; } - pose_target.pose.position.z = 0.10; + pose_target.pose.position.z = 0.095; res->poses.poses.push_back(pose_target.pose); } @@ -142,11 +142,8 @@ void ObjectPoseDetectorNode::cameraInfoCb(const sensor_msgs::msg::CameraInfo::Sh dist_coeffs_.at(0, static_cast(i)) = msg->d[i]; } - cam_frame_ = msg->header.frame_id; camera_info_received_ = true; - RCLCPP_INFO(this->get_logger(), "Received camera info from topic."); - cam_info_sub_.reset(); } diff --git a/test/detect_all_markers.py b/test/detect_all_markers.py index aab01e6..e11c8d1 100644 --- a/test/detect_all_markers.py +++ b/test/detect_all_markers.py @@ -15,7 +15,7 @@ def main(): service_name = "/detect_blocks_poses" target_frame_fallback = "base_link" - z_offset = 0.01 + z_offset = 0.0 cli = director.node.create_client(DetectBlocksPoses, service_name) static_broadcaster = StaticTransformBroadcaster(director.node) diff --git a/test/go_above_markers.py b/test/go_above_markers.py index 2632b99..3a1d40d 100644 --- a/test/go_above_markers.py +++ b/test/go_above_markers.py @@ -33,7 +33,7 @@ def main(): service_name = "/detect_blocks_poses" target_frame_fallback = "base_link" - z_offset = 0.01 + z_offset = 0.005 wait_at_target_s = 3.0 cli = director.node.create_client(DetectBlocksPoses, service_name) From 434b0981c9965198818184d2579fb8e545eb6bf0 Mon Sep 17 00:00:00 2001 From: AntoniRL Date: Mon, 30 Mar 2026 14:17:00 +0200 Subject: [PATCH 19/35] add CHANGELOG --- CHANGELOG.md | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) create mode 100644 CHANGELOG.md diff --git a/CHANGELOG.md b/CHANGELOG.md new file mode 100644 index 0000000..dc03a6c --- /dev/null +++ b/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 From ff039806eb52c2a6e9024213cbb3d1b184f9c6d9 Mon Sep 17 00:00:00 2001 From: AntoniRL Date: Mon, 30 Mar 2026 14:44:41 +0200 Subject: [PATCH 20/35] cuncle the req in service --- include/scene_objects_manager/object_pose_detector.hpp | 2 +- src/object_pose_detector.cpp | 6 +----- srv/DetectBlocksPoses.srv | 3 --- 3 files changed, 2 insertions(+), 9 deletions(-) diff --git a/include/scene_objects_manager/object_pose_detector.hpp b/include/scene_objects_manager/object_pose_detector.hpp index 94c9b0f..34dffc9 100644 --- a/include/scene_objects_manager/object_pose_detector.hpp +++ b/include/scene_objects_manager/object_pose_detector.hpp @@ -35,7 +35,7 @@ class ObjectPoseDetectorNode : public rclcpp::Node { void imageCb(const sensor_msgs::msg::Image::SharedPtr msg); void onDetect( - const std::shared_ptr req, + const std::shared_ptr, std::shared_ptr res); void cameraInfoCb(const sensor_msgs::msg::CameraInfo::SharedPtr msg); diff --git a/src/object_pose_detector.cpp b/src/object_pose_detector.cpp index 24f3621..f22992d 100644 --- a/src/object_pose_detector.cpp +++ b/src/object_pose_detector.cpp @@ -46,12 +46,8 @@ void ObjectPoseDetectorNode::imageCb(const sensor_msgs::msg::Image::SharedPtr ms } void ObjectPoseDetectorNode::onDetect( - const std::shared_ptr req, + const std::shared_ptr, std::shared_ptr res) { - if (!req->detect) { - RCLCPP_INFO(this->get_logger(), "start_detection=false -> returning empty PoseArray (no detection)."); - return; - } if (!camera_info_received_) { RCLCPP_INFO(this->get_logger(), "Camera info not received yet."); return; diff --git a/srv/DetectBlocksPoses.srv b/srv/DetectBlocksPoses.srv index 90d8772..41666ff 100644 --- a/srv/DetectBlocksPoses.srv +++ b/srv/DetectBlocksPoses.srv @@ -1,6 +1,3 @@ -# Request -bool detect - --- # Response geometry_msgs/PoseArray poses From 81d2770b0d40b7a8911dfd211c06f978599248cc Mon Sep 17 00:00:00 2001 From: AntoniRL Date: Mon, 30 Mar 2026 15:13:36 +0200 Subject: [PATCH 21/35] order includes --- .clang-format | 73 +------------------ .../object_pose_detector.hpp | 45 ++++++------ src/scene_objects_manager.cpp | 15 ++-- 3 files changed, 30 insertions(+), 103 deletions(-) diff --git a/.clang-format b/.clang-format index d00892a..5ad3bba 100644 --- a/.clang-format +++ b/.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" diff --git a/include/scene_objects_manager/object_pose_detector.hpp b/include/scene_objects_manager/object_pose_detector.hpp index 34dffc9..1ce4a60 100644 --- a/include/scene_objects_manager/object_pose_detector.hpp +++ b/include/scene_objects_manager/object_pose_detector.hpp @@ -1,42 +1,43 @@ #ifndef SCENE_OBJECTS_MANAGER__OBJECT_POSE_DETECTOR_HPP_ #define SCENE_OBJECTS_MANAGER__OBJECT_POSE_DETECTOR_HPP_ -#include -#include -#include -#include -#include - #include #include #include -#include -#include -#include #include #include -#include #include -#include "ament_index_cpp/get_package_share_directory.hpp" -#include "geometry_msgs/msg/pose_array.hpp" -#include "rclcpp/rclcpp.hpp" -#include "scene_objects_manager/srv/detect_blocks_poses.hpp" -#include "sensor_msgs/msg/camera_info.hpp" -#include "sensor_msgs/msg/image.hpp" +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include namespace sobjmanager { class ObjectPoseDetectorNode : public rclcpp::Node { -public: + public: ObjectPoseDetectorNode(); -private: + private: void imageCb(const sensor_msgs::msg::Image::SharedPtr msg); - void onDetect( - const std::shared_ptr, - std::shared_ptr res); + void onDetect(const std::shared_ptr, + std::shared_ptr res); void cameraInfoCb(const sensor_msgs::msg::CameraInfo::SharedPtr msg); @@ -47,7 +48,7 @@ class ObjectPoseDetectorNode : public rclcpp::Node { std::mutex mtx_; std::optional last_rgb_; rclcpp::Time last_rgb_stamp_; - bool camera_info_received_{ false }; + bool camera_info_received_{false}; std::string image_topic_; std::string cam_info_topic_; diff --git a/src/scene_objects_manager.cpp b/src/scene_objects_manager.cpp index 83a7c4d..8fff684 100644 --- a/src/scene_objects_manager.cpp +++ b/src/scene_objects_manager.cpp @@ -1,7 +1,7 @@ #include "scene_objects_manager/scene_objects_manager.hpp" -#include "scene_objects_manager/object_pose_detector.hpp" #include "scene_objects_manager/args_parser.hpp" +#include "scene_objects_manager/object_pose_detector.hpp" #include "scene_objects_manager/spawner.hpp" #include "scene_objects_manager/yaml_parser.hpp" @@ -18,19 +18,14 @@ int main(int argc, char* argv[]) { auto detector_node = std::make_shared(); auto scene_objs = load_scene_objects_from_yaml(launch_args.cfg_path); - RCLCPP_INFO( - manager_node->get_logger(), - "Loaded %d object(s) from the configuration file.", - static_cast(scene_objs.size())); + RCLCPP_INFO(manager_node->get_logger(), "Loaded %d object(s) from the configuration file.", + static_cast(scene_objs.size())); auto spawner = Spawner(launch_args.planning_frame_id); for (const auto& obj : scene_objs) { spawner.spawn_object(obj); - RCLCPP_INFO( - manager_node->get_logger(), - "Spawned new scene object \"%s\" (unique_id: %s).", - obj.pretty_name.c_str(), - obj.unique_id.c_str()); + RCLCPP_INFO(manager_node->get_logger(), "Spawned new scene object \"%s\" (unique_id: %s).", obj.pretty_name.c_str(), + obj.unique_id.c_str()); } RCLCPP_INFO(manager_node->get_logger(), "Spawned all objects."); From e314e74f10281989dbbf12f7d23fb558f3ad07ed Mon Sep 17 00:00:00 2001 From: AntoniRL Date: Mon, 30 Mar 2026 15:39:38 +0200 Subject: [PATCH 22/35] snake_case --- .../object_pose_detector.hpp | 12 ++++--- src/object_pose_detector.cpp | 35 +++++++------------ 2 files changed, 20 insertions(+), 27 deletions(-) diff --git a/include/scene_objects_manager/object_pose_detector.hpp b/include/scene_objects_manager/object_pose_detector.hpp index 1ce4a60..68627cd 100644 --- a/include/scene_objects_manager/object_pose_detector.hpp +++ b/include/scene_objects_manager/object_pose_detector.hpp @@ -27,6 +27,8 @@ #include #include +using DetectBlocksPosesSrv = scene_objects_manager::srv::DetectBlocksPoses; + namespace sobjmanager { class ObjectPoseDetectorNode : public rclcpp::Node { @@ -34,12 +36,12 @@ class ObjectPoseDetectorNode : public rclcpp::Node { ObjectPoseDetectorNode(); private: - void imageCb(const sensor_msgs::msg::Image::SharedPtr msg); + void image_cb(const sensor_msgs::msg::Image::SharedPtr msg); - void onDetect(const std::shared_ptr, - std::shared_ptr res); + void on_detect(const std::shared_ptr, + std::shared_ptr res); - void cameraInfoCb(const sensor_msgs::msg::CameraInfo::SharedPtr msg); + void camera_info_cb(const sensor_msgs::msg::CameraInfo::SharedPtr msg); rclcpp::Subscription::SharedPtr image_sub_; rclcpp::Service::SharedPtr detect_blocks_srv_; @@ -48,7 +50,7 @@ class ObjectPoseDetectorNode : public rclcpp::Node { std::mutex mtx_; std::optional last_rgb_; rclcpp::Time last_rgb_stamp_; - bool camera_info_received_{false}; + bool camera_info_received_; std::string image_topic_; std::string cam_info_topic_; diff --git a/src/object_pose_detector.cpp b/src/object_pose_detector.cpp index f22992d..c0c71f2 100644 --- a/src/object_pose_detector.cpp +++ b/src/object_pose_detector.cpp @@ -9,28 +9,27 @@ ObjectPoseDetectorNode::ObjectPoseDetectorNode() : rclcpp::Node("object_pose_det output_frame_ = this->declare_parameter("output_frame", "base_link"); cam_frame_ = this->declare_parameter("cam_frame", "cam_scene_rgb_camera_optical_frame_cal"); + camera_info_received_ = false; + tf_buffer_ = std::make_unique(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); image_sub_ = this->create_subscription( - image_topic_, - rclcpp::SensorDataQoS(), - std::bind(&ObjectPoseDetectorNode::imageCb, this, std::placeholders::_1)); + image_topic_, rclcpp::SensorDataQoS(), std::bind(&ObjectPoseDetectorNode::image_cb, this, std::placeholders::_1)); cam_info_sub_ = this->create_subscription( - cam_info_topic_, - rclcpp::SensorDataQoS(), - std::bind(&ObjectPoseDetectorNode::cameraInfoCb, this, std::placeholders::_1)); + cam_info_topic_, rclcpp::SensorDataQoS(), + std::bind(&ObjectPoseDetectorNode::camera_info_cb, this, std::placeholders::_1)); detect_blocks_srv_ = this->create_service( "detect_blocks_poses", - std::bind(&ObjectPoseDetectorNode::onDetect, this, std::placeholders::_1, std::placeholders::_2)); + std::bind(&ObjectPoseDetectorNode::on_detect, this, std::placeholders::_1, std::placeholders::_2)); RCLCPP_INFO(this->get_logger(), "Detector node started. Subscriptionto: %s", image_topic_.c_str()); RCLCPP_INFO(this->get_logger(), "Service ready: /detect_blocks_poses"); } -void ObjectPoseDetectorNode::imageCb(const sensor_msgs::msg::Image::SharedPtr msg) { +void ObjectPoseDetectorNode::image_cb(const sensor_msgs::msg::Image::SharedPtr msg) { cv_bridge::CvImageConstPtr cv_ptr; try { cv_ptr = cv_bridge::toCvShare(msg, "bgr8"); @@ -45,9 +44,8 @@ void ObjectPoseDetectorNode::imageCb(const sensor_msgs::msg::Image::SharedPtr ms } } -void ObjectPoseDetectorNode::onDetect( - const std::shared_ptr, - std::shared_ptr res) { +void ObjectPoseDetectorNode::on_detect(const std::shared_ptr, + std::shared_ptr res) { if (!camera_info_received_) { RCLCPP_INFO(this->get_logger(), "Camera info not received yet."); return; @@ -85,16 +83,9 @@ void ObjectPoseDetectorNode::onDetect( pose_cam_.pose.position.z = tvecs[i][2]; cv::Mat R; cv::Rodrigues(rvecs[i], R); - tf2::Matrix3x3 tf3d( - R.at(0, 0), - R.at(0, 1), - R.at(0, 2), - R.at(1, 0), - R.at(1, 1), - R.at(1, 2), - R.at(2, 0), - R.at(2, 1), - R.at(2, 2)); + tf2::Matrix3x3 tf3d(R.at(0, 0), R.at(0, 1), R.at(0, 2), R.at(1, 0), + R.at(1, 1), R.at(1, 2), R.at(2, 0), R.at(2, 1), + R.at(2, 2)); double roll, pitch, yaw; tf3d.getRPY(roll, pitch, yaw); @@ -121,7 +112,7 @@ void ObjectPoseDetectorNode::onDetect( } } -void ObjectPoseDetectorNode::cameraInfoCb(const sensor_msgs::msg::CameraInfo::SharedPtr msg) { +void ObjectPoseDetectorNode::camera_info_cb(const sensor_msgs::msg::CameraInfo::SharedPtr msg) { if (camera_info_received_) { return; } From 11406abc0bcacda1353652761e7cd3dae7010559 Mon Sep 17 00:00:00 2001 From: AntoniRL Date: Mon, 30 Mar 2026 15:42:52 +0200 Subject: [PATCH 23/35] update message --- src/object_pose_detector.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/object_pose_detector.cpp b/src/object_pose_detector.cpp index c0c71f2..04a7aff 100644 --- a/src/object_pose_detector.cpp +++ b/src/object_pose_detector.cpp @@ -58,7 +58,7 @@ void ObjectPoseDetectorNode::on_detect(const std::shared_ptrget_logger(), "cv::imread failed"); + RCLCPP_ERROR(this->get_logger(), "Failed: no image in buffer to detect objects."); return; } From e01d774231caddc295a444ae78615395c223c685 Mon Sep 17 00:00:00 2001 From: AntoniRL Date: Mon, 30 Mar 2026 15:46:24 +0200 Subject: [PATCH 24/35] update tests --- src/object_pose_detector.cpp | 10 +++++----- test/detect_all_markers.py | 5 +---- test/go_above_markers.py | 5 +---- 3 files changed, 7 insertions(+), 13 deletions(-) diff --git a/src/object_pose_detector.cpp b/src/object_pose_detector.cpp index 04a7aff..1b92b47 100644 --- a/src/object_pose_detector.cpp +++ b/src/object_pose_detector.cpp @@ -66,15 +66,15 @@ void ObjectPoseDetectorNode::on_detect(const std::shared_ptrposes.header.frame_id = output_frame_; res->poses.poses.clear(); - std::vector markerIds; - std::vector> markerCorners, rejectedCandidates; + std::vector marker_ids; + std::vector> marker_corners, rejected_candidates; cv::Ptr parameters = cv::aruco::DetectorParameters::create(); cv::Ptr dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250); - cv::aruco::detectMarkers(img, dictionary, markerCorners, markerIds, parameters, rejectedCandidates); + cv::aruco::detectMarkers(img, dictionary, marker_corners, marker_ids, parameters, rejected_candidates); - cv::aruco::estimatePoseSingleMarkers(markerCorners, aruco_size_, camera_matrix_, dist_coeffs_, rvecs, tvecs); + cv::aruco::estimatePoseSingleMarkers(marker_corners, aruco_size_, camera_matrix_, dist_coeffs_, rvecs, tvecs); - for (size_t i = 0; i < markerIds.size(); i++) { + for (size_t i = 0; i < marker_ids.size(); i++) { geometry_msgs::msg::PoseStamped pose_cam_; pose_cam_.header.stamp = this->now(); pose_cam_.header.frame_id = cam_frame_; diff --git a/test/detect_all_markers.py b/test/detect_all_markers.py index e11c8d1..035b2ad 100644 --- a/test/detect_all_markers.py +++ b/test/detect_all_markers.py @@ -27,10 +27,7 @@ def main(): rclpy.shutdown() return - req = DetectBlocksPoses.Request() - req.detect = True - - future = cli.call_async(req) + future = cli.call_async() rclpy.spin_until_future_complete(director.node, future, timeout_sec=10.0) resp = future.result() diff --git a/test/go_above_markers.py b/test/go_above_markers.py index 3a1d40d..9c8c199 100644 --- a/test/go_above_markers.py +++ b/test/go_above_markers.py @@ -44,10 +44,7 @@ def main(): rclpy.shutdown() return - req = DetectBlocksPoses.Request() - req.detect = True - - future = cli.call_async(req) + future = cli.call_async() rclpy.spin_until_future_complete(director.node, future, timeout_sec=10.0) resp = future.result() From 6f40d0bbc247bd4f5052015b95b5682ffb7fe586 Mon Sep 17 00:00:00 2001 From: AntoniRL Date: Mon, 30 Mar 2026 15:49:33 +0200 Subject: [PATCH 25/35] update variable names --- include/scene_objects_manager/object_pose_detector.hpp | 3 ++- src/object_pose_detector.cpp | 10 +++++----- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/include/scene_objects_manager/object_pose_detector.hpp b/include/scene_objects_manager/object_pose_detector.hpp index 68627cd..75643e6 100644 --- a/include/scene_objects_manager/object_pose_detector.hpp +++ b/include/scene_objects_manager/object_pose_detector.hpp @@ -61,7 +61,8 @@ class ObjectPoseDetectorNode : public rclcpp::Node { cv::Mat camera_matrix_; cv::Mat dist_coeffs_; - std::vector rvecs, tvecs; + std::vector rvecs_; + std::vector tvecs_; std::unique_ptr tf_buffer_; std::shared_ptr tf_listener_; diff --git a/src/object_pose_detector.cpp b/src/object_pose_detector.cpp index 1b92b47..555dc15 100644 --- a/src/object_pose_detector.cpp +++ b/src/object_pose_detector.cpp @@ -72,17 +72,17 @@ void ObjectPoseDetectorNode::on_detect(const std::shared_ptr dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250); cv::aruco::detectMarkers(img, dictionary, marker_corners, marker_ids, parameters, rejected_candidates); - cv::aruco::estimatePoseSingleMarkers(marker_corners, aruco_size_, camera_matrix_, dist_coeffs_, rvecs, tvecs); + cv::aruco::estimatePoseSingleMarkers(marker_corners, aruco_size_, camera_matrix_, dist_coeffs_, rvecs_, tvecs_); for (size_t i = 0; i < marker_ids.size(); i++) { geometry_msgs::msg::PoseStamped pose_cam_; pose_cam_.header.stamp = this->now(); pose_cam_.header.frame_id = cam_frame_; - pose_cam_.pose.position.x = tvecs[i][0]; - pose_cam_.pose.position.y = tvecs[i][1]; - pose_cam_.pose.position.z = tvecs[i][2]; + pose_cam_.pose.position.x = tvecs_[i][0]; + pose_cam_.pose.position.y = tvecs_[i][1]; + pose_cam_.pose.position.z = tvecs_[i][2]; cv::Mat R; - cv::Rodrigues(rvecs[i], R); + cv::Rodrigues(rvecs_[i], R); tf2::Matrix3x3 tf3d(R.at(0, 0), R.at(0, 1), R.at(0, 2), R.at(1, 0), R.at(1, 1), R.at(1, 2), R.at(2, 0), R.at(2, 1), R.at(2, 2)); From 180ae3cb064b71709de25eea0d7b1f6e8df07d10 Mon Sep 17 00:00:00 2001 From: AntoniRL Date: Mon, 30 Mar 2026 16:05:22 +0200 Subject: [PATCH 26/35] upgrade the quesions --- src/object_pose_detector.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/object_pose_detector.cpp b/src/object_pose_detector.cpp index 555dc15..c9b8532 100644 --- a/src/object_pose_detector.cpp +++ b/src/object_pose_detector.cpp @@ -91,7 +91,7 @@ void ObjectPoseDetectorNode::on_detect(const std::shared_ptr Date: Mon, 30 Mar 2026 16:20:37 +0200 Subject: [PATCH 27/35] update allocating memory --- .../object_pose_detector.hpp | 1 + src/object_pose_detector.cpp | 19 +++++++++++-------- 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/include/scene_objects_manager/object_pose_detector.hpp b/include/scene_objects_manager/object_pose_detector.hpp index 75643e6..937c643 100644 --- a/include/scene_objects_manager/object_pose_detector.hpp +++ b/include/scene_objects_manager/object_pose_detector.hpp @@ -51,6 +51,7 @@ class ObjectPoseDetectorNode : public rclcpp::Node { std::optional 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_; diff --git a/src/object_pose_detector.cpp b/src/object_pose_detector.cpp index c9b8532..ab54488 100644 --- a/src/object_pose_detector.cpp +++ b/src/object_pose_detector.cpp @@ -10,6 +10,7 @@ ObjectPoseDetectorNode::ObjectPoseDetectorNode() : rclcpp::Node("object_pose_det cam_frame_ = this->declare_parameter("cam_frame", "cam_scene_rgb_camera_optical_frame_cal"); camera_info_received_ = false; + z_offset_of_block_ = 0.095; tf_buffer_ = std::make_unique(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); @@ -74,6 +75,13 @@ void ObjectPoseDetectorNode::on_detect(const std::shared_ptrnow(); @@ -81,19 +89,15 @@ void ObjectPoseDetectorNode::on_detect(const std::shared_ptr(0, 0), R.at(0, 1), R.at(0, 2), R.at(1, 0), R.at(1, 1), R.at(1, 2), R.at(2, 0), R.at(2, 1), R.at(2, 2)); - double roll, pitch, yaw; tf3d.getRPY(roll, pitch, yaw); - double step = M_PI / 2.0; - yaw -= step * std::round(yaw / step) - step; + yaw -= k_angle_step * std::round(yaw / k_angle_step) - k_angle_step; - tf2::Quaternion q_yaw_only; q_yaw_only.setRPY(0.0, 0.0, yaw); q_yaw_only.normalize(); pose_cam_.pose.orientation = tf2::toMsg(q_yaw_only); @@ -106,16 +110,15 @@ void ObjectPoseDetectorNode::on_detect(const std::shared_ptrposes.poses.push_back(pose_target.pose); } } void ObjectPoseDetectorNode::camera_info_cb(const sensor_msgs::msg::CameraInfo::SharedPtr msg) { - if (camera_info_received_) { + if (camera_info_received_) return; - } camera_matrix_ = cv::Mat(3, 3, CV_64F); for (int r = 0; r < 3; ++r) { From 0e7ccc7453d5e8307ba12446213da04d0b72103d Mon Sep 17 00:00:00 2001 From: AntoniRL Date: Mon, 30 Mar 2026 16:22:43 +0200 Subject: [PATCH 28/35] update names of variables --- src/object_pose_detector.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/object_pose_detector.cpp b/src/object_pose_detector.cpp index ab54488..7f34c0d 100644 --- a/src/object_pose_detector.cpp +++ b/src/object_pose_detector.cpp @@ -121,9 +121,9 @@ void ObjectPoseDetectorNode::camera_info_cb(const sensor_msgs::msg::CameraInfo:: return; camera_matrix_ = cv::Mat(3, 3, CV_64F); - for (int r = 0; r < 3; ++r) { - for (int c = 0; c < 3; ++c) { - camera_matrix_.at(r, c) = msg->k[r * 3 + c]; + for (int row = 0; row < 3; ++row) { + for (int col = 0; col < 3; ++col) { + camera_matrix_.at(row, col) = msg->k[row * 3 + col]; } } From 431ac100e1579c6d02bda596880809a1b4e79131 Mon Sep 17 00:00:00 2001 From: AntoniRL Date: Mon, 30 Mar 2026 16:26:25 +0200 Subject: [PATCH 29/35] standarize for loops --- src/object_pose_detector.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/object_pose_detector.cpp b/src/object_pose_detector.cpp index 7f34c0d..3bf8390 100644 --- a/src/object_pose_detector.cpp +++ b/src/object_pose_detector.cpp @@ -121,14 +121,14 @@ void ObjectPoseDetectorNode::camera_info_cb(const sensor_msgs::msg::CameraInfo:: return; camera_matrix_ = cv::Mat(3, 3, CV_64F); - for (int row = 0; row < 3; ++row) { - for (int col = 0; col < 3; ++col) { + for (int row = 0; row < 3; row++) { + for (int col = 0; col < 3; col++) { camera_matrix_.at(row, col) = msg->k[row * 3 + col]; } } dist_coeffs_ = cv::Mat(1, static_cast(msg->d.size()), CV_64F); - for (size_t i = 0; i < msg->d.size(); ++i) { + for (size_t i = 0; i < msg->d.size(); i++) { dist_coeffs_.at(0, static_cast(i)) = msg->d[i]; } From b495abfbabef52b183cb87fb0821b9b4740dbe05 Mon Sep 17 00:00:00 2001 From: AntoniRL Date: Mon, 30 Mar 2026 16:28:30 +0200 Subject: [PATCH 30/35] delate all comments --- src/scene_objects_manager.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/scene_objects_manager.cpp b/src/scene_objects_manager.cpp index 8fff684..2676c64 100644 --- a/src/scene_objects_manager.cpp +++ b/src/scene_objects_manager.cpp @@ -13,7 +13,6 @@ int main(int argc, char* argv[]) { return parse_args("SceneObjectsSpawner", "0.0.0", args); }(); - // auto const node = std::make_shared(); auto manager_node = std::make_shared(); auto detector_node = std::make_shared(); @@ -34,7 +33,6 @@ int main(int argc, char* argv[]) { executor_.add_node(detector_node); executor_.spin(); - // rclcpp::spin(node); rclcpp::shutdown(); return 0; } From 4c05fcbb9961ba87d15552f8317c986998c4e9eb Mon Sep 17 00:00:00 2001 From: AntoniRL Date: Mon, 30 Mar 2026 16:32:23 +0200 Subject: [PATCH 31/35] sort aplhabetically --- CMakeLists.txt | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1a5e0e3..edc0356 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,18 +9,18 @@ endif() find_package(ament_cmake REQUIRED) find_package(ament_index_cpp REQUIRED) find_package(rclcpp REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(geometry_msgs 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(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) +find_package(yaml-cpp REQUIRED) include(FetchContent) FetchContent_Declare(argparse From 0090414adfa61902fa17130c74a95406c5774b1b Mon Sep 17 00:00:00 2001 From: AntoniRL Date: Mon, 30 Mar 2026 16:36:40 +0200 Subject: [PATCH 32/35] update CMakeListas --- CMakeLists.txt | 30 ++++++++++++++++++------------ 1 file changed, 18 insertions(+), 12 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index edc0356..26239de 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,21 +6,27 @@ 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(cv_bridge REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(moveit_msgs REQUIRED) -find_package(moveit_ros_planning_interface REQUIRED) -find_package(OpenCV REQUIRED) -find_package(rosidl_default_generators REQUIRED) -find_package(std_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(tf2_geometry_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 From e5691aa6c028daa8113d2b58fcadb5eb08f33dbb Mon Sep 17 00:00:00 2001 From: AntoniRL Date: Tue, 31 Mar 2026 14:39:16 +0200 Subject: [PATCH 33/35] repair tests --- CMakeLists.txt | 2 -- test/detect_all_markers.py | 3 ++- test/go_above_markers.py | 3 ++- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 26239de..a34d068 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -70,8 +70,6 @@ target_include_directories( 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) diff --git a/test/detect_all_markers.py b/test/detect_all_markers.py index 035b2ad..bbd1453 100644 --- a/test/detect_all_markers.py +++ b/test/detect_all_markers.py @@ -27,7 +27,8 @@ def main(): rclpy.shutdown() return - future = cli.call_async() + req = DetectBlocksPoses.Request() + future = cli.call_async(req) rclpy.spin_until_future_complete(director.node, future, timeout_sec=10.0) resp = future.result() diff --git a/test/go_above_markers.py b/test/go_above_markers.py index 9c8c199..279812b 100644 --- a/test/go_above_markers.py +++ b/test/go_above_markers.py @@ -44,7 +44,8 @@ def main(): rclpy.shutdown() return - future = cli.call_async() + req = DetectBlocksPoses.Request() + future = cli.call_async(req) rclpy.spin_until_future_complete(director.node, future, timeout_sec=10.0) resp = future.result() From 662df35f9a80abd21418ae40473fedfa21bacd97 Mon Sep 17 00:00:00 2001 From: AntoniRL Date: Tue, 31 Mar 2026 14:39:54 +0200 Subject: [PATCH 34/35] delate config --- config/object_pose_detector_config.yaml | 7 ------- 1 file changed, 7 deletions(-) delete mode 100644 config/object_pose_detector_config.yaml diff --git a/config/object_pose_detector_config.yaml b/config/object_pose_detector_config.yaml deleted file mode 100644 index ada5324..0000000 --- a/config/object_pose_detector_config.yaml +++ /dev/null @@ -1,7 +0,0 @@ -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" From db6a7391138722bb672b8c4589908176e8c72317 Mon Sep 17 00:00:00 2001 From: AntoniRL Date: Wed, 1 Apr 2026 12:21:11 +0200 Subject: [PATCH 35/35] prek run all files --- include/scene_objects_manager/args_parser.hpp | 7 +++---- include/scene_objects_manager/scene_object.hpp | 4 ++-- include/scene_objects_manager/scene_objects_manager.hpp | 2 +- include/scene_objects_manager/spawner.hpp | 4 ++-- src/args_parser.cpp | 7 +++---- src/object_spawner.cpp | 5 ++--- src/spawner.cpp | 5 ++--- 7 files changed, 15 insertions(+), 19 deletions(-) diff --git a/include/scene_objects_manager/args_parser.hpp b/include/scene_objects_manager/args_parser.hpp index 11af868..c92d242 100644 --- a/include/scene_objects_manager/args_parser.hpp +++ b/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& input_args); +const LaunchArguments parse_args(const std::string& program_name, + const std::string& program_version, + const std::vector& input_args); } // namespace sobjmanager #endif // SCENE_OBJECTS_MANAGER__ARGS_PARSER_HPP_ diff --git a/include/scene_objects_manager/scene_object.hpp b/include/scene_objects_manager/scene_object.hpp index d25e70e..621c9f4 100644 --- a/include/scene_objects_manager/scene_object.hpp +++ b/include/scene_objects_manager/scene_object.hpp @@ -25,8 +25,8 @@ struct SceneObject { Pose pose; static const inline std::map 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 PRIMITIVE_UINT_MAP = reverse_map(PRIMITIVE_STR_MAP); }; diff --git a/include/scene_objects_manager/scene_objects_manager.hpp b/include/scene_objects_manager/scene_objects_manager.hpp index 2d035ec..518399c 100644 --- a/include/scene_objects_manager/scene_objects_manager.hpp +++ b/include/scene_objects_manager/scene_objects_manager.hpp @@ -12,7 +12,7 @@ namespace sobjmanager { class SceneObjectsManager : public rclcpp::Node { -public: + public: SceneObjectsManager() : Node("scene_objects_manager") {}; }; diff --git a/include/scene_objects_manager/spawner.hpp b/include/scene_objects_manager/spawner.hpp index f8b3ec6..b5bbe7d 100644 --- a/include/scene_objects_manager/spawner.hpp +++ b/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); diff --git a/src/args_parser.cpp b/src/args_parser.cpp index cc29256..eb38322 100644 --- a/src/args_parser.cpp +++ b/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& input_args) { +const LaunchArguments parse_args(const std::string& program_name, + const std::string& program_version, + const std::vector& input_args) { argparse::ArgumentParser program(program_name, program_version, argparse::default_arguments::help); LaunchArguments args; diff --git a/src/object_spawner.cpp b/src/object_spawner.cpp index 1579a3f..d62cb33 100644 --- a/src/object_spawner.cpp +++ b/src/object_spawner.cpp @@ -3,9 +3,8 @@ namespace sobjmanager { ObjectsSpawner::ObjectsSpawner(std::string planning_frame_id) - : _planning_frame_id(planning_frame_id) - , _planning_scene_interface(moveit::planning_interface::PlanningSceneInterface()) { -} + : _planning_frame_id(planning_frame_id), + _planning_scene_interface(moveit::planning_interface::PlanningSceneInterface()) {} void ObjectsSpawner::spawn_object(const SceneObject& obj) { spawn(make_collision_object(obj)); diff --git a/src/spawner.cpp b/src/spawner.cpp index 04992f8..f640ff1 100644 --- a/src/spawner.cpp +++ b/src/spawner.cpp @@ -3,9 +3,8 @@ namespace sobjmanager { Spawner::Spawner(std::string planning_frame_id) - : _planning_frame_id(planning_frame_id) - , _planning_scene_interface(moveit::planning_interface::PlanningSceneInterface()) { -} + : _planning_frame_id(planning_frame_id), + _planning_scene_interface(moveit::planning_interface::PlanningSceneInterface()) {} void Spawner::spawn_object(const SceneObject& obj) { spawn(make_collision_object(obj));