diff --git a/image_proc/CMakeLists.txt b/image_proc/CMakeLists.txt index 64d69538b..789ed6186 100644 --- a/image_proc/CMakeLists.txt +++ b/image_proc/CMakeLists.txt @@ -86,6 +86,18 @@ rclcpp_components_register_node(crop_non_zero EXECUTABLE crop_non_zero_node ) +# track_marker component and node +ament_auto_add_library(track_marker SHARED + src/track_marker.cpp +) +target_compile_definitions(track_marker + PRIVATE "COMPOSITION_BUILDING_DLL" +) +rclcpp_components_register_node(track_marker + PLUGIN "image_proc::TrackMarkerNode" + EXECUTABLE track_marker_node +) + if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") target_link_libraries(image_proc "stdc++fs") endif() diff --git a/image_proc/doc/components.rst b/image_proc/doc/components.rst index 8e8bab9d4..62a715fab 100644 --- a/image_proc/doc/components.rst +++ b/image_proc/doc/components.rst @@ -122,7 +122,7 @@ Parameters image_proc::ResizeNode ---------------------- -Takes image and/or camera info and resize them. Also available as +Takes image and camera info and resize them. Also available as standalone node with the name ``resize_node``. Subscribed Topics @@ -150,3 +150,28 @@ Parameters * **scale_width** (float, default: 1.0): Width scaling of image. * **height** (float): Absolute height of resized image, if ``use_scale`` is false. * **width** (float): Absolute width of resized image, if ``use_scale`` is false. + +image_proc::TrackMarkerNode +--------------------------- +Takes an image, detects an Aruco marker and publishes a ``geometry_msgs/PoseStamped`` +of where the marker is located. Also available as standalone node with the name +``track_marker_node``. + +Subscribed Topics +^^^^^^^^^^^^^^^^^ + * **image** (sensor_msgs/Image): Image topic to process. + * **camera_info** (sensor_msgs/CameraInfo): Camera metadata. + +Published Topics +^^^^^^^^^^^^^^^^ + * **tracked_pose** (geometry_msgs/PoseStamped): Pose of the marker. + +Parameters +^^^^^^^^^^ + * **dictionary** (int, default: 10): Marker dictionary to use. + Values correspond to cv.aruco enum. + The default of 10 corresponds to the DICT_6X6_250 dictionary. + * **image_transport** (string, default: raw): Image transport to use. + * **marker_id** (int, default: 0): The ID of the marker to use. + * **marker_size** (double, default: 0.05): Size of the marker edge, + in meters. diff --git a/image_proc/doc/tutorials.rst b/image_proc/doc/tutorials.rst index b787c6ce9..ddff19c59 100644 --- a/image_proc/doc/tutorials.rst +++ b/image_proc/doc/tutorials.rst @@ -127,3 +127,21 @@ In a separate terminal (on your home machine, if you are running on a robot): $ ros2 run image_view image_view --ros-args -r image:=my_camera/image_rect_color This will display an undistorted color image from ``my_camera``. + +Using the TrackMarkerNode +------------------------- +When generating markers, be sure to pay attention to the selection +of the dictionary. The default dictionary is ``DICT_6X6_250`` which +means you want your marker to be of the 6X6 size, with an ID of 0-249. + +There are two ways to generate markers: + + * The `OpenCV Tutorial `_ + shows programmatic ways to generate markers. + * There are a variety of online Aruco marker generation webpages, + `this one ` is very easy to generate + individual markers. + +Once the marker is printed, be sure to set the ``marker_id`` and +``marker_size`` parameters for the node. It is recommended to measure +the marker size as printing the marker could incur scaling. diff --git a/image_proc/include/image_proc/track_marker.hpp b/image_proc/include/image_proc/track_marker.hpp new file mode 100644 index 000000000..6c212913f --- /dev/null +++ b/image_proc/include/image_proc/track_marker.hpp @@ -0,0 +1,74 @@ +// Copyright 2024 Open Navigation LLC +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef IMAGE_PROC__TRACK_MARKER_HPP_ +#define IMAGE_PROC__TRACK_MARKER_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace image_proc +{ + +class TrackMarkerNode : public rclcpp::Node +{ +public: + explicit TrackMarkerNode(const rclcpp::NodeOptions &); + +private: + image_transport::CameraSubscriber sub_camera_; + + int queue_size_; + int marker_id_; + double marker_size_; + std::string image_topic_; + rclcpp::Publisher::SharedPtr pub_; + + cv::Ptr detector_params_; + cv::Ptr dictionary_; + + void imageCb( + const sensor_msgs::msg::Image::ConstSharedPtr & image_msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info_msg); +}; + +} // namespace image_proc + +#endif // IMAGE_PROC__TRACK_MARKER_HPP_ diff --git a/image_proc/package.xml b/image_proc/package.xml index e8924ff6a..0c06ea17c 100644 --- a/image_proc/package.xml +++ b/image_proc/package.xml @@ -24,6 +24,7 @@ cv_bridge image_geometry image_transport + geometry_msgs libopencv-dev rclcpp rclcpp_components diff --git a/image_proc/src/track_marker.cpp b/image_proc/src/track_marker.cpp new file mode 100644 index 000000000..c0586e475 --- /dev/null +++ b/image_proc/src/track_marker.cpp @@ -0,0 +1,157 @@ +// Copyright 2024 Open Navigation LLC +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace image_proc +{ + +TrackMarkerNode::TrackMarkerNode(const rclcpp::NodeOptions & options) +: rclcpp::Node("TrackMarkerNode", options) +{ + // TransportHints does not actually declare the parameter + this->declare_parameter("image_transport", "raw"); + + // For compressed topics to remap appropriately, we need to pass a + // fully expanded and remapped topic name to image_transport + auto node_base = this->get_node_base_interface(); + image_topic_ = node_base->resolve_topic_or_service_name("image", false); + + // Declare parameters before we setup any publishers or subscribers + marker_id_ = this->declare_parameter("marker_id", 0); + marker_size_ = this->declare_parameter("marker_size", 0.05); + // Default dictionary is cv::aruco::DICT_6X6_250 + int dict_id = this->declare_parameter("dictionary", 10); + + detector_params_ = cv::aruco::DetectorParameters::create(); + dictionary_ = cv::aruco::getPredefinedDictionary(dict_id); + + // Setup lazy subscriber using publisher connection callback + rclcpp::PublisherOptions pub_options; + pub_options.event_callbacks.matched_callback = + [this](rclcpp::MatchedInfo &) + { + if (pub_->get_subscription_count() == 0) { + sub_camera_.shutdown(); + } else if (!sub_camera_) { + // Create subscriber with QoS matched to subscribed topic publisher + auto qos_profile = getTopicQosProfile(this, image_topic_); + image_transport::TransportHints hints(this); + sub_camera_ = image_transport::create_camera_subscription( + this, image_topic_, std::bind( + &TrackMarkerNode::imageCb, + this, std::placeholders::_1, std::placeholders::_2), + hints.getTransport(), qos_profile); + } + }; + + // Allow overriding QoS settings (history, depth, reliability) + pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); + + // Create publisher + pub_ = this->create_publisher( + "tracked_pose", 10, pub_options); +} + +void TrackMarkerNode::imageCb( + const sensor_msgs::msg::Image::ConstSharedPtr & image_msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info_msg) +{ + cv_bridge::CvImageConstPtr cv_ptr; + try { + cv_ptr = cv_bridge::toCvShare(image_msg); + } catch (cv_bridge::Exception & e) { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + return; + } + + std::vector marker_ids; + std::vector> marker_corners; + cv::aruco::detectMarkers(cv_ptr->image, dictionary_, marker_corners, marker_ids); + + for (size_t i = 0; i < marker_ids.size(); ++i) { + if (marker_ids[i] == marker_id_) { + // This is our desired marker + std::vector> corners; + corners.push_back(marker_corners[i]); + + // Copy the matrices since they are const and OpenCV functions are not + auto k = info_msg->k; + auto d = info_msg->d; + + // Get the camera info + cv::Mat intrinsics(3, 3, CV_64FC1, reinterpret_cast(k.data())); + cv::Mat dist_coeffs(info_msg->d.size(), 1, CV_64FC1, reinterpret_cast(d.data())); + + // Estimate pose + std::vector rvecs, tvecs; + cv::aruco::estimatePoseSingleMarkers( + corners, marker_size_, intrinsics, dist_coeffs, rvecs, tvecs); + + // Publish pose of marker + geometry_msgs::msg::PoseStamped pose; + pose.header = image_msg->header; + // Fill in pose + pose.pose.position.x = tvecs[0][0]; + pose.pose.position.y = tvecs[0][1]; + pose.pose.position.z = tvecs[0][2]; + // Convert angle-axis to quaternion + cv::Quatd q = cv::Quatd::createFromRvec(rvecs[0]); + pose.pose.orientation.x = q.x; + pose.pose.orientation.y = q.y; + pose.pose.orientation.z = q.z; + pose.pose.orientation.w = q.w; + pub_->publish(pose); + } + } +} + +} // namespace image_proc + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(image_proc::TrackMarkerNode)