diff --git a/aruco/package.xml b/aruco/package.xml index 71c269a..4012e62 100644 --- a/aruco/package.xml +++ b/aruco/package.xml @@ -1,7 +1,7 @@ aruco - 5.0.4 + 5.1.0 The ARUCO Library has been developed by the Ava group of the Univeristy of Cordoba(Spain). It provides real-time marker based 3D pose estimation using AR markers. diff --git a/aruco_msgs/package.xml b/aruco_msgs/package.xml index 9136fc4..5164d7e 100644 --- a/aruco_msgs/package.xml +++ b/aruco_msgs/package.xml @@ -1,7 +1,7 @@ aruco_msgs - 5.0.4 + 5.1.0 The aruco_msgs package Sai Kishor Kothakota Bence Magyar diff --git a/aruco_ros/CMakeLists.txt b/aruco_ros/CMakeLists.txt index 5f6f4a9..da685c3 100644 --- a/aruco_ros/CMakeLists.txt +++ b/aruco_ros/CMakeLists.txt @@ -4,6 +4,12 @@ project(aruco_ros) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() + +# compile release by default +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release CACHE STRING "Build type." FORCE) +endif() + set(CMAKE_CXX_STANDARD 11) # C++11... set(CMAKE_CXX_STANDARD_REQUIRED ON) #...is required... set(CMAKE_CXX_EXTENSIONS ON) #...with compiler extensions like gnu++11 @@ -14,6 +20,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS geometry_msgs image_transport rclcpp + rclcpp_components rclpy tf2 tf2_ros @@ -44,19 +51,25 @@ target_include_directories(aruco_ros_utils ament_target_dependencies(aruco_ros_utils ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_link_libraries(aruco_ros_utils ${OpenCV_LIBRARIES}) -add_executable(single src/simple_single.cpp - src/aruco_ros_utils.cpp) +add_library(single_component + SHARED + src/simple_single.cpp + src/aruco_ros_utils.cpp) -target_include_directories(single +target_include_directories(single_component PUBLIC include) -target_include_directories(single +target_include_directories(single_component SYSTEM PUBLIC ${OpenCV_INCLUDE_DIRS} ) -ament_target_dependencies(single ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(single ${OpenCV_LIBRARIES}) +ament_target_dependencies(single_component ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(single_component ${OpenCV_LIBRARIES}) + +rclcpp_components_register_node(single_component + PLUGIN aruco_ros::ArUcoSimple + EXECUTABLE single) add_executable(double src/simple_double.cpp src/aruco_ros_utils.cpp) @@ -71,18 +84,25 @@ target_include_directories(double ament_target_dependencies(double ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_link_libraries(double ${OpenCV_LIBRARIES}) -add_executable(marker_publisher src/marker_publish.cpp - src/aruco_ros_utils.cpp) -target_include_directories(marker_publisher +add_library(marker_publisher_component + SHARED + src/marker_publish.cpp + src/aruco_ros_utils.cpp) + +target_include_directories(marker_publisher_component PUBLIC include) -target_include_directories(marker_publisher +target_include_directories(marker_publisher_component SYSTEM PUBLIC ${OpenCV_INCLUDE_DIRS} ) -ament_target_dependencies(marker_publisher ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(marker_publisher ${OpenCV_LIBRARIES}) +ament_target_dependencies(marker_publisher_component ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(marker_publisher_component ${OpenCV_LIBRARIES}) + +rclcpp_components_register_node(marker_publisher_component + PLUGIN aruco_ros::ArUcoMarkerPublisher + EXECUTABLE marker_publisher) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -102,8 +122,10 @@ install(TARGETS aruco_ros_utils LIBRARY DESTINATION lib RUNTIME DESTINATION bin) -install(TARGETS marker_publisher single double - DESTINATION lib/${PROJECT_NAME}) +install(TARGETS marker_publisher_component single_component double + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME}) + install(DIRECTORY include/ DESTINATION include diff --git a/aruco_ros/package.xml b/aruco_ros/package.xml index 48dc8b9..0d38d50 100644 --- a/aruco_ros/package.xml +++ b/aruco_ros/package.xml @@ -1,7 +1,7 @@ aruco_ros - 5.0.4 + 5.1.0 The ARUCO Library has been developed by the Ava group of the Univeristy of Cordoba(Spain). It provides real-time marker based 3D pose estimation using AR markers. @@ -16,16 +16,17 @@ ament_cmake + aruco + aruco_msgs cv_bridge geometry_msgs image_transport rclcpp + rclcpp_component + sensor_msgs tf2 - tf2_ros tf2_geometry_msgs - aruco - aruco_msgs - sensor_msgs + tf2_ros visualization_msgs ament_lint_auto diff --git a/aruco_ros/src/marker_publish.cpp b/aruco_ros/src/marker_publish.cpp index 063db95..b9d3979 100644 --- a/aruco_ros/src/marker_publish.cpp +++ b/aruco_ros/src/marker_publish.cpp @@ -34,10 +34,6 @@ */ #include -#include "aruco/aruco.h" -#include "aruco/cvdrawingutils.h" -#include "aruco_ros/aruco_ros_utils.hpp" -#include "aruco_msgs/msg/marker_array.hpp" #include "cv_bridge/cv_bridge.h" #include "image_transport/image_transport.hpp" @@ -46,23 +42,29 @@ #include "rcpputils/asserts.hpp" #include "sensor_msgs/image_encodings.hpp" #include "std_msgs/msg/u_int32_multi_array.hpp" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/buffer.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +#include "aruco/aruco.h" +#include "aruco/cvdrawingutils.h" +#include "aruco_msgs/msg/marker_array.hpp" +#include "aruco_ros/aruco_ros_utils.hpp" using namespace std::chrono_literals; -class ArucoMarkerPublisher : public rclcpp::Node -{ +namespace aruco_ros { +class ArUcoMarkerPublisher { private: - rclcpp::Node::SharedPtr subNode; + rclcpp::Node::SharedPtr node_; + rclcpp::Node::SharedPtr sub_node_; // ArUco stuff - aruco::MarkerDetector mDetector_; - aruco::CameraParameters camParam_; + aruco::MarkerDetector detector_; + aruco::CameraParameters cam_param_; std::vector markers_; // node params - bool useRectifiedImages_; + bool use_rectified_images_; std::string marker_frame_; std::string camera_frame_; std::string reference_frame_; @@ -71,6 +73,7 @@ class ArucoMarkerPublisher : public rclcpp::Node // ROS pub-sub std::unique_ptr it_; image_transport::Subscriber image_sub_; + rclcpp::Subscription::SharedPtr cam_info_sub_; image_transport::Publisher image_pub_; image_transport::Publisher debug_pub_; @@ -80,98 +83,107 @@ class ArucoMarkerPublisher : public rclcpp::Node std::unique_ptr tf_buffer_; std::shared_ptr marker_msg_; - cv::Mat inImage_; - bool useCamInfo_; + cv::Mat in_image_; + bool use_cam_info_; + bool cam_info_received_; std_msgs::msg::UInt32MultiArray marker_list_msg_; public: - ArucoMarkerPublisher() - : Node("marker_publisher"), useCamInfo_(true) - { + ArUcoMarkerPublisher(const rclcpp::NodeOptions &options) + : node_(rclcpp::Node::make_shared("marker_publisher", options)), use_cam_info_(true), + cam_info_received_(false) { + setup(); + } + + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() const { + return node_->get_node_base_interface(); } - bool setup() - { - subNode = this->create_sub_node(this->get_name()); + bool setup() { + RCLCPP_INFO(node_->get_logger(), "*************************"); + RCLCPP_INFO(node_->get_logger(), " ArUcoMarkerPublisher "); + RCLCPP_INFO(node_->get_logger(), "*************************"); + + sub_node_ = node_->create_sub_node(node_->get_name()); // Declare node parameters - this->declare_parameter("marker_size", 0.05); - this->declare_parameter("reference_frame", ""); - this->declare_parameter("camera_frame", ""); - this->declare_parameter("image_is_rectified", true); - this->declare_parameter("use_camera_info", true); + node_->declare_parameter("marker_size", 0.05); + node_->declare_parameter("reference_frame", ""); + node_->declare_parameter("camera_frame", ""); + node_->declare_parameter("image_is_rectified", true); + node_->declare_parameter("use_camera_info", true); - tf_buffer_ = std::make_unique(this->get_clock()); + tf_buffer_ = std::make_unique(node_->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); - it_ = std::make_unique(shared_from_this()); - image_sub_ = it_->subscribe("/image", 1, &ArucoMarkerPublisher::image_callback, this); - - this->get_parameter_or("use_camera_info", useCamInfo_, true); - if (useCamInfo_) { - RCLCPP_INFO(this->get_logger(), "Waiting for the camera info..."); - sensor_msgs::msg::CameraInfo camera_info; - rclcpp::wait_for_message( - camera_info, - shared_from_this(), "/camera_info"); - RCLCPP_INFO(this->get_logger(), "Successfully obtained the camera info!"); - - this->get_parameter_or("marker_size", marker_size_, 0.05); - this->get_parameter_or("image_is_rectified", useRectifiedImages_, true); - this->get_parameter_or("reference_frame", reference_frame_, ""); - this->get_parameter_or("camera_frame", camera_frame_, ""); - camParam_ = aruco_ros::rosCameraInfo2ArucoCamParams(camera_info, useRectifiedImages_); + it_ = std::make_unique(node_); + image_sub_ = it_->subscribe("/image", 1, &ArUcoMarkerPublisher::on_image, this); + + node_->get_parameter_or("use_camera_info", use_cam_info_, true); + node_->get_parameter_or("marker_size", marker_size_, 0.05); + if (use_cam_info_) { + cam_info_sub_ = node_->create_subscription( + "/camera_info", 1, + std::bind(&ArUcoMarkerPublisher::on_cam_info, this, std::placeholders::_1)); + node_->get_parameter_or("image_is_rectified", use_rectified_images_, true); + node_->get_parameter_or("reference_frame", reference_frame_, ""); + node_->get_parameter_or("camera_frame", camera_frame_, ""); rcpputils::assert_true( - !(camera_frame_.empty() && !reference_frame_.empty()), - "Either the camera frame is empty and also reference frame is empty.."); + !(camera_frame_.empty() && !reference_frame_.empty()), + "Either the camera frame is empty and also reference frame is empty.."); if (reference_frame_.empty()) { reference_frame_ = camera_frame_; } } else { - camParam_ = aruco::CameraParameters(); + cam_param_ = aruco::CameraParameters(); } - image_pub_ = it_->advertise(this->get_name() + std::string("/result"), 1); - debug_pub_ = it_->advertise(this->get_name() + std::string("/debug"), 1); - marker_pub_ = subNode->create_publisher("markers", 100); + image_pub_ = it_->advertise(node_->get_name() + std::string("/result"), 1); + debug_pub_ = it_->advertise(node_->get_name() + std::string("/debug"), 1); + marker_pub_ = sub_node_->create_publisher("markers", 100); marker_list_pub_ = - subNode->create_publisher("markers_list", 10); + sub_node_->create_publisher("markers_list", 10); marker_msg_ = aruco_msgs::msg::MarkerArray::Ptr(new aruco_msgs::msg::MarkerArray()); marker_msg_->header.frame_id = reference_frame_; - RCLCPP_INFO(this->get_logger(), "Successfully setup the marker publisher!"); + + log_parameters(); + RCLCPP_INFO(node_->get_logger(), "Successfully setup the marker publisher!"); return true; } - bool getTransform( - const std::string & refFrame, const std::string & childFrame, - geometry_msgs::msg::TransformStamped & transform) - { + void log_parameters() { + RCLCPP_INFO(node_->get_logger(), " * Marker size: %f", marker_size_); + RCLCPP_INFO(node_->get_logger(), " * Reference frame: %s", reference_frame_.c_str()); + RCLCPP_INFO(node_->get_logger(), " * Camera frame: %s", camera_frame_.c_str()); + RCLCPP_INFO(node_->get_logger(), " * Image is rectified: %s", + use_rectified_images_ ? "true" : "false"); + RCLCPP_INFO(node_->get_logger(), " * Use camera info: %s", use_cam_info_ ? "true" : "false"); + } + + bool get_transform(const std::string &refFrame, const std::string &childFrame, + geometry_msgs::msg::TransformStamped &transform) { std::string errMsg; - if (!tf_buffer_->canTransform( - refFrame, childFrame, tf2::TimePointZero, - tf2::durationFromSec(0.5), &errMsg)) - { - RCLCPP_ERROR_STREAM(this->get_logger(), "Unable to get pose from TF : " << errMsg.c_str()); + if (!tf_buffer_->canTransform(refFrame, childFrame, tf2::TimePointZero, + tf2::durationFromSec(0.5), &errMsg)) { + RCLCPP_ERROR_STREAM(node_->get_logger(), "Unable to get pose from TF : " << errMsg.c_str()); return false; } else { try { - transform = tf_buffer_->lookupTransform( - refFrame, childFrame, tf2::TimePointZero, tf2::durationFromSec( - 0.5)); - } catch (const tf2::TransformException & e) { - RCLCPP_ERROR_STREAM( - this->get_logger(), - "Error in lookupTransform of " << childFrame << " in " << refFrame << " : " << e.what()); + transform = tf_buffer_->lookupTransform(refFrame, childFrame, tf2::TimePointZero, + tf2::durationFromSec(0.5)); + } catch (const tf2::TransformException &e) { + RCLCPP_ERROR_STREAM(node_->get_logger(), "Error in lookupTransform of " + << childFrame << " in " << refFrame << " : " + << e.what()); return false; } } return true; } - void image_callback(const sensor_msgs::msg::Image::ConstSharedPtr & msg) - { + void on_image(const sensor_msgs::msg::Image::ConstSharedPtr &msg) { bool publishMarkers = marker_pub_->get_subscription_count() > 0; bool publishMarkersList = marker_list_pub_->get_subscription_count() > 0; bool publishImage = image_pub_.getNumSubscribers() > 0; @@ -185,13 +197,13 @@ class ArucoMarkerPublisher : public rclcpp::Node cv_bridge::CvImagePtr cv_ptr; try { cv_ptr = cv_bridge::toCvCopy(*msg.get(), sensor_msgs::image_encodings::RGB8); - inImage_ = cv_ptr->image; + in_image_ = cv_ptr->image; // clear out previous detection results markers_.clear(); // ok, let's detect - mDetector_.detect(inImage_, markers_, camParam_, marker_size_, false); + detector_.detect(in_image_, markers_, cam_param_, marker_size_, false); // marker array publish if (publishMarkers) { @@ -200,27 +212,27 @@ class ArucoMarkerPublisher : public rclcpp::Node marker_msg_->header.stamp = curr_stamp; for (std::size_t i = 0; i < markers_.size(); ++i) { - aruco_msgs::msg::Marker & marker_i = marker_msg_->markers.at(i); + aruco_msgs::msg::Marker &marker_i = marker_msg_->markers.at(i); marker_i.header.stamp = curr_stamp; marker_i.id = markers_.at(i).id; marker_i.confidence = 1.0; } // if there is camera info let's do 3D stuff - if (useCamInfo_) { + if (use_cam_info_ && cam_info_received_) { // get the current transform from the camera frame to output ref frame tf2::Stamped cameraToReference; cameraToReference.setIdentity(); if (reference_frame_ != camera_frame_) { geometry_msgs::msg::TransformStamped transform; - getTransform(reference_frame_, camera_frame_, transform); + get_transform(reference_frame_, camera_frame_, transform); tf2::fromMsg(transform, cameraToReference); } // now find the transform for each detected marker for (std::size_t i = 0; i < markers_.size(); ++i) { - aruco_msgs::msg::Marker & marker_i = marker_msg_->markers.at(i); + aruco_msgs::msg::Marker &marker_i = marker_msg_->markers.at(i); tf2::Transform transform = aruco_ros::arucoMarker2Tf2(markers_[i]); transform = static_cast(cameraToReference) * transform; tf2::toMsg(transform, marker_i.pose.pose); @@ -245,13 +257,13 @@ class ArucoMarkerPublisher : public rclcpp::Node // draw detected markers on the image for visualization for (std::size_t i = 0; i < markers_.size(); ++i) { - markers_[i].draw(inImage_, cv::Scalar(0, 0, 255), 2); + markers_[i].draw(in_image_, cv::Scalar(0, 0, 255), 2); } // draw a 3D cube in each marker if there is 3D info - if (camParam_.isValid() && marker_size_ > 0) { + if (cam_param_.isValid() && marker_size_ > 0) { for (std::size_t i = 0; i < markers_.size(); ++i) { - aruco::CvDrawingUtils::draw3dAxis(inImage_, markers_[i], camParam_); + aruco::CvDrawingUtils::draw3dAxis(in_image_, markers_[i], cam_param_); } } @@ -261,7 +273,7 @@ class ArucoMarkerPublisher : public rclcpp::Node cv_bridge::CvImage out_msg; out_msg.header.stamp = curr_stamp; out_msg.encoding = sensor_msgs::image_encodings::RGB8; - out_msg.image = inImage_; + out_msg.image = in_image_; image_pub_.publish(out_msg.toImageMsg()); } @@ -271,20 +283,23 @@ class ArucoMarkerPublisher : public rclcpp::Node cv_bridge::CvImage debug_msg; debug_msg.header.stamp = curr_stamp; debug_msg.encoding = sensor_msgs::image_encodings::MONO8; - debug_msg.image = mDetector_.getThresholdedImage(); + debug_msg.image = detector_.getThresholdedImage(); debug_pub_.publish(debug_msg.toImageMsg()); } - } catch (cv_bridge::Exception & e) { - RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + } catch (cv_bridge::Exception &e) { + RCLCPP_ERROR(node_->get_logger(), "cv_bridge exception: %s", e.what()); + } + } + + // wait for one camerainfo, then shut down that subscriber + void on_cam_info(const sensor_msgs::msg::CameraInfo &msg) { + if (!cam_info_received_) { + cam_param_ = aruco_ros::rosCameraInfo2ArucoCamParams(msg, use_rectified_images_); + cam_info_received_ = true; } } }; +} // namespace aruco_ros -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - std::shared_ptr marker_pub = std::make_shared(); - marker_pub->setup(); - rclcpp::spin(marker_pub); - rclcpp::shutdown(); -} +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(aruco_ros::ArUcoMarkerPublisher) diff --git a/aruco_ros/src/simple_single.cpp b/aruco_ros/src/simple_single.cpp index 520f355..8f86263 100644 --- a/aruco_ros/src/simple_single.cpp +++ b/aruco_ros/src/simple_single.cpp @@ -35,10 +35,6 @@ #include -#include "aruco/aruco.h" -#include "aruco/cvdrawingutils.h" -#include "aruco_ros/aruco_ros_utils.hpp" - #include "cv_bridge/cv_bridge.h" #include "geometry_msgs/msg/point_stamped.hpp" #include "geometry_msgs/msg/vector3_stamped.hpp" @@ -46,251 +42,266 @@ #include "rclcpp/rclcpp.hpp" #include "rcpputils/asserts.hpp" #include "sensor_msgs/image_encodings.hpp" -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2_ros/buffer.h" +#include "tf2_ros/transform_broadcaster.h" #include "tf2_ros/transform_listener.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "visualization_msgs/msg/marker.hpp" -class ArucoSimple : public rclcpp::Node -{ +#include "aruco/aruco.h" +#include "aruco/cvdrawingutils.h" +#include "aruco_ros/aruco_ros_utils.hpp" + +namespace aruco_ros { +class ArUcoSimple { private: - rclcpp::Node::SharedPtr subNode; - cv::Mat inImage; - aruco::CameraParameters camParam; - tf2::Stamped rightToLeft; - bool useRectifiedImages; - aruco::MarkerDetector mDetector; - std::vector markers; - rclcpp::Subscription::SharedPtr cam_info_sub; - bool cam_info_received; - image_transport::Publisher image_pub; - image_transport::Publisher debug_pub; - rclcpp::Publisher::SharedPtr pose_pub; - rclcpp::Publisher::SharedPtr transform_pub; - rclcpp::Publisher::SharedPtr position_pub; + rclcpp::Node::SharedPtr node_; + rclcpp::Node::SharedPtr sub_node_; + cv::Mat in_image_; + aruco::CameraParameters cam_param_; + tf2::Stamped right_to_left_; + bool use_rectified_images_; + aruco::MarkerDetector detector_; + std::string detection_mode_; + std::vector markers_; + rclcpp::Subscription::SharedPtr cam_info_sub_; + bool cam_info_received_; + image_transport::Publisher image_pub_; + image_transport::Publisher debug_pub_; + rclcpp::Publisher::SharedPtr pose_pub_; + rclcpp::Publisher::SharedPtr transform_pub_; + rclcpp::Publisher::SharedPtr position_pub_; // rviz visualization marker - rclcpp::Publisher::SharedPtr marker_pub; - rclcpp::Publisher::SharedPtr pixel_pub; - std::string marker_frame; - std::string camera_frame; - std::string reference_frame; + rclcpp::Publisher::SharedPtr marker_pub_; + rclcpp::Publisher::SharedPtr pixel_pub_; + std::string marker_frame_; + std::string camera_frame_; + std::string reference_frame_; - double marker_size; - int marker_id; + double marker_size_; + float min_marker_size_; // percentage of image area + int marker_id_; std::unique_ptr it_; - image_transport::Subscriber image_sub; + image_transport::Subscriber image_sub_; std::shared_ptr tf_listener_; std::unique_ptr tf_buffer_; std::unique_ptr tf_broadcaster_; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_parameter_change_handle_; + public: - ArucoSimple() - : Node("aruco_single"), cam_info_received(false) - { + ArUcoSimple(const rclcpp::NodeOptions &options) + : node_(rclcpp::Node::make_shared("aruco_single", options)), cam_info_received_(false) { + setup(); } - bool setup() - { - tf_buffer_ = std::make_unique(this->get_clock()); + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() const { + return node_->get_node_base_interface(); + } + + bool setup() { + RCLCPP_INFO(node_->get_logger(), "*****************"); + RCLCPP_INFO(node_->get_logger(), " ArUcoSimple "); + RCLCPP_INFO(node_->get_logger(), "*****************"); + + tf_buffer_ = std::make_unique(node_->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); - subNode = this->create_sub_node(this->get_name()); - - it_ = std::make_unique(shared_from_this()); - tf_broadcaster_ = std::make_unique(this); - if (this->has_parameter("corner_refinement")) { - RCLCPP_WARN( - this->get_logger(), - "Corner refinement options have been removed in ArUco 3.0.0, " - "corner_refinement ROS parameter is deprecated"); + sub_node_ = node_->create_sub_node(node_->get_name()); + + it_ = std::make_unique(node_); + tf_broadcaster_ = std::make_unique(node_); + if (node_->has_parameter("corner_refinement")) { + RCLCPP_WARN(node_->get_logger(), + "Corner refinement options have been removed in ArUco 3.0.0, " + "corner_refinement ROS parameter is deprecated"); } - aruco::MarkerDetector::Params params = mDetector.getParameters(); + aruco::MarkerDetector::Params params = detector_.getParameters(); std::string thresh_method; switch (params.thresMethod) { - case aruco::MarkerDetector::ThresMethod::THRES_ADAPTIVE: - thresh_method = "THRESH_ADAPTIVE"; - break; - case aruco::MarkerDetector::ThresMethod::THRES_AUTO_FIXED: - thresh_method = "THRESH_AUTO_FIXED"; - break; - default: - thresh_method = "UNKNOWN"; - break; + case aruco::MarkerDetector::ThresMethod::THRES_ADAPTIVE: + thresh_method = "THRESH_ADAPTIVE"; + break; + case aruco::MarkerDetector::ThresMethod::THRES_AUTO_FIXED: + thresh_method = "THRESH_AUTO_FIXED"; + break; + default: + thresh_method = "UNKNOWN"; + break; } // Print parameters of ArUco marker detector: - RCLCPP_INFO_STREAM(this->get_logger(), "Threshold method: " << thresh_method); - - // Declare node parameters - this->declare_parameter("marker_size", 0.05); - this->declare_parameter("marker_id", 300); - this->declare_parameter("reference_frame", ""); - this->declare_parameter("camera_frame", ""); - this->declare_parameter("marker_frame", ""); - this->declare_parameter("image_is_rectified", true); - this->declare_parameter("min_marker_size", 0.02); - this->declare_parameter("detection_mode", ""); - - float min_marker_size; // percentage of image area - this->get_parameter_or("min_marker_size", min_marker_size, 0.02); - - std::string detection_mode; - this->get_parameter_or("detection_mode", detection_mode, "DM_FAST"); - if (detection_mode == "DM_FAST") { - mDetector.setDetectionMode(aruco::DM_FAST, min_marker_size); - } else if (detection_mode == "DM_VIDEO_FAST") { - mDetector.setDetectionMode(aruco::DM_VIDEO_FAST, min_marker_size); - } else { + RCLCPP_INFO_STREAM(node_->get_logger(), "Threshold method: " << thresh_method); + + // Declare node_ parameters + node_->declare_parameter("marker_size", 0.05); + node_->declare_parameter("marker_id", 300); + node_->declare_parameter("reference_frame", ""); + node_->declare_parameter("camera_frame", ""); + node_->declare_parameter("marker_frame", ""); + node_->declare_parameter("image_is_rectified", true); + node_->declare_parameter("min_marker_size", 0.02); + node_->declare_parameter("detection_mode", "DM_NORMAL"); + + node_->get_parameter_or("min_marker_size", min_marker_size_, 0.02); + + node_->get_parameter_or("detection_mode", detection_mode_, "DM_NORMAL"); + if (detection_mode_ == "DM_FAST") { + detector_.setDetectionMode(aruco::DM_FAST, min_marker_size_); + } else if (detection_mode_ == "DM_VIDEO_FAST") { + detector_.setDetectionMode(aruco::DM_VIDEO_FAST, min_marker_size_); + } else if (detection_mode_ == "DM_NORMAL") { // Aruco version 2 mode - mDetector.setDetectionMode(aruco::DM_NORMAL, min_marker_size); + detector_.setDetectionMode(aruco::DM_NORMAL, min_marker_size_); + } else { + RCLCPP_ERROR(node_->get_logger(), "Unknown detection mode: %s", detection_mode_.c_str()); + return false; } - RCLCPP_INFO_STREAM( - this->get_logger(), "Marker size min: " << min_marker_size << " of image area"); - RCLCPP_INFO_STREAM(this->get_logger(), "Detection mode: " << detection_mode); - - image_sub = it_->subscribe("/image", 1, &ArucoSimple::image_callback, this); - cam_info_sub = this->create_subscription( - "/camera_info", 1, std::bind( - &ArucoSimple::cam_info_callback, this, - std::placeholders::_1)); - - image_pub = it_->advertise(this->get_name() + std::string("/result"), 1); - debug_pub = it_->advertise(this->get_name() + std::string("/debug"), 1); - pose_pub = subNode->create_publisher("pose", 100); - transform_pub = - subNode->create_publisher("transform", 100); - position_pub = subNode->create_publisher("position", 100); - marker_pub = subNode->create_publisher("marker", 10); - pixel_pub = subNode->create_publisher("pixel", 10); - - this->get_parameter_or("marker_size", marker_size, 0.05); - this->get_parameter_or("marker_id", marker_id, 300); - this->get_parameter_or("reference_frame", reference_frame, ""); - this->get_parameter_or("camera_frame", camera_frame, ""); - this->get_parameter_or("marker_frame", marker_frame, ""); - this->get_parameter_or("image_is_rectified", useRectifiedImages, true); + image_sub_ = it_->subscribe("/image", 1, &ArUcoSimple::on_image, this); + cam_info_sub_ = node_->create_subscription( + "/camera_info", 1, std::bind(&ArUcoSimple::on_cam_info, this, std::placeholders::_1)); + + image_pub_ = it_->advertise(node_->get_name() + std::string("/result"), 1); + debug_pub_ = it_->advertise(node_->get_name() + std::string("/debug"), 1); + pose_pub_ = sub_node_->create_publisher("pose", 100); + transform_pub_ = + sub_node_->create_publisher("transform", 100); + position_pub_ = + sub_node_->create_publisher("position", 100); + marker_pub_ = sub_node_->create_publisher("marker", 10); + pixel_pub_ = sub_node_->create_publisher("pixel", 10); + + node_->get_parameter_or("marker_size", marker_size_, 0.05); + node_->get_parameter_or("marker_id", marker_id_, 300); + node_->get_parameter_or("reference_frame", reference_frame_, ""); + node_->get_parameter_or("camera_frame", camera_frame_, ""); + node_->get_parameter_or("marker_frame", marker_frame_, ""); + node_->get_parameter_or("image_is_rectified", use_rectified_images_, true); rcpputils::assert_true( - camera_frame != "" && marker_frame != "", - "Found the camera frame or the marker_frame to be empty!. camera_frame : " + - camera_frame + " and marker_frame : " + marker_frame); + camera_frame_ != "" && marker_frame_ != "", + "Found the camera frame or the marker_frame_ to be empty!. camera_frame_ : " + + camera_frame_ + " and marker_frame_ : " + marker_frame_); - if (reference_frame.empty()) { - reference_frame = camera_frame; + if (reference_frame_.empty()) { + reference_frame_ = camera_frame_; } - RCLCPP_INFO( - this->get_logger(), "ArUco node started with marker size of %f m and marker id to track: %d", - marker_size, marker_id); - RCLCPP_INFO( - this->get_logger(), "ArUco node will publish pose to TF with %s as parent and %s as child.", - reference_frame.c_str(), marker_frame.c_str()); + // Parameter callbacks + // dyn_rec_server.setCallback(boost::bind(&ArUcoSimple::reconf_callback, this, _1, _2)); + on_parameter_change_handle_ = node_->add_on_set_parameters_callback( + std::bind(&ArUcoSimple::on_set_parameter, this, std::placeholders::_1)); - // dyn_rec_server.setCallback(boost::bind(&ArucoSimple::reconf_callback, this, _1, _2)); - RCLCPP_INFO(this->get_logger(), "Setup of aruco_simple node is successful!"); + log_parameters(); + RCLCPP_INFO(node_->get_logger(), "Setup of ArUcoSimple successful!"); return true; } - bool getTransform( - const std::string & refFrame, const std::string & childFrame, - geometry_msgs::msg::TransformStamped & transform) - { + void log_parameters() { + RCLCPP_INFO(node_->get_logger(), " * Marker size: %f m", marker_size_); + RCLCPP_INFO(node_->get_logger(), " * Min marker size: %f %% of image area", min_marker_size_); + RCLCPP_INFO(node_->get_logger(), " * Marker id: %d", marker_id_); + RCLCPP_INFO(node_->get_logger(), " * Reference frame: %s", reference_frame_.c_str()); + RCLCPP_INFO(node_->get_logger(), " * Camera frame: %s", camera_frame_.c_str()); + RCLCPP_INFO(node_->get_logger(), " * Marker frame: %s", marker_frame_.c_str()); + RCLCPP_INFO(node_->get_logger(), " * Image is rectified: %s", + use_rectified_images_ ? "true" : "false"); + RCLCPP_INFO(node_->get_logger(), " * Detection mode: %s", detection_mode_.c_str()); + RCLCPP_INFO(node_->get_logger(), + "ArUco node will publish pose to TF with %s as parent and %s as child.", + reference_frame_.c_str(), marker_frame_.c_str()); + } + + bool get_transform(const std::string &refFrame, const std::string &childFrame, + geometry_msgs::msg::TransformStamped &transform) { std::string errMsg; - if (!tf_buffer_->canTransform( - refFrame, childFrame, tf2::TimePointZero, - tf2::durationFromSec(0.5), &errMsg)) - { - RCLCPP_ERROR_STREAM(this->get_logger(), "Unable to get pose from TF: " << errMsg); + if (!tf_buffer_->canTransform(refFrame, childFrame, tf2::TimePointZero, + tf2::durationFromSec(0.5), &errMsg)) { + RCLCPP_ERROR_STREAM(node_->get_logger(), "Unable to get pose from TF: " << errMsg); return false; } else { try { - transform = tf_buffer_->lookupTransform( - refFrame, childFrame, tf2::TimePointZero, tf2::durationFromSec( - 0.5)); - } catch (const tf2::TransformException & e) { - RCLCPP_ERROR_STREAM( - this->get_logger(), - "Error in lookupTransform of " << childFrame << " in " << refFrame << " : " << e.what()); + transform = tf_buffer_->lookupTransform(refFrame, childFrame, tf2::TimePointZero, + tf2::durationFromSec(0.5)); + } catch (const tf2::TransformException &e) { + RCLCPP_ERROR_STREAM(node_->get_logger(), "Error in lookupTransform of " + << childFrame << " in " << refFrame << " : " + << e.what()); return false; } } return true; } - void image_callback(const sensor_msgs::msg::Image::ConstSharedPtr & msg) - { - if ((image_pub.getNumSubscribers() == 0) && - (debug_pub.getNumSubscribers() == 0) && - (pose_pub->get_subscription_count() == 0) && - (transform_pub->get_subscription_count() == 0) && - (position_pub->get_subscription_count() == 0) && - (marker_pub->get_subscription_count() == 0) && - (pixel_pub->get_subscription_count() == 0)) - { - RCLCPP_DEBUG(this->get_logger(), "No subscribers, not looking for ArUco markers"); + void on_image(const sensor_msgs::msg::Image::ConstSharedPtr &msg) { + if ((image_pub_.getNumSubscribers() == 0) && (debug_pub_.getNumSubscribers() == 0) && + (pose_pub_->get_subscription_count() == 0) && + (transform_pub_->get_subscription_count() == 0) && + (position_pub_->get_subscription_count() == 0) && + (marker_pub_->get_subscription_count() == 0) && + (pixel_pub_->get_subscription_count() == 0)) { + RCLCPP_DEBUG(node_->get_logger(), "No subscribers, not looking for ArUco markers_"); return; } - if (cam_info_received) { + if (cam_info_received_) { builtin_interfaces::msg::Time curr_stamp = msg->header.stamp; cv_bridge::CvImagePtr cv_ptr; try { cv_ptr = cv_bridge::toCvCopy(*msg, sensor_msgs::image_encodings::RGB8); - inImage = cv_ptr->image; + in_image_ = cv_ptr->image; - // detection results will go into "markers" - markers.clear(); + // detection results will go into "markers_" + markers_.clear(); // ok, let's detect - mDetector.detect(inImage, markers, camParam, marker_size, false); + detector_.detect(in_image_, markers_, cam_param_, marker_size_, false); // for each marker, draw info and its boundaries in the image - for (std::size_t i = 0; i < markers.size(); ++i) { + for (std::size_t i = 0; i < markers_.size(); ++i) { // only publishing the selected marker - if (markers[i].id == marker_id) { - tf2::Transform transform = aruco_ros::arucoMarker2Tf2(markers[i]); + if (markers_[i].id == marker_id_) { + tf2::Transform transform = aruco_ros::arucoMarker2Tf2(markers_[i]); tf2::Stamped cameraToReference; cameraToReference.setIdentity(); - if (reference_frame != camera_frame) { + if (reference_frame_ != camera_frame_) { geometry_msgs::msg::TransformStamped transform_stamped; - getTransform(reference_frame, camera_frame, transform_stamped); + get_transform(reference_frame_, camera_frame_, transform_stamped); tf2::fromMsg(transform_stamped, cameraToReference); } transform = static_cast(cameraToReference) * - static_cast(rightToLeft) * - transform; + static_cast(right_to_left_) * transform; geometry_msgs::msg::TransformStamped stampedTransform; - stampedTransform.header.frame_id = reference_frame; + stampedTransform.header.frame_id = reference_frame_; stampedTransform.header.stamp = curr_stamp; - stampedTransform.child_frame_id = marker_frame; + stampedTransform.child_frame_id = marker_frame_; tf2::toMsg(transform, stampedTransform.transform); tf_broadcaster_->sendTransform(stampedTransform); geometry_msgs::msg::PoseStamped poseMsg; poseMsg.header = stampedTransform.header; tf2::toMsg(transform, poseMsg.pose); - poseMsg.header.frame_id = reference_frame; + poseMsg.header.frame_id = reference_frame_; poseMsg.header.stamp = curr_stamp; - pose_pub->publish(poseMsg); + pose_pub_->publish(poseMsg); - transform_pub->publish(stampedTransform); + transform_pub_->publish(stampedTransform); geometry_msgs::msg::Vector3Stamped positionMsg; positionMsg.header = stampedTransform.header; positionMsg.vector = stampedTransform.transform.translation; - position_pub->publish(positionMsg); + position_pub_->publish(positionMsg); geometry_msgs::msg::PointStamped pixelMsg; pixelMsg.header = stampedTransform.header; - pixelMsg.point.x = markers[i].getCenter().x; - pixelMsg.point.y = markers[i].getCenter().y; + pixelMsg.point.x = markers_[i].getCenter().x; + pixelMsg.point.y = markers_[i].getCenter().y; pixelMsg.point.z = 0; - pixel_pub->publish(pixelMsg); + pixel_pub_->publish(pixelMsg); // publish rviz marker representing the ArUco marker patch visualization_msgs::msg::Marker visMarker; @@ -299,8 +310,8 @@ class ArucoSimple : public rclcpp::Node visMarker.type = visualization_msgs::msg::Marker::CUBE; visMarker.action = visualization_msgs::msg::Marker::ADD; visMarker.pose = poseMsg.pose; - visMarker.scale.x = marker_size; - visMarker.scale.y = marker_size; + visMarker.scale.x = marker_size_; + visMarker.scale.y = marker_size_; visMarker.scale.z = 0.001; visMarker.color.r = 1.0; visMarker.color.g = 0; @@ -308,74 +319,82 @@ class ArucoSimple : public rclcpp::Node visMarker.color.a = 1.0; visMarker.lifetime = builtin_interfaces::msg::Duration(); visMarker.lifetime.sec = 3; - marker_pub->publish(visMarker); + marker_pub_->publish(visMarker); } - // but drawing all the detected markers - markers[i].draw(inImage, cv::Scalar(0, 0, 255), 2); + // but drawing all the detected markers_ + markers_[i].draw(in_image_, cv::Scalar(0, 0, 255), 2); } // draw a 3d cube in each marker if there is 3d info - if (camParam.isValid() && marker_size > 0) { - for (std::size_t i = 0; i < markers.size(); ++i) { - aruco::CvDrawingUtils::draw3dAxis(inImage, markers[i], camParam); + if (cam_param_.isValid() && marker_size_ > 0) { + for (std::size_t i = 0; i < markers_.size(); ++i) { + aruco::CvDrawingUtils::draw3dAxis(in_image_, markers_[i], cam_param_); } } - if (image_pub.getNumSubscribers() > 0) { + if (image_pub_.getNumSubscribers() > 0) { // show input with augmented information cv_bridge::CvImage out_msg; out_msg.header.stamp = curr_stamp; out_msg.encoding = sensor_msgs::image_encodings::RGB8; - out_msg.image = inImage; - image_pub.publish(out_msg.toImageMsg()); + out_msg.image = in_image_; + image_pub_.publish(out_msg.toImageMsg()); } - if (debug_pub.getNumSubscribers() > 0) { + if (debug_pub_.getNumSubscribers() > 0) { // show also the internal image resulting from the threshold operation cv_bridge::CvImage debug_msg; debug_msg.header.stamp = curr_stamp; debug_msg.encoding = sensor_msgs::image_encodings::MONO8; - debug_msg.image = mDetector.getThresholdedImage(); - debug_pub.publish(debug_msg.toImageMsg()); + debug_msg.image = detector_.getThresholdedImage(); + debug_pub_.publish(debug_msg.toImageMsg()); } - } catch (cv_bridge::Exception & e) { - RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + } catch (cv_bridge::Exception &e) { + RCLCPP_ERROR(node_->get_logger(), "cv_bridge exception: %s", e.what()); return; } } } // wait for one camerainfo, then shut down that subscriber - void cam_info_callback(const sensor_msgs::msg::CameraInfo & msg) - { - if (!cam_info_received) { - camParam = aruco_ros::rosCameraInfo2ArucoCamParams(msg, useRectifiedImages); + void on_cam_info(const sensor_msgs::msg::CameraInfo &msg) { + if (!cam_info_received_) { + cam_param_ = aruco_ros::rosCameraInfo2ArucoCamParams(msg, use_rectified_images_); // handle cartesian offset between stereo pairs // see the sensor_msgs/CameraInfo documentation for details - rightToLeft.setIdentity(); - rightToLeft.setOrigin(tf2::Vector3(-msg.p[3] / msg.p[0], -msg.p[7] / msg.p[5], 0.0)); + right_to_left_.setIdentity(); + right_to_left_.setOrigin(tf2::Vector3(-msg.p[3] / msg.p[0], -msg.p[7] / msg.p[5], 0.0)); + + cam_info_received_ = true; + } + } - cam_info_received = true; + rcl_interfaces::msg::SetParametersResult + on_set_parameter(const std::vector ¶meters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + for (const auto ¶meter : parameters) { + if (parameter.get_name() == "marker_size") { + marker_size_ = parameter.as_double(); + } else if (parameter.get_name() == "marker_id") { + marker_id_ = parameter.as_int(); + } } + return result; } -// void reconf_callback(aruco_ros::ArucoThresholdConfig & config, uint32_t level) -// { -// mDetector.setDetectionMode( -// aruco::DetectionMode(config.detection_mode), -// config.min_image_size); -// if (config.normalizeImage) { -// RCLCPP_WARN("normalizeImageIllumination is unimplemented!"); -// } -// } + // void reconf_callback(aruco_ros::ArucoThresholdConfig & config, uint32_t level) + // { + // detector_.setDetectionMode( + // aruco::DetectionMode(config.detection_mode), + // config.min_image_size); + // if (config.normalizeImage) { + // RCLCPP_WARN("normalizeImageIllumination is unimplemented!"); + // } + // } }; +} // namespace aruco_ros -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - std::shared_ptr aruco_simple = std::make_shared(); - aruco_simple->setup(); - rclcpp::spin(aruco_simple); - rclcpp::shutdown(); -} +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(aruco_ros::ArUcoSimple)