From dd30e776c1b26b7f13d50cb9bd45a3df7c89e47d Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Fri, 10 Apr 2026 18:57:32 +0200 Subject: [PATCH] Created vision node --- toid_vision/CMakeLists.txt | 60 ++++++ toid_vision/LICENSE | 202 ++++++++++++++++++ .../include/toid_vision/toid_vision.hpp | 67 ++++++ toid_vision/package.xml | 18 ++ toid_vision/src/toid_vision.cpp | 177 +++++++++++++++ toid_vision/src/vision.cpp | 10 + 6 files changed, 534 insertions(+) create mode 100644 toid_vision/CMakeLists.txt create mode 100644 toid_vision/LICENSE create mode 100644 toid_vision/include/toid_vision/toid_vision.hpp create mode 100644 toid_vision/package.xml create mode 100644 toid_vision/src/toid_vision.cpp create mode 100644 toid_vision/src/vision.cpp diff --git a/toid_vision/CMakeLists.txt b/toid_vision/CMakeLists.txt new file mode 100644 index 0000000..c0e59f6 --- /dev/null +++ b/toid_vision/CMakeLists.txt @@ -0,0 +1,60 @@ + +cmake_minimum_required(VERSION 3.8) +project(toid_vision) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) + +set(PACKAGE_DEPS + rclcpp + sensor_msgs + message_filters + cv_bridge + OpenCV + + tf2 + tf2_ros + tf2_geometry_msgs +) + +foreach(PACKAGE ${PACKAGE_DEPS}) + find_package(${PACKAGE} REQUIRED) +endforeach() + +find_package(OpenCV 4 REQUIRED + COMPONENTS + opencv_core + opencv_aruco + opencv_imgproc + opencv_imgcodecs +) + + +set(SOURCES + src/toid_vision.cpp + src/vision.cpp +) + +add_executable(toid_vision ${SOURCES}) + +target_include_directories( + toid_vision + PRIVATE + include +) + +ament_target_dependencies(toid_vision ${PACKAGE_DEPS}) + +install(TARGETS toid_vision DESTINATION lib/${PROJECT_NAME}) + +target_compile_features( + toid_vision PUBLIC + c_std_99 + cxx_std_17 +) + + +ament_package() diff --git a/toid_vision/LICENSE b/toid_vision/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/toid_vision/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/toid_vision/include/toid_vision/toid_vision.hpp b/toid_vision/include/toid_vision/toid_vision.hpp new file mode 100644 index 0000000..85072b4 --- /dev/null +++ b/toid_vision/include/toid_vision/toid_vision.hpp @@ -0,0 +1,67 @@ +#pragma once + +#include +#include + +#include "message_filters/subscriber.hpp" +#include "message_filters/sync_policies/approximate_time.hpp" +#include "message_filters/time_synchronizer.hpp" +#include "opencv2/aruco.hpp" +#include "opencv2/objdetect.hpp" +#include "opencv2/opencv.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/camera_info.hpp" +#include "sensor_msgs/msg/compressed_image.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "std_msgs/msg/string.hpp" +#include "tf2/LinearMath/Quaternion.hpp" +#include "tf2/LinearMath/Transform.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace toid +{ + +class NutDetector : public rclcpp::Node +{ + using SyncPolicy = message_filters::sync_policies::ApproximateTime< + sensor_msgs::msg::CompressedImage, sensor_msgs::msg::CameraInfo>; + +public: + NutDetector(rclcpp::NodeOptions &options); + +public: + void compressed_topic_callback( + const sensor_msgs::msg::CompressedImage::ConstSharedPtr & msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & cam_info); + + void process_image( + cv::Mat & decodedImage, const std_msgs::msg::Header & header, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & cam_info); + + void extract_markers(cv::Mat & image); + + void get_pose(cv::Vec3d & tvec, cv::Vec3d & rvec, geometry_msgs::msg::Pose & out_pose); + + message_filters::Subscriber compressed_img_sub_; + message_filters::Subscriber cam_info_sub_; + std::shared_ptr> sync_; + + + rclcpp::Publisher::SharedPtr pose_pub_; + rclcpp::Publisher::SharedPtr flip_pub_; + + std::vector> markerCorners_; + std::vector markerIds_; + std::vector, int>> markers_; + + std::string camera_frame_id_ = "camera"; + bool is_blue_; + + cv::Ptr detectorParams_; + cv::Ptr dictionary_; + + + message_filters::Subscriber img_sub_; +}; + +} // namespace toid \ No newline at end of file diff --git a/toid_vision/package.xml b/toid_vision/package.xml new file mode 100644 index 0000000..4ef3ce6 --- /dev/null +++ b/toid_vision/package.xml @@ -0,0 +1,18 @@ + + + + toid_vision + 0.0.0 + TODO: Package description + pimpest + Apache-2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/toid_vision/src/toid_vision.cpp b/toid_vision/src/toid_vision.cpp new file mode 100644 index 0000000..398faee --- /dev/null +++ b/toid_vision/src/toid_vision.cpp @@ -0,0 +1,177 @@ +#include "toid_vision/toid_vision.hpp" + +#include +#include + +namespace toid +{ + +NutDetector::NutDetector(rclcpp::NodeOptions & options) +: Node("compressed_image_subscriber", options) +{ + detectorParams_ = cv::aruco::DetectorParameters::create(); + dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_100); + + bool use_compressed = true; + this->declare_parameter("use_compressed", rclcpp::ParameterValue(true)); + this->get_parameter("use_compressed", use_compressed); + + this->declare_parameter("camera_frame_id", rclcpp::ParameterValue("camera")); + this->get_parameter("camera_frame_id", camera_frame_id_); + + this->declare_parameter("is_blue", rclcpp::ParameterValue(true)); + this->get_parameter("is_blue", is_blue_); + + compressed_img_sub_.subscribe(this, "/camera/image_raw/compressed"); + cam_info_sub_.subscribe(this, "/camera/camera_info"); + + sync_ = std::make_shared>( + SyncPolicy(2), compressed_img_sub_, cam_info_sub_); + + using namespace std::placeholders; + sync_->registerCallback(std::bind(&NutDetector::compressed_topic_callback, this, _1, _2)); + RCLCPP_INFO(this->get_logger(), "NutDetector activated... scanning for DEEZ NUTZ"); + + pose_pub_ = + this->create_publisher("/closest_acorn", rclcpp::QoS(1)); +flip_pub_ = + this->create_publisher("/to_flip", rclcpp::QoS(1)); +} + +void NutDetector::compressed_topic_callback( + const sensor_msgs::msg::CompressedImage::ConstSharedPtr & msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & cam_info) +{ + try { + cv::Mat rawData(1, msg->data.size(), CV_8UC1, (void *)msg->data.data()); + cv::Mat decodedImage = cv::imdecode(rawData, cv::IMREAD_COLOR); + + if (decodedImage.empty()) { + RCLCPP_ERROR(this->get_logger(), "Could not decode image!"); + return; + } + + process_image(decodedImage, msg->header, cam_info); + + } catch (const std::exception & e) { + RCLCPP_ERROR(this->get_logger(), "Error processing image: %s", e.what()); + } +} + +void NutDetector::process_image( + cv::Mat & decodedImage, const std_msgs::msg::Header & header, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & cam_info) +{ + markers_.clear(); + + extract_markers(decodedImage); + + if (markers_.empty()) { + return; + } + + std::vector rvecs, tvecs; + + cv::Mat k = cv::Mat(3, 3, CV_64F, (void *)cam_info->k.data()); + cv::aruco::estimatePoseSingleMarkers(markerCorners_, 0.03, k, cam_info->d, rvecs, tvecs); + + size_t prev = 0; + std::string colors = ""; + + for (size_t i = 0; i < markerIds_.size(); i++) { + const int idx = markers_[i].second; + if (markerIds_[idx] != 47 && markerIds_[idx] != 36) { + continue; + } + + RCLCPP_DEBUG( + this->get_logger(), "Marker %ld at %lf %lf", i, markerCorners_[idx][0].x, + markerCorners_[idx][0].y); + + RCLCPP_DEBUG( + this->get_logger(), "World coords: %lf %lf %lf", tvecs[idx][0], tvecs[idx][1], tvecs[idx][2]); + + float dx = tvecs[idx][0] - tvecs[prev][0]; + float dy = tvecs[idx][1] - tvecs[prev][1]; + float dz = tvecs[idx][2] - tvecs[prev][2]; + + float dist = std::sqrt(dx * dx + dy * dy + dz * dz); + + RCLCPP_DEBUG(this->get_logger(), "Dist from prev: %lf", dist); + + if (dist > 0.07) { + continue; + } + + prev = idx; + + colors += (markerIds_[idx] == 36) ? "b" : "y"; + + if (colors.size() > 3) { + break; + } + } + + if (colors.size() == 4) { + RCLCPP_DEBUG(this->get_logger(), "Last seen orientation: %s", colors.c_str()); + for(int i = 0; i<4; i++) { + if(colors[i] == 'b') { + colors[i] = (is_blue_)? '0' : '1'; + } else { + colors[i] = (is_blue_)? '1' : '0'; + } + } + + RCLCPP_DEBUG(this->get_logger(), "Last seen orientation: %s", colors.c_str()); + std_msgs::msg::String m; + m.data = colors; + flip_pub_->publish(m); + } + + geometry_msgs::msg::PoseStamped pose; + get_pose(tvecs[markers_[0].second], rvecs[markers_[0].second], pose.pose); + pose.header.frame_id = camera_frame_id_; + pose.header.stamp = header.stamp; + pose_pub_->publish(pose); +} + +void NutDetector::extract_markers(cv::Mat & image) +{ + cv::aruco::detectMarkers(image, dictionary_, markerCorners_, markerIds_, detectorParams_); + + // cv::aruco::drawDetectedMarkers(image, markerCorners_, markerIds_); + + markers_.reserve(markerIds_.size()); + markers_.resize(markerIds_.size()); + + for (size_t i = 0; i < markerIds_.size(); i++) { + markers_[i].first = markerCorners_[i]; + markers_[i].second = i; + } + + std::sort( + markers_.begin(), markers_.end(), + [](std::pair, int> & a, std::pair, int> & b) { + return a.first[0].y > b.first[0].y; + }); +} + +void NutDetector::get_pose(cv::Vec3d & tvec, cv::Vec3d & rvec, geometry_msgs::msg::Pose & out_pose) +{ + cv::Mat R; + cv::Rodrigues(rvec, R); + tf2::Matrix3x3 tf2_rot{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; + tf2_rot.getRotation(q); + + tf2::convert(q, out_pose.orientation); + + out_pose.position.x = tvec[0]; + out_pose.position.y = tvec[1]; + out_pose.position.z = tvec[2]; +} + +} // namespace toid \ No newline at end of file diff --git a/toid_vision/src/vision.cpp b/toid_vision/src/vision.cpp new file mode 100644 index 0000000..fe41f4f --- /dev/null +++ b/toid_vision/src/vision.cpp @@ -0,0 +1,10 @@ +#include "toid_vision/toid_vision.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