Base

.gitignore

@@ -0,0 +1,144 @@
+# Created by https://www.toptal.com/developers/gitignore/api/c++,visualstudiocode,ros2,ros,cmake
+# Edit at https://www.toptal.com/developers/gitignore?templates=c++,visualstudiocode,ros2,ros,cmake
+
+### C++ ###
+# Prerequisites
+*.d
+
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
+
+### CMake ###
+CMakeLists.txt.user
+CMakeCache.txt
+CMakeFiles
+CMakeScripts
+Testing
+Makefile
+cmake_install.cmake
+install_manifest.txt
+compile_commands.json
+CTestTestfile.cmake
+_deps
+
+### CMake Patch ###
+# External projects
+*-prefix/
+
+### ROS ###
+devel/
+logs/
+build/
+bin/
+lib/
+msg_gen/
+srv_gen/
+msg/*Action.msg
+msg/*ActionFeedback.msg
+msg/*ActionGoal.msg
+msg/*ActionResult.msg
+msg/*Feedback.msg
+msg/*Goal.msg
+msg/*Result.msg
+msg/_*.py
+build_isolated/
+devel_isolated/
+
+# Generated by dynamic reconfigure
+*.cfgc
+/cfg/cpp/
+/cfg/*.py
+
+# Ignore generated docs
+*.dox
+*.wikidoc
+
+# eclipse stuff
+.project
+.cproject
+
+# qcreator stuff
+
+srv/_*.py
+*.pcd
+*.pyc
+qtcreator-*
+*.user
+
+/planning/cfg
+/planning/docs
+/planning/src
+
+*~
+
+# Emacs
+.#*
+
+# Catkin custom files
+CATKIN_IGNORE
+
+### ROS2 ###
+install/
+log/
+
+# Ignore generated docs
+
+# eclipse stuff
+
+# qcreator stuff
+
+
+
+# Emacs
+
+# Colcon custom files
+COLCON_IGNORE
+AMENT_IGNORE
+
+### VisualStudioCode ###
+.vscode/*
+!.vscode/settings.json
+!.vscode/tasks.json
+!.vscode/launch.json
+!.vscode/extensions.json
+!.vscode/*.code-snippets
+
+# Local History for Visual Studio Code
+.history/
+
+# Built Visual Studio Code Extensions
+*.vsix
+
+### VisualStudioCode Patch ###
+# Ignore all local history of files
+.history
+.ionide
+
+# End of https://www.toptal.com/developers/gitignore/api/c++,visualstudiocode,ros2,ros,cmake

CMakeLists.txt

@@ -0,0 +1,98 @@
+cmake_minimum_required(VERSION 3.5)
+project(aruco_fractal_tracker)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(cv_bridge REQUIRED)
+find_package(aruco REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+include_directories(include)
+
+add_library(aruco_fractal_tracker_node SHARED src/aruco_fractal_tracker_node.cpp)
+target_link_libraries(aruco_fractal_tracker_node aruco)
+ament_target_dependencies(aruco_fractal_tracker_node 
+  rclcpp 
+  cv_bridge 
+  geometry_msgs 
+  sensor_msgs 
+  rclcpp_components 
+  aruco 
+  OpenCV
+  tf2
+  tf2_geometry_msgs
+)
+rclcpp_components_register_nodes(aruco_fractal_tracker_node "fractal_tracker::ArucoFractalTracker")
+
+add_executable(aruco_fractal_tracker src/aruco_fractal_tracker_node_main.cpp)
+target_link_libraries(aruco_fractal_tracker aruco_fractal_tracker_node)
+
+install(
+  DIRECTORY include/aruco_fractal_tracker
+  DESTINATION include
+)
+
+install(
+  TARGETS aruco_fractal_tracker_node
+  EXPORT export_aruco_fractal_tracker_node
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES DESTINATION include
+)
+
+install(
+  TARGETS aruco_fractal_tracker
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_libraries(aruco_fractal_tracker_node)
+
+ament_export_dependencies(
+  rclcpp 
+  geometry_msgs 
+  sensor_msgs 
+  rclcpp_components 
+  cv_bridge 
+  aruco
+  OpenCV
+  tf2
+  tf2_geometry_msgs
+)
+
+ament_package()

example_marker/marker_configuration.yaml

@@ -0,0 +1,38 @@
+%YAML:1.0
+---
+codeid: fractalmarkers
+mInfoType: 2
+fractal_levels: 3
+fractal_external_id: 0
+markers:
+   - { id:0, bits:[ 0, 0, 1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0,
+       0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 0, 0,
+       0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 1, 0, 0, 0, 0, 0, 0,
+       0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+       0, 0, 0, 0, 1, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+       1, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 1,
+       0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0,
+       0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0,
+       0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+       0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1,
+       1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0,
+       0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 1, 0, 0, 0, 0, 0, 0,
+       0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+       0, 0, 0, 0, 1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+       1, 1, 1, 0, 1, 1, 1, 0, 1, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 1 ],
+       corners:[ [ -1., 1., 0. ], [ 1., 1., 0. ], [ 1., -1., 0. ], [ -1.,
+       -1., 0. ] ], submarkers_id:[ 1 ] }
+   - { id:1, bits:[ 0, 1, 0, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 1,
+       0, 0, 0, 0, 1, 1, 0, 1, 0, 0, 0, 0, 1, 0, 1, 1, 0, 0, 0, 0, 1, 0,
+       1, 1, 0, 0, 0, 0, 1, 0, 1, 1, 1, 1, 1, 1, 1, 0, 0, 1, 0, 0, 1, 1,
+       0, 1 ], corners:[ [ -6.9999998807907104e-01,
+       6.9999998807907104e-01, 0. ], [ 6.9999998807907104e-01,
+       6.9999998807907104e-01, 0. ], [ 6.9999998807907104e-01,
+       -6.9999998807907104e-01, 0. ], [ -6.9999998807907104e-01,
+       -6.9999998807907104e-01, 0. ] ], submarkers_id:[ 2 ] }
+   - { id:2, bits:[ 1, 1, 0, 1, 0, 1, 0, 0, 0 ], corners:[ [
+       -2.8000000119209290e-01, 2.8000000119209290e-01, 0. ], [
+       2.8000000119209290e-01, 2.8000000119209290e-01, 0. ], [
+       2.8000000119209290e-01, -2.8000000119209290e-01, 0. ], [
+       -2.8000000119209290e-01, -2.8000000119209290e-01, 0. ] ],
+       submarkers_id:[] }

include/aruco_fractal_tracker/aruco_fractal_tracker_node.hpp

@@ -0,0 +1,34 @@
+#ifndef ARUCO_FRACTAL_TRACKER__ARUCO_FRACTAL_TRACKER_NODE_HPP_
+#define ARUCO_FRACTAL_TRACKER__ARUCO_FRACTAL_TRACKER_NODE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <sensor_msgs/msg/image.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <tf2_ros/transform_broadcaster.h>
+
+#include <aruco/fractaldetector.h>
+
+#include <memory>
+
+namespace fractal_tracker
+{
+class ArucoFractalTracker : public rclcpp::Node
+{
+public:
+  explicit ArucoFractalTracker(const rclcpp::NodeOptions& options);
+
+private:  
+  aruco::FractalDetector detector_;
+
+  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
+
+  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_pub_;
+  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr marker_pose_pub_;
+  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+
+  void imageCallback(const sensor_msgs::msg::Image::SharedPtr msg);
+
+}; // class ArucoFractalTracker
+}  // namespace fractal_tracker
+#endif  // ARUCO_FRACTAL_TRACKER__ARUCO_FRACTAL_TRACKER_NODE_HPP_

package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>aruco_fractal_tracker</name>
+  <version>1.0.0</version>
+  <description>ROS2 package designed to to detect and estimate pose of ArUco fractal markers.</description>
+  <maintainer email="dmitry.anikin@proton.me">Dmitry Anikin</maintainer>
+  <license>GPL-3.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>cv_bridge</depend>
+  <depend>opencv4</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>