diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..11f750c --- /dev/null +++ b/.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 diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100755 index 0000000..b2fe8bb --- /dev/null +++ b/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( 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() diff --git a/example_marker/marker.png b/example_marker/marker.png new file mode 100644 index 0000000..18c2ee1 Binary files /dev/null and b/example_marker/marker.png differ diff --git a/example_marker/marker_configuration.yaml b/example_marker/marker_configuration.yaml new file mode 100644 index 0000000..2d9fc14 --- /dev/null +++ b/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:[] } diff --git a/include/aruco_fractal_tracker/aruco_fractal_tracker_node.hpp b/include/aruco_fractal_tracker/aruco_fractal_tracker_node.hpp new file mode 100755 index 0000000..ca2ddb5 --- /dev/null +++ b/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 + +#include +#include +#include + +#include + +#include + +namespace fractal_tracker +{ +class ArucoFractalTracker : public rclcpp::Node +{ +public: + explicit ArucoFractalTracker(const rclcpp::NodeOptions& options); + +private: + aruco::FractalDetector detector_; + + rclcpp::Subscription::SharedPtr image_sub_; + + rclcpp::Publisher::SharedPtr image_pub_; + rclcpp::Publisher::SharedPtr marker_pose_pub_; + std::shared_ptr 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_ diff --git a/package.xml b/package.xml new file mode 100755 index 0000000..443ecad --- /dev/null +++ b/package.xml @@ -0,0 +1,28 @@ + + + + aruco_fractal_tracker + 1.0.0 + ROS2 package designed to to detect and estimate pose of ArUco fractal markers. + Dmitry Anikin + GPL-3.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + rclcpp + rclcpp_components + sensor_msgs + geometry_msgs + cv_bridge + opencv4 + tf2 + tf2_geometry_msgs + tf2_ros + + + ament_cmake + + diff --git a/src/aruco_fractal_tracker_node.cpp b/src/aruco_fractal_tracker_node.cpp new file mode 100755 index 0000000..49172c4 --- /dev/null +++ b/src/aruco_fractal_tracker_node.cpp @@ -0,0 +1,173 @@ +#include "aruco_fractal_tracker/aruco_fractal_tracker_node.hpp" + +#include +#include + +#include +#include + +#include +#include +#include + +namespace fractal_tracker +{ +ArucoFractalTracker::ArucoFractalTracker(const rclcpp::NodeOptions &options) + : Node("aruco_fractal_tracker", options) +{ + this->declare_parameter("marker_configuration", ""); + this->declare_parameter("marker_size", 0.0); + this->declare_parameter("cam_params_file", ""); + + auto cam_params_file = this->get_parameter("cam_params_file").get_value(); + aruco::CameraParameters cam_params; + cam_params.readFromXMLFile(cam_params_file); + if (!cam_params.isValid()) + throw std::invalid_argument("Invalid camera parameters!"); + + image_sub_ = this->create_subscription( + "image_input_topic", 10, std::bind(&ArucoFractalTracker::imageCallback, this, std::placeholders::_1)); + + image_pub_ = this->create_publisher("image_output_topic", 10); + + marker_pose_pub_ = this->create_publisher( + "poses_output_topic", 10); + + auto marker_configuration = this->get_parameter("marker_configuration").get_value(); + auto marker_size = this->get_parameter("marker_size").get_value(); + + detector_.setConfiguration(marker_configuration); + detector_.setParams(cam_params, marker_size); + + tf_broadcaster_ = std::make_shared(*this); +} + +void ArucoFractalTracker::imageCallback(const sensor_msgs::msg::Image::SharedPtr msg) +{ + cv_bridge::CvImagePtr cv_ptr; + cv::Mat gray; + + try + { + cv_ptr = cv_bridge::toCvCopy(msg); + } + catch (cv_bridge::Exception& e) + { + RCLCPP_ERROR_STREAM(this->get_logger(), "cv_bridge exception: " << e.what()); + return; + } + + cv::cvtColor(cv_ptr->image, gray, CV_BGR2GRAY); + + if (detector_.detect(gray)) + { + detector_.drawMarkers(cv_ptr->image); + + std::vector markers = detector_.getMarkers(); + for (auto&& marker : markers) + marker.draw(cv_ptr->image, cv::Scalar(255, 255, 255), 2); + + detector_.draw2d(cv_ptr->image); + + if (detector_.poseEstimation()) + { + cv::Mat tvec = detector_.getTvec(); + cv::Mat rvec = detector_.getRvec(); + detector_.draw3d(cv_ptr->image); + + cv::Mat rmatrix; + cv::Rodrigues(rvec, rmatrix); + tf2::Matrix3x3 tf2_rot(rmatrix.at(0, 0), rmatrix.at(0, 1), rmatrix.at(0, 2), + rmatrix.at(1, 0), rmatrix.at(1, 1), rmatrix.at(1, 2), + rmatrix.at(2, 0), rmatrix.at(2, 1), rmatrix.at(2, 2)); + + tf2::Vector3 tf2_translation(tvec.at(0, 0), tvec.at(1, 0), tvec.at(2, 0)); + tf2::Transform tf2_transform(tf2_rot, tf2_translation); + tf2::Quaternion quat; + tf2_rot.getRotation(quat); + + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = msg->header.frame_id; + pose.header.stamp = this->get_clock()->now(); + pose.pose.position.x = tf2_translation.x(); + pose.pose.position.y = tf2_translation.y(); + pose.pose.position.z = tf2_translation.z(); + pose.pose.orientation.x = quat.getX(); + pose.pose.orientation.y = quat.getY(); + pose.pose.orientation.z = quat.getZ(); + pose.pose.orientation.w = quat.getW(); + + marker_pose_pub_->publish(pose); + + + int base_line = 0; + int font_face = cv::FONT_HERSHEY_PLAIN; + double font_scale = 1; + int thickness = 1; + int line_height = cv::getTextSize("W", font_face, font_scale, thickness, &base_line).height + 5; + + cv::Point pos_text_pos(10, 10 + line_height); + cv::putText(cv_ptr->image, "POSITION", pos_text_pos, font_face, font_scale, cv::Scalar(255, 255, 255), 3, cv::LINE_AA); + cv::putText(cv_ptr->image, "POSITION", pos_text_pos, font_face, font_scale, cv::Scalar(0, 0, 0), 1, cv::LINE_AA); + pos_text_pos.y += line_height; + cv::putText(cv_ptr->image, cv::format("X=%.2f", tf2_translation.x()), pos_text_pos, font_face, font_scale, cv::Scalar(255, 255, 255), 3, cv::LINE_AA); + cv::putText(cv_ptr->image, cv::format("X=%.2f", tf2_translation.x()), pos_text_pos, font_face, font_scale, cv::Scalar(0, 0, 0), 1, cv::LINE_AA); + pos_text_pos.y += line_height; + cv::putText(cv_ptr->image, cv::format("Y=%.2f", tf2_translation.y()), pos_text_pos, font_face, font_scale, cv::Scalar(255, 255, 255), 3, cv::LINE_AA); + cv::putText(cv_ptr->image, cv::format("Y=%.2f", tf2_translation.y()), pos_text_pos, font_face, font_scale, cv::Scalar(0, 0, 0), 1, cv::LINE_AA); + pos_text_pos.y += line_height; + cv::putText(cv_ptr->image, cv::format("Z=%.2f", tf2_translation.z()), pos_text_pos, font_face, font_scale, cv::Scalar(255, 255, 255), 3, cv::LINE_AA); + cv::putText(cv_ptr->image, cv::format("Z=%.2f", tf2_translation.z()), pos_text_pos, font_face, font_scale, cv::Scalar(0, 0, 0), 1, cv::LINE_AA); + + cv::Point ori_text_pos(cv_ptr->image.cols - 150, 10 + line_height); + cv::putText(cv_ptr->image, "ORIENTATION", ori_text_pos, font_face, font_scale, cv::Scalar(255, 255, 255), 3, cv::LINE_AA); + cv::putText(cv_ptr->image, "ORIENTATION", ori_text_pos, font_face, font_scale, cv::Scalar(0, 0, 0), 1, cv::LINE_AA); + ori_text_pos.y += line_height; + cv::putText(cv_ptr->image, cv::format("X=%.2f", quat.getX()), ori_text_pos, font_face, font_scale, cv::Scalar(255, 255, 255), 3, cv::LINE_AA); + cv::putText(cv_ptr->image, cv::format("X=%.2f", quat.getX()), ori_text_pos, font_face, font_scale, cv::Scalar(0, 0, 0), 1, cv::LINE_AA); + ori_text_pos.y += line_height; + cv::putText(cv_ptr->image, cv::format("Y=%.2f", quat.getY()), ori_text_pos, font_face, font_scale, cv::Scalar(255, 255, 255), 3, cv::LINE_AA); + cv::putText(cv_ptr->image, cv::format("Y=%.2f", quat.getY()), ori_text_pos, font_face, font_scale, cv::Scalar(0, 0, 0), 1, cv::LINE_AA); + ori_text_pos.y += line_height; + cv::putText(cv_ptr->image, cv::format("Z=%.2f", quat.getZ()), ori_text_pos, font_face, font_scale, cv::Scalar(255, 255, 255), 3, cv::LINE_AA); + cv::putText(cv_ptr->image, cv::format("Z=%.2f", quat.getZ()), ori_text_pos, font_face, font_scale, cv::Scalar(0, 0, 0), 1, cv::LINE_AA); + ori_text_pos.y += line_height; + cv::putText(cv_ptr->image, cv::format("W=%.2f", quat.getW()), ori_text_pos, font_face, font_scale, cv::Scalar(255, 255, 255), 3, cv::LINE_AA); + cv::putText(cv_ptr->image, cv::format("W=%.2f", quat.getW()), ori_text_pos, font_face, font_scale, cv::Scalar(0, 0, 0), 1, cv::LINE_AA); + + geometry_msgs::msg::TransformStamped transform; + transform.header.stamp = this->get_clock()->now(); + transform.header.frame_id = msg->header.frame_id; + transform.child_frame_id = "marker_frame"; + transform.transform.translation.x = tf2_translation.x(); + transform.transform.translation.y = tf2_translation.y(); + transform.transform.translation.z = tf2_translation.z(); + transform.transform.rotation.x = quat.getX(); + transform.transform.rotation.y = quat.getY(); + transform.transform.rotation.z = quat.getZ(); + transform.transform.rotation.w = quat.getW(); + + tf_broadcaster_->sendTransform(transform); + } + } + else + { + cv::putText(cv_ptr->image, "NOT FOUND", cv::Point(20, 30), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(255, 255, 255), 3, cv::LINE_AA); + cv::putText(cv_ptr->image, "NOT FOUND", cv::Point(20, 30), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0, 0, 0), 1, cv::LINE_AA); + } + + try + { + image_pub_->publish(*cv_ptr->toImageMsg()); + } + catch (cv_bridge::Exception& e) + { + RCLCPP_ERROR_STREAM(this->get_logger(), "cv_bridge exception: " << e.what()); + return; + } +} + +} // namespace ssl + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(fractal_tracker::ArucoFractalTracker) \ No newline at end of file diff --git a/src/aruco_fractal_tracker_node_main.cpp b/src/aruco_fractal_tracker_node_main.cpp new file mode 100755 index 0000000..f9be6a0 --- /dev/null +++ b/src/aruco_fractal_tracker_node_main.cpp @@ -0,0 +1,12 @@ +#include "aruco_fractal_tracker/aruco_fractal_tracker_node.hpp" + + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + rclcpp::spin(std::make_shared(options)); + rclcpp::shutdown(); + + return 0; +} \ No newline at end of file